Use timing channel from PointCloud2, if available. (#896)

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -32,6 +32,9 @@
 #include "geometry_msgs/Vector3.h"
 #include "glog/logging.h"
 #include "nav_msgs/OccupancyGrid.h"
+#include "pcl/point_cloud.h"
+#include "pcl/point_types.h"
+#include "pcl_conversions/pcl_conversions.h"
 #include "ros/ros.h"
 #include "ros/serialization.h"
 #include "sensor_msgs/Imu.h"
@@ -39,6 +42,34 @@
 #include "sensor_msgs/MultiEchoLaserScan.h"
 #include "sensor_msgs/PointCloud2.h"
 
+namespace {
+
+// Sizes of PCL point types have to be 4n floats for alignment, as described in
+// http://pointclouds.org/documentation/tutorials/adding_custom_ptype.php
+struct PointXYZT {
+  float x;
+  float y;
+  float z;
+  float time;
+};
+
+struct PointXYZIT {
+  PCL_ADD_POINT4D;
+  float intensity;
+  float time;
+  float unused_padding[2];
+};
+
+}  // namespace
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    PointXYZT, (float, x, x)(float, y, y)(float, z, z)(float, time, time))
+
+POINT_CLOUD_REGISTER_POINT_STRUCT(
+    PointXYZIT,
+    (float, x, x)(float, y, y)(float, z, z)(float, intensity,
+                                            intensity)(float, time, time))
+
 namespace cartographer_ros {
 namespace {
 
@@ -183,29 +214,63 @@ ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
 
 std::tuple<::cartographer::sensor::PointCloudWithIntensities,
            ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) {
+ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg) {
   PointCloudWithIntensities point_cloud;
   // We check for intensity field here to avoid run-time warnings if we pass in
   // 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, 0.f);
-      point_cloud.intensities.push_back(point.intensity);
+  if (PointCloud2HasField(msg, "intensity")) {
+    if (PointCloud2HasField(msg, "time")) {
+      pcl::PointCloud<PointXYZIT> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.emplace_back(point.x, point.y, point.z, point.time);
+        point_cloud.intensities.push_back(point.intensity);
+      }
+    } else {
+      pcl::PointCloud<pcl::PointXYZI> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
+        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, 0.f);
-      point_cloud.intensities.push_back(1.0);
+    // If we don't have an intensity field, just copy XYZ and fill in 1.0f.
+    if (PointCloud2HasField(msg, "time")) {
+      pcl::PointCloud<PointXYZT> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.emplace_back(point.x, point.y, point.z, point.time);
+        point_cloud.intensities.push_back(1.0f);
+      }
+    } else {
+      pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
+      pcl::fromROSMsg(msg, pcl_point_cloud);
+      point_cloud.points.reserve(pcl_point_cloud.size());
+      point_cloud.intensities.reserve(pcl_point_cloud.size());
+      for (const auto& point : pcl_point_cloud) {
+        point_cloud.points.emplace_back(point.x, point.y, point.z, 0.f);
+        point_cloud.intensities.push_back(1.0f);
+      }
     }
   }
-  return std::make_tuple(point_cloud, FromRos(message.header.stamp));
+  ::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
+  if (!point_cloud.points.empty()) {
+    const double duration = point_cloud.points.back()[3];
+    timestamp += cartographer::common::FromSeconds(duration);
+    for (Eigen::Vector4f& point : point_cloud.points) {
+      point[3] -= duration;
+      CHECK_LE(point[3], 0) << "Encountered a point with a larger stamp than "
+                               "the last point in the cloud.";
+    }
+  }
+  return std::make_tuple(point_cloud, timestamp);
 }
 
 LandmarkData ToLandmarkData(const LandmarkList& landmark_list) {

cartographer_ros/cartographer_ros/msg_conversion.h

@@ -17,7 +17,6 @@
 #ifndef CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
 #define CARTOGRAPHER_ROS_CARTOGRAPHER_ROS_MSG_CONVERSION_H
 
-#include "cartographer/common/port.h"
 #include "cartographer/common/time.h"
 #include "cartographer/io/submap_painter.h"
 #include "cartographer/sensor/landmark_data.h"
@@ -28,9 +27,6 @@
 #include "geometry_msgs/Transform.h"
 #include "geometry_msgs/TransformStamped.h"
 #include "nav_msgs/OccupancyGrid.h"
-#include "pcl/point_cloud.h"
-#include "pcl/point_types.h"
-#include "pcl_conversions/pcl_conversions.h"
 #include "sensor_msgs/Imu.h"
 #include "sensor_msgs/LaserScan.h"
 #include "sensor_msgs/MultiEchoLaserScan.h"
@@ -63,7 +59,7 @@ ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg);
 
 std::tuple<::cartographer::sensor::PointCloudWithIntensities,
            ::cartographer::common::Time>
-ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message);
+ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& msg);
 
 ::cartographer::sensor::LandmarkData ToLandmarkData(
     const cartographer_ros_msgs::LandmarkList& landmark_list);

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -165,14 +165,10 @@ void SensorBridge::HandleMultiEchoLaserScanMessage(
 void SensorBridge::HandlePointCloud2Message(
     const std::string& sensor_id,
     const sensor_msgs::PointCloud2::ConstPtr& msg) {
-  pcl::PointCloud<pcl::PointXYZ> pcl_point_cloud;
-  pcl::fromROSMsg(*msg, pcl_point_cloud);
-  carto::sensor::TimedPointCloud point_cloud;
-  for (const auto& point : pcl_point_cloud) {
-    point_cloud.emplace_back(point.x, point.y, point.z, 0.f);
-  }
-  HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
-                    point_cloud);
+  carto::sensor::PointCloudWithIntensities point_cloud;
+  carto::common::Time time;
+  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
+  HandleRangefinder(sensor_id, time, msg->header.frame_id, point_cloud.points);
 }
 
 const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
@@ -222,6 +218,9 @@ void SensorBridge::HandleLaserScan(
 void SensorBridge::HandleRangefinder(
     const std::string& sensor_id, const carto::common::Time time,
     const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
+  if (!ranges.empty()) {
+    CHECK_LE(ranges.back()[3], 0);
+  }
   const auto sensor_to_tracking =
       tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
   if (sensor_to_tracking != nullptr) {