Added localization_pose output topic (geometry_msgs/PoseWithCovarianceStamped) when a localization occurs

CMakeLists.txt

@@ -19,7 +19,7 @@ find_package(find_object_2d)
 
 ## System dependencies are found with CMake's conventions
 # find_package(Boost REQUIRED COMPONENTS system)
-find_package(RTABMap 0.17.0 REQUIRED)
+find_package(RTABMap 0.17.2 REQUIRED)
 
 find_package(OpenCV REQUIRED)
 

include/rtabmap_ros/CoreWrapper.h

@@ -216,6 +216,7 @@ private:
 	ros::Publisher mapGraphPub_;
 	ros::Publisher labelsPub_;
 	ros::Publisher mapPathPub_;
+	ros::Publisher localizationPosePub_;
 	ros::Subscriber initialPoseSub_;
 
 	//Planning stuff

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>rtabmap_ros</name>
-  <version>0.17.0</version>
+  <version>0.17.2</version>
   <description>RTAB-Map's ros-pkg. RTAB-Map is a RGB-D SLAM approach with real-time constraints.</description>
   <maintainer email="matlabbe@gmail.com">Mathieu Labbe</maintainer>
   <author>Mathieu Labbe</author>

src/CoreWrapper.cpp

@@ -210,6 +210,7 @@ void CoreWrapper::onInit()
 	mapGraphPub_ = nh.advertise<rtabmap_ros::MapGraph>("mapGraph", 1);
 	labelsPub_ = nh.advertise<visualization_msgs::MarkerArray>("labels", 1);
 	mapPathPub_ = nh.advertise<nav_msgs::Path>("mapPath", 1);
+	localizationPosePub_ = nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("localization_pose", 1);
 	initialPoseSub_ = nh.subscribe("initialpose", 1, &CoreWrapper::initialPoseCallback, this);
 
 	// planning topics
@@ -1497,6 +1498,18 @@ void CoreWrapper::process(
 			{
 				// Publish local graph, info
 				this->publishStats(stamp);
+				if(localizationPosePub_.getNumSubscribers() &&
+					!rtabmap_.getStatistics().localizationCovariance().empty())
+				{
+					geometry_msgs::PoseWithCovarianceStamped poseMsg;
+					poseMsg.header.frame_id = mapFrameId_;
+					poseMsg.header.stamp = stamp;
+					rtabmap_ros::transformToPoseMsg(mapToOdom_*odom, poseMsg.pose.pose);
+					poseMsg.pose.covariance;
+					const cv::Mat & cov = rtabmap_.getStatistics().localizationCovariance();
+					memcpy(poseMsg.pose.covariance.data(), cov.data, cov.total()*sizeof(double));
+					localizationPosePub_.publish(poseMsg);
+				}
 				std::map<int, rtabmap::Transform> filteredPoses = rtabmap_.getLocalOptimizedPoses();
 
 				// create a tmp signature with latest sensory data if latest signature was ignored