MsgConversion/laser: check if intensity field has the supported datatype before converting. PointCloud2Depth: added {cloud}_transformed output topic (with same stamp and frame_id than the depth image).

src/MsgConversion.cpp

@@ -1788,10 +1788,21 @@ bool convertScanMsg(
 	bool containIntensity = false;
 	for(unsigned int i=0; i<scanOut.fields.size(); ++i)
 	{
-		if(scanOut.fields[i].name.compare("intensity") == 0)
+		if(scanOut.fields[i].datatype == sensor_msgs::PointField::FLOAT32)
 		{
 			containIntensity = true;
 		}
+		else
+		{
+			static bool warningShown = false;
+			if(!warningShown)
+			{
+				ROS_WARN("The input scan cloud has an \"intensity\" field "
+						"but the datatype (%d) is not supported. Intensity will be ignored. "
+						"This message is only shown once.", scanOut.fields[i].datatype);
+				warningShown = true;
+			}
+		}
 	}
 
 	rtabmap::LaserScan::Format format;
@@ -1860,7 +1871,21 @@ bool convertScan3dMsg(
 		}
 		if(scan3dMsg->fields[i].name.compare("intensity") == 0)
 		{
-			containIntensity = true;
+			if(scan3dMsg->fields[i].datatype == sensor_msgs::PointField::FLOAT32)
+			{
+				containIntensity = true;
+			}
+			else
+			{
+				static bool warningShown = false;
+				if(!warningShown)
+				{
+					ROS_WARN("The input scan cloud has an \"intensity\" field "
+							"but the datatype (%d) is not supported. Intensity will be ignored. "
+							"This message is only shown once.", scan3dMsg->fields[i].datatype);
+					warningShown = true;
+				}
+			}
 		}
 	}
 

src/nodelets/pointcloud_to_depthimage.cpp

@@ -46,6 +46,7 @@ SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 #include <pcl/point_cloud.h>
 #include <pcl/point_types.h>
 #include <pcl_conversions/pcl_conversions.h>
+#include <pcl_ros/transforms.h>
 
 #include <message_filters/sync_policies/approximate_time.h>
 #include <message_filters/sync_policies/exact_time.h>
@@ -121,6 +122,7 @@ private:
 		image_transport::ImageTransport it(nh);
 		depthImage16Pub_ = it.advertise("image_raw", 1); // 16 bits unsigned in mm
 		depthImage32Pub_ = it.advertise("image", 1);     // 32 bits float in meters
+		pointCloudTransformedPub_ = nh.advertise<sensor_msgs::PointCloud2>(nh.resolveName("cloud")+"_transformed", 1);
 
 		if(approx)
 		{
@@ -154,8 +156,8 @@ private:
 				cloudDisplacement = rtabmap_ros::getTransform(
 						pointCloud2Msg->header.frame_id,
 						fixedFrameId_,
-						pointCloud2Msg->header.stamp,
 						cameraInfoMsg->header.stamp,
+						pointCloud2Msg->header.stamp,
 						*listener_,
 						waitForTransform_);
 			}
@@ -177,7 +179,7 @@ private:
 				return;
 			}
 
-			rtabmap::Transform localTransform = cloudDisplacement.inverse()*cloudToCamera;
+			rtabmap::Transform localTransform = cloudDisplacement*cloudToCamera;
 
 			rtabmap::CameraModel model = rtabmap_ros::cameraModelFromROS(*cameraInfoMsg, localTransform);
 
@@ -217,6 +219,14 @@ private:
 						depthImage.image = rtabmap::util2d::fillDepthHoles(depthImage.image, fillHolesSize_, fillHolesError_);
 					}
 				}
+
+				if(pointCloudTransformedPub_.getNumSubscribers()>0)
+				{
+					sensor_msgs::PointCloud2 pointCloud2Out;
+					pcl_ros::transformPointCloud(model.localTransform().inverse().toEigen4f(), *pointCloud2Msg, pointCloud2Out);
+					pointCloud2Out.header = cameraInfoMsg->header;
+					pointCloudTransformedPub_.publish(pointCloud2Out);
+				}
 			}
 
 			depthImage.header = cameraInfoMsg->header;
@@ -250,6 +260,7 @@ private:
 private:
 	image_transport::Publisher depthImage16Pub_;
 	image_transport::Publisher depthImage32Pub_;
+	ros::Publisher pointCloudTransformedPub_;
 	message_filters::Subscriber<sensor_msgs::PointCloud2> pointCloudSub_;
 	message_filters::Subscriber<sensor_msgs::CameraInfo> cameraInfoSub_;
 	std::string fixedFrameId_;