HandleRangefinder time refers to newest point. (#612)

This is necessary so that sensor::Collator queues range data
after previous odometry and IMU data, and LocalTrajectoryBuilder
will be able to unwarp each point.

cartographer_ros/cartographer_ros/assets_writer_main.cc

@@ -87,13 +87,15 @@ std::unique_ptr<carto::io::PointsBatch> HandleMessage(
   points_batch->start_time = start_time;
   points_batch->frame_id = message.header.frame_id;
 
-  carto::sensor::PointCloudWithIntensities point_cloud =
+  ::cartographer::sensor::PointCloudWithIntensities point_cloud;
+  ::cartographer::common::Time point_cloud_time;
+  std::tie(point_cloud, point_cloud_time) =
       ToPointCloudWithIntensities(message);
   CHECK_EQ(point_cloud.intensities.size(), point_cloud.points.size());
 
   for (size_t i = 0; i < point_cloud.points.size(); ++i) {
     const carto::common::Time time =
-        start_time + carto::common::FromSeconds(point_cloud.points[i][3]);
+        point_cloud_time + carto::common::FromSeconds(point_cloud.points[i][3]);
     if (!transform_interpolation_buffer.Has(time)) {
       continue;
     }

cartographer_ros/cartographer_ros/msg_conversion.cc

@@ -91,8 +91,8 @@ float GetFirstEcho(const sensor_msgs::LaserEcho& echo) {
 
 // For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
 template <typename LaserMessageType>
-PointCloudWithIntensities LaserScanToPointCloudWithIntensities(
+std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
-    const LaserMessageType& msg) {
+LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
   CHECK_GE(msg.range_min, 0.f);
   CHECK_GE(msg.range_max, msg.range_min);
   if (msg.angle_increment > 0.f) {
@@ -124,7 +124,15 @@ PointCloudWithIntensities LaserScanToPointCloudWithIntensities(
     }
     angle += msg.angle_increment;
   }
-  return point_cloud;
+  ::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 (auto& point : point_cloud.points) {
+      point[3] -= duration;
+    }
+  }
+  return std::make_tuple(point_cloud, timestamp);
 }
 
 bool PointCloud2HasField(const sensor_msgs::PointCloud2& pc2,
@@ -153,18 +161,21 @@ sensor_msgs::PointCloud2 ToPointCloud2Message(
   return msg;
 }
 
-PointCloudWithIntensities ToPointCloudWithIntensities(
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-    const sensor_msgs::LaserScan& msg) {
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
   return LaserScanToPointCloudWithIntensities(msg);
 }
 
-PointCloudWithIntensities ToPointCloudWithIntensities(
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-    const sensor_msgs::MultiEchoLaserScan& msg) {
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg) {
   return LaserScanToPointCloudWithIntensities(msg);
 }
 
-PointCloudWithIntensities ToPointCloudWithIntensities(
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-    const sensor_msgs::PointCloud2& message) {
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message) {
   PointCloudWithIntensities point_cloud;
   // We check for intensity field here to avoid run-time warnings if we pass in
   // a PointCloud2 without intensity.
@@ -186,7 +197,7 @@ PointCloudWithIntensities ToPointCloudWithIntensities(
       point_cloud.intensities.push_back(1.0);
     }
   }
-  return point_cloud;
+  return std::make_tuple(point_cloud, FromRos(message.header.stamp));
 }
 
 Rigid3d ToRigid3d(const geometry_msgs::TransformStamped& transform) {

cartographer_ros/cartographer_ros/msg_conversion.h

@@ -18,6 +18,7 @@
 #define CARTOGRAPHER_ROS_MSG_CONVERSION_H_
 
 #include "cartographer/common/port.h"
+#include "cartographer/common/time.h"
 #include "cartographer/sensor/point_cloud.h"
 #include "cartographer/transform/rigid_transform.h"
 #include "geometry_msgs/Pose.h"
@@ -45,14 +46,20 @@ geometry_msgs::Pose ToGeometryMsgPose(
 
 geometry_msgs::Point ToGeometryMsgPoint(const Eigen::Vector3d& vector3d);
 
-::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
+// Converts ROS message to point cloud. Returns the time when the last point
-    const sensor_msgs::LaserScan& msg);
+// was acquired (different from the ROS timestamp). Timing of points is given in
+// the fourth component of each point relative to `Time`.
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg);
 
-::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-    const sensor_msgs::MultiEchoLaserScan& msg);
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::MultiEchoLaserScan& msg);
 
-::cartographer::sensor::PointCloudWithIntensities ToPointCloudWithIntensities(
+std::tuple<::cartographer::sensor::PointCloudWithIntensities,
-    const sensor_msgs::PointCloud2& message);
+           ::cartographer::common::Time>
+ToPointCloudWithIntensities(const sensor_msgs::PointCloud2& message);
 
 ::cartographer::transform::Rigid3d ToRigid3d(
     const geometry_msgs::TransformStamped& transform);

cartographer_ros/cartographer_ros/msg_conversion_test.cc

@@ -35,7 +35,8 @@ TEST(MsgConversion, LaserScanToPointCloud) {
   laser_scan.range_min = 0.f;
   laser_scan.range_max = 10.f;
 
-  const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points;
+  const auto point_cloud =
+      std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
   EXPECT_TRUE(
       point_cloud[0].isApprox(Eigen::Vector4f(1.f, 0.f, 0.f, 0.f), 1e-6));
   EXPECT_TRUE(point_cloud[1].isApprox(
@@ -71,7 +72,8 @@ TEST(MsgConversion, LaserScanToPointCloudWithInfinityAndNaN) {
   laser_scan.range_min = 2.f;
   laser_scan.range_max = 10.f;
 
-  const auto point_cloud = ToPointCloudWithIntensities(laser_scan).points;
+  const auto point_cloud =
+      std::get<0>(ToPointCloudWithIntensities(laser_scan)).points;
   ASSERT_EQ(2, point_cloud.size());
   EXPECT_TRUE(
       point_cloud[0].isApprox(Eigen::Vector4f(0.f, 2.f, 0.f, 0.f), 1e-6));

cartographer_ros/cartographer_ros/rosbag_validate_main.cc

@@ -174,7 +174,7 @@ class RangeDataChecker {
   template <typename MessageType>
   RangeChecksum ComputeRangeChecksum(const MessageType& message) {
     const cartographer::sensor::TimedPointCloud& point_cloud =
-        ToPointCloudWithIntensities(message).points;
+        std::get<0>(ToPointCloudWithIntensities(message)).points;
     Eigen::Vector4f points_sum = Eigen::Vector4f::Zero();
     for (const auto& point : point_cloud) {
       points_sum += point;

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -116,15 +116,19 @@ void SensorBridge::HandleImuMessage(const std::string& sensor_id,
 
 void SensorBridge::HandleLaserScanMessage(
     const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
-  HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
+  ::cartographer::sensor::PointCloudWithIntensities point_cloud;
-                  ToPointCloudWithIntensities(*msg));
+  ::cartographer::common::Time time;
+  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
+  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
 }
 
 void SensorBridge::HandleMultiEchoLaserScanMessage(
     const std::string& sensor_id,
     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-  HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
+  ::cartographer::sensor::PointCloudWithIntensities point_cloud;
-                  ToPointCloudWithIntensities(*msg));
+  ::cartographer::common::Time time;
+  std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
+  HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
 }
 
 void SensorBridge::HandlePointCloud2Message(
@@ -143,24 +147,30 @@ void SensorBridge::HandlePointCloud2Message(
 const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
 
 void SensorBridge::HandleLaserScan(
-    const std::string& sensor_id, const carto::common::Time start_time,
+    const std::string& sensor_id, const carto::common::Time time,
     const std::string& frame_id,
     const carto::sensor::PointCloudWithIntensities& points) {
+  CHECK_LE(points.points.back()[3], 0);
   // TODO(gaschler): Use per-point time instead of subdivisions.
   for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
     const size_t start_index =
         points.points.size() * i / num_subdivisions_per_laser_scan_;
     const size_t end_index =
         points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
-    const carto::sensor::TimedPointCloud subdivision(
+    carto::sensor::TimedPointCloud subdivision(
         points.points.begin() + start_index, points.points.begin() + end_index);
     if (start_index == end_index) {
       continue;
     }
-    const size_t middle_index = (start_index + end_index) / 2;
+    const double time_to_subdivision_end = subdivision.back()[3];
+    // `subdivision_time` is the end of the measurement so sensor::Collator will
+    // send all other sensor data first.
     const carto::common::Time subdivision_time =
-        start_time +
+        time + carto::common::FromSeconds(time_to_subdivision_end);
-        carto::common::FromSeconds(points.points.at(middle_index)[3]);
+    for (auto& point : subdivision) {
+      point[3] -= time_to_subdivision_end;
+    }
+    CHECK_EQ(subdivision.back()[3], 0);
     HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
   }
 }