Merge branch 'devel' of https://github.com/introlab/rtabmap_ros

include/rtabmap_ros/CoreWrapper.h

@@ -40,6 +40,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <std_msgs/Empty.h>
 #include <std_msgs/Int32.h>
 #include <nav_msgs/GetMap.h>
+#include <geometry_msgs/PoseWithCovarianceStamped.h>
 
 #include <rtabmap/core/Parameters.h>
 #include <rtabmap/core/Rtabmap.h>
@@ -116,6 +117,7 @@ private:
 	void defaultCallback(const sensor_msgs::ImageConstPtr & imageMsg); // no odom
 
 	void userDataAsyncCallback(const rtabmap_ros::UserDataConstPtr & dataMsg);
+	void globalPoseAsyncCallback(const geometry_msgs::PoseWithCovarianceStampedConstPtr & globalPoseMsg);
 
 	void goalCommonCallback(int id, const std::string & label, const rtabmap::Transform & pose, const ros::Time & stamp, double * planningTime = 0);
 	void goalCallback(const geometry_msgs::PoseStampedConstPtr & msg);
@@ -127,8 +129,7 @@ private:
 			const rtabmap::SensorData & data,
 			const rtabmap::Transform & odom = rtabmap::Transform(),
 			const std::string & odomFrameId = "",
-			float odomRotationalVariance = 1.0,
-			float odomTransitionalVariance = 1.0);
+			const cv::Mat & odomCovariance = cv::Mat::eye(6,6,CV_64FC1));
 
 	bool updateRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
 	bool resetRtabmapCallback(std_srvs::Empty::Request&, std_srvs::Empty::Response&);
@@ -175,8 +176,7 @@ private:
 	rtabmap::Transform lastPose_;
 	ros::Time lastPoseStamp_;
 	bool lastPoseIntermediate_;
-	float rotVariance_;
-	float transVariance_;
+	cv::Mat covariance_;
 	rtabmap::Transform currentMetricGoal_;
 	rtabmap::Transform lastPublishedMetricGoal_;
 	bool latestNodeWasReached_;
@@ -258,6 +258,9 @@ private:
 	ros::Subscriber userDataAsyncSub_;
 	cv::Mat userData_;
 
+	ros::Subscriber globalPoseAsyncSub_;
+	geometry_msgs::PoseWithCovarianceStamped globalPose_;
+
 	bool stereoToDepth_;
 	bool odomSensorSync_;
 	float rate_;