Added reading intensity values from point clouds (#321)

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -126,6 +126,16 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities(
   return point_cloud;
 }
 
+bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
+                         const std::string& field_name) {
+  for (const auto& field : pc2.fields) {
+    if (field.name == field_name) {
+      return true;
+    }
+  }
+  return false;
+}
+
 }  // namespace
 
 sensor_msgs::PointCloud2 ToPointCloud2Message(
@@ -154,14 +164,26 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
 
 PointCloudWithIntensities ToPointCloudWithIntensities(
     const sensor_msgs::PointCloud2& message) {
-  pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
-  pcl::fromROSMsg(message, pcl_point_cloud);
   PointCloudWithIntensities point_cloud;
-
+  // We check for intensity field here to avoid run-time warnings if we pass in
-  // TODO(hrapp): How to get reflectivities from PCL?
+  // a PointCloud2 without intensity.
+  if (PointCloud2HasField(message, "intensity")) {
+    pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
+    pcl::fromROSMsg(message, pcl_point_cloud);
     for (const auto& point : pcl_point_cloud) {
       point_cloud.points.emplace_back(point.x, point.y, point.z);
-    point_cloud.intensities.push_back(1.);
+      point_cloud.intensities.push_back(point.intensity);
+    }
+  } else {
+    pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
+    pcl::fromROSMsg(message, pcl_point_cloud);
+
+    // If we don't have an intensity field, just copy XYZ and fill in
+    // 1.0.
+    for (const auto& point : pcl_point_cloud) {
+      point_cloud.points.emplace_back(point.x, point.y, point.z);
+      point_cloud.intensities.push_back(1.0);
+    }
   }
   return point_cloud;
 }