IcpOdometry: keeping intensity when converting clouds. PointCloudAssembler: option to synchronize with odom_info to assemble only keyframes

src/nodelets/icp_odometry.cpp

@@ -273,7 +273,7 @@ private:
 		sensor_msgs::PointCloud2 scanOut;
 		laser_geometry::LaserProjection projection;
 		projection.transformLaserScanToPointCloud(scanMsg->header.frame_id, *scanMsg, scanOut, this->tfListener());
-		pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+		pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
 		pcl::fromROSMsg(scanOut, *pclScan);
 		pclScan->is_dense = true;
 
@@ -305,7 +305,7 @@ private:
 				{
 					normals = util3d::computeFastOrganizedNormals2D(pclScan, scanNormalK_, scanNormalRadius_);
 				}
-				pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+				pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
 				pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
 				scan = util3d::laserScan2dFromPointCloud(*pclScanNormal);
 
@@ -405,7 +405,7 @@ private:
 		int maxLaserScans = scanCloudMaxPoints_;
 		if(containNormals)
 		{
-			pcl::PointCloud<pcl::PointNormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointNormal>);
+			pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZINormal>);
 			pcl::fromROSMsg(cloudMsg, *pclScan);
 			if(pclScan->size() && scanDownsamplingStep_ > 1)
 			{
@@ -423,7 +423,7 @@ private:
 		}
 		else
 		{
-			pcl::PointCloud<pcl::PointXYZ>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZ>);
+			pcl::PointCloud<pcl::PointXYZI>::Ptr pclScan(new pcl::PointCloud<pcl::PointXYZI>);
 			pcl::fromROSMsg(cloudMsg, *pclScan);
 			if(pclScan->size() && scanDownsamplingStep_ > 1)
 			{
@@ -448,7 +448,7 @@ private:
 				{
 					//compute normals
 					pcl::PointCloud<pcl::Normal>::Ptr normals = util3d::computeNormals(pclScan, scanNormalK_, scanNormalRadius_);
-					pcl::PointCloud<pcl::PointNormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointNormal>);
+					pcl::PointCloud<pcl::PointXYZINormal>::Ptr pclScanNormal(new pcl::PointCloud<pcl::PointXYZINormal>);
 					pcl::concatenateFields(*pclScan, *normals, *pclScanNormal);
 					scan = util3d::laserScanFromPointCloud(*pclScanNormal);
 

src/nodelets/point_cloud_assembler.cpp

@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <message_filters/sync_policies/exact_time.h>
 
 #include <rtabmap_ros/MsgConversion.h>
+#include <rtabmap_ros/OdomInfo.h>
 #include <rtabmap/core/util3d.h>
 #include <rtabmap/core/util3d_filtering.h>
 
@@ -67,6 +68,7 @@ public:
 		warningThread_(0),
 		callbackCalled_(false),
 		exactSync_(0),
+		exactInfoSync_(0),
 		maxClouds_(0),
 		assemblingTime_(0),
 		skipClouds_(0),
@@ -81,6 +83,7 @@ public:
 	virtual ~PointCloudAssembler()
 	{
 		delete exactSync_;
+		delete exactInfoSync_;
 
 		if(warningThread_)
 		{
@@ -97,6 +100,7 @@ private:
 		ros::NodeHandle & pnh = getPrivateNodeHandle();
 
 		int queueSize = 5;
+		bool subscribeOdomInfo = false;
 
 		pnh.param("queue_size", queueSize, queueSize);
 		pnh.param("fixed_frame_id", fixedFrameId_, fixedFrameId_);
@@ -107,6 +111,7 @@ private:
 		pnh.param("range_min", rangeMin_, rangeMin_);
 		pnh.param("range_max", rangeMax_, rangeMax_);
 		pnh.param("voxel_size", voxelSize_, voxelSize_);
+		pnh.param("subscribe_odom_info", subscribeOdomInfo, subscribeOdomInfo);
 		ROS_ASSERT(maxClouds_>0 || assemblingTime_ >0.0);
 
 		cloudsSkipped_ = skipClouds_;
@@ -119,6 +124,21 @@ private:
 								getName().c_str(),
 								cloudSub_.getTopic().c_str());
 		}
+		else if(subscribeOdomInfo)
+		{
+			syncCloudSub_.subscribe(nh, "cloud", 1);
+			syncOdomSub_.subscribe(nh, "odom", 1);
+			syncOdomInfoSub_.subscribe(nh, "odom_info", 1);
+			exactInfoSync_ = new message_filters::Synchronizer<syncInfoPolicy>(syncInfoPolicy(queueSize), syncCloudSub_, syncOdomSub_, syncOdomInfoSub_);
+			exactInfoSync_->registerCallback(boost::bind(&rtabmap_ros::PointCloudAssembler::callbackCloudOdomInfo, this, _1, _2, _3));
+			subscribedTopicsMsg = uFormat("\n%s subscribed to (exact sync):\n   %s,\n   %s",
+								getName().c_str(),
+								syncCloudSub_.getTopic().c_str(),
+								syncOdomSub_.getTopic().c_str(),
+								syncOdomInfoSub_.getTopic().c_str());
+
+			warningThread_ = new boost::thread(boost::bind(&PointCloudAssembler::warningLoop, this, subscribedTopicsMsg));
+		}
 		else
 		{
 			syncCloudSub_.subscribe(nh, "cloud", 1);
@@ -156,6 +176,32 @@ private:
 		}
 	}
 
+	void callbackCloudOdomInfo(
+			const sensor_msgs::PointCloud2ConstPtr & cloudMsg,
+			const nav_msgs::OdometryConstPtr & odomMsg,
+			const rtabmap_ros::OdomInfoConstPtr & odomInfoMsg)
+	{
+		callbackCalled_ = true;
+		rtabmap::Transform odom = rtabmap_ros::transformFromPoseMsg(odomMsg->pose.pose);
+		if(!odom.isNull())
+		{
+			if(odomInfoMsg->keyFrameAdded)
+			{
+				fixedFrameId_ = odomMsg->header.frame_id;
+				callbackCloud(cloudMsg);
+			}
+			else
+			{
+				NODELET_INFO("Skipping non keyframe...");
+			}
+		}
+		else
+		{
+			NODELET_WARN("Reseting point cloud assembler as null odometry has been received.");
+			clouds_.clear();
+		}
+	}
+
 	void callbackCloud(const sensor_msgs::PointCloud2ConstPtr & cloudMsg)
 	{
 		if(cloudPub_.getNumSubscribers())
@@ -267,9 +313,12 @@ private:
 	ros::Publisher cloudPub_;
 
 	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry> syncPolicy;
+	typedef message_filters::sync_policies::ExactTime<sensor_msgs::PointCloud2, nav_msgs::Odometry, rtabmap_ros::OdomInfo> syncInfoPolicy;
 	message_filters::Synchronizer<syncPolicy>* exactSync_;
+	message_filters::Synchronizer<syncInfoPolicy>* exactInfoSync_;
 	message_filters::Subscriber<sensor_msgs::PointCloud2> syncCloudSub_;
 	message_filters::Subscriber<nav_msgs::Odometry> syncOdomSub_;
+	message_filters::Subscriber<rtabmap_ros::OdomInfo> syncOdomInfoSub_;
 
 	int maxClouds_;
 	int skipClouds_;