OdomInfo msg: added local scan map field. OdometryROS: publishing odom_local_scan_map

include/rtabmap_ros/OdometryROS.h

@@ -98,6 +98,7 @@ private:
 	ros::Publisher odomPub_;
 	ros::Publisher odomInfoPub_;
 	ros::Publisher odomLocalMap_;
+	ros::Publisher odomLocalScanMap_;
 	ros::Publisher odomLastFrame_;
 	ros::ServiceServer resetSrv_;
 	ros::ServiceServer resetToPoseSrv_;

msg/OdomInfo.msg

@@ -30,6 +30,7 @@ int32 inliers
 float32 variance
 int32 features
 int32 localMapSize
+int32 localScanMapSize
 float32 timeEstimation
 float32 timeParticleFiltering
 float32 stamp
@@ -52,3 +53,7 @@ int32[] cornerInliers
 geometry_msgs/Transform transform
 geometry_msgs/Transform transformFiltered
 
+# compressed local scan map data
+# use rtabmap::util3d::uncompressData() from "rtabmap/core/util3d.h"
+uint8[] localScanMap
+

src/MsgConversion.cpp

@@ -31,6 +31,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <zlib.h>
 #include <ros/ros.h>
 #include <rtabmap/core/util3d.h>
+#include <rtabmap/core/Compression.h>
 #include <rtabmap/utilite/UStl.h>
 #include <rtabmap/utilite/ULogger.h>
 #include <pcl_conversions/pcl_conversions.h>
@@ -711,6 +712,7 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 	info.features = msg.features;
 	info.inliers = msg.inliers;
 	info.localMapSize = msg.localMapSize;
+	info.localScanMapSize = msg.localScanMapSize;
 	info.timeEstimation = msg.timeEstimation;
 	info.variance = msg.variance;
 	info.timeParticleFiltering =  msg.timeParticleFiltering;
@@ -742,6 +744,8 @@ rtabmap::OdometryInfo odomInfoFromROS(const rtabmap_ros::OdomInfo & msg)
 		info.localMap.insert(std::make_pair(msg.localMapKeys[i], point3fFromROS(msg.localMapValues[i])));
 	}
 
+	info.localScanMap = rtabmap::uncompressData(msg.localScanMap);
+
 	return info;
 }
 
@@ -752,6 +756,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.features = info.features;
 	msg.inliers = info.inliers;
 	msg.localMapSize = info.localMapSize;
+	msg.localScanMapSize = info.localScanMapSize;
 	msg.timeEstimation = info.timeEstimation;
 	msg.variance = info.variance;
 	msg.timeParticleFiltering =  info.timeParticleFiltering;
@@ -777,6 +782,7 @@ void odomInfoToROS(const rtabmap::OdometryInfo & info, rtabmap_ros::OdomInfo & m
 	msg.localMapKeys = uKeys(info.localMap);
 	points3fToROS(uValues(info.localMap), msg.localMapValues);
 
+	msg.localScanMap = rtabmap::compressData(info.localScanMap);
 }
 
 }

src/OdometryROS.cpp

@@ -39,6 +39,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <rtabmap/core/Rtabmap.h>
 #include <rtabmap/core/OdometryF2M.h>
 #include <rtabmap/core/OdometryF2F.h>
+#include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_transforms.h>
 #include <rtabmap/core/Memory.h>
 #include <rtabmap/core/Signature.h>
@@ -98,6 +99,7 @@ void OdometryROS::onInit()
 	odomPub_ = nh.advertise<nav_msgs::Odometry>("odom", 1);
 	odomInfoPub_ = nh.advertise<rtabmap_ros::OdomInfo>("odom_info", 1);
 	odomLocalMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_map", 1);
+	odomLocalScanMap_ = nh.advertise<sensor_msgs::PointCloud2>("odom_local_scan_map", 1);
 	odomLastFrame_ = nh.advertise<sensor_msgs::PointCloud2>("odom_last_frame", 1);
 
 	Transform initialPose = Transform::getIdentity();
@@ -502,6 +504,25 @@ void OdometryROS::processData(const SensorData & data, const ros::Time & stamp)
 				NODELET_ERROR("ERROR, Wrong Type of Odometry, Shouldn't happen");
 			}
 		}
+
+		if(odomLocalScanMap_.getNumSubscribers() && !info.localScanMap.empty())
+		{
+			sensor_msgs::PointCloud2 cloudMsg;
+			if(info.localScanMap.channels() == 6)
+			{
+				pcl::PointCloud<pcl::PointNormal>::Ptr cloud = util3d::laserScanToPointCloudNormal(info.localScanMap);
+				pcl::toROSMsg(*cloud, cloudMsg);
+			}
+			else
+			{
+				pcl::PointCloud<pcl::PointXYZ>::Ptr cloud = util3d::laserScanToPointCloud(info.localScanMap);
+				pcl::toROSMsg(*cloud, cloudMsg);
+			}
+
+			cloudMsg.header.stamp = stamp; // use corresponding time stamp to image
+			cloudMsg.header.frame_id = odomFrameId_;
+			odomLocalScanMap_.publish(cloudMsg);
+		}
 	}
 	else if(publishNullWhenLost_)
 	{