Add support for subdividing laser scan messages. (#428)

This adds an option to subdivide sensor_msgs/LaserScan and
sensor_msgs/MultiEchoLaserScan messages into multiple point
clouds. For slow spinning laser scanners this allows unwarping
using googlecartographer/cartographer#408.

cartographer_ros/cartographer_ros/configuration_files_test.cc

@@ -20,6 +20,7 @@
 #include "cartographer/common/configuration_file_resolver.h"
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer_ros/node_options.h"
+#include "cartographer_ros/trajectory_options.h"
 #include "gtest/gtest.h"
 #include "ros/package.h"
 

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -50,6 +50,7 @@ int MapBuilderBridge::AddTrajectory(
   CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
   sensor_bridges_[trajectory_id] =
       cartographer::common::make_unique<SensorBridge>(
+          trajectory_options.num_subdivisions_per_laser_scan,
           trajectory_options.tracking_frame,
           node_options_.lookup_transform_timeout_sec, tf_buffer_,
           map_builder_.GetTrajectoryBuilder(trajectory_id));

cartographer_ros/cartographer_ros/node_options.h

@@ -21,7 +21,6 @@
 
 #include "cartographer/common/lua_parameter_dictionary.h"
 #include "cartographer/mapping/map_builder.h"
-#include "cartographer_ros/trajectory_options.h"
 
 namespace cartographer_ros {
 

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -39,10 +39,11 @@ const string& CheckNoLeadingSlash(const string& frame_id) {
 }  // namespace
 
 SensorBridge::SensorBridge(
-    const string& tracking_frame, const double lookup_transform_timeout_sec,
+    const int num_subdivisions_per_laser_scan, const string& tracking_frame,
-    tf2_ros::Buffer* const tf_buffer,
+    const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
     carto::mapping::TrajectoryBuilder* const trajectory_builder)
-    : tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
+    : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
+      tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
       trajectory_builder_(trajectory_builder) {}
 
 void SensorBridge::HandleOdometryMessage(
@@ -87,15 +88,17 @@ void SensorBridge::HandleImuMessage(const string& sensor_id,
 
 void SensorBridge::HandleLaserScanMessage(
     const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
-  HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
+  HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
-                    ToPointCloudWithIntensities(*msg).points);
+                  ToPointCloudWithIntensities(*msg).points,
+                  msg->time_increment);
 }
 
 void SensorBridge::HandleMultiEchoLaserScanMessage(
     const string& sensor_id,
     const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
-  HandleRangefinder(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
+  HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
-                    ToPointCloudWithIntensities(*msg).points);
+                  ToPointCloudWithIntensities(*msg).points,
+                  msg->time_increment);
 }
 
 void SensorBridge::HandlePointCloud2Message(
@@ -112,6 +115,25 @@ void SensorBridge::HandlePointCloud2Message(
 
 const TfBridge& SensorBridge::tf_bridge() const { return tf_bridge_; }
 
+void SensorBridge::HandleLaserScan(const string& sensor_id,
+                                   const carto::common::Time start_time,
+                                   const string& frame_id,
+                                   const carto::sensor::PointCloud& points,
+                                   const double seconds_between_points) {
+  for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
+    const size_t start_index =
+        points.size() * i / num_subdivisions_per_laser_scan_;
+    const size_t end_index =
+        points.size() * (i + 1) / num_subdivisions_per_laser_scan_;
+    const carto::sensor::PointCloud subdivision(points.begin() + start_index,
+                                                points.begin() + end_index);
+    const carto::common::Time subdivision_time =
+        start_time + carto::common::FromSeconds((start_index + end_index) / 2. *
+                                                seconds_between_points);
+    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
+  }
+}
+
 void SensorBridge::HandleRangefinder(const string& sensor_id,
                                      const carto::common::Time time,
                                      const string& frame_id,

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -36,8 +36,8 @@ namespace cartographer_ros {
 class SensorBridge {
  public:
   explicit SensorBridge(
-      const string& tracking_frame, double lookup_transform_timeout_sec,
+      int num_subdivisions_per_laser_scan, const string& tracking_frame,
-      tf2_ros::Buffer* tf_buffer,
+      double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
       ::cartographer::mapping::TrajectoryBuilder* trajectory_builder);
 
   SensorBridge(const SensorBridge&) = delete;
@@ -58,11 +58,17 @@ class SensorBridge {
   const TfBridge& tf_bridge() const;
 
  private:
+  void HandleLaserScan(const string& sensor_id,
+                       ::cartographer::common::Time start_time,
+                       const string& frame_id,
+                       const ::cartographer::sensor::PointCloud& points,
+                       double seconds_between_points);
   void HandleRangefinder(const string& sensor_id,
-                         const ::cartographer::common::Time time,
+                         ::cartographer::common::Time time,
                          const string& frame_id,
                          const ::cartographer::sensor::PointCloud& ranges);
 
+  const int num_subdivisions_per_laser_scan_;
   const TfBridge tf_bridge_;
   ::cartographer::mapping::TrajectoryBuilder* const trajectory_builder_;
 };

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -14,12 +14,26 @@
  * limitations under the License.
  */
 
-#include "cartographer_ros/node_options.h"
+#include "cartographer_ros/trajectory_options.h"
 
 #include "glog/logging.h"
 
 namespace cartographer_ros {
 
+namespace {
+
+void CheckTrajectoryOptions(const TrajectoryOptions& options) {
+  CHECK_GE(options.num_subdivisions_per_laser_scan, 1);
+  CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
+               (options.num_point_clouds > 0),
+           1)
+      << "Configuration error: 'use_laser_scan', "
+         "'use_multi_echo_laser_scan' and 'num_point_clouds' are "
+         "mutually exclusive, but one is required.";
+}
+
+}  // namespace
+
 TrajectoryOptions CreateTrajectoryOptions(
     ::cartographer::common::LuaParameterDictionary* const
         lua_parameter_dictionary) {
@@ -38,16 +52,12 @@ TrajectoryOptions CreateTrajectoryOptions(
   options.use_laser_scan = lua_parameter_dictionary->GetBool("use_laser_scan");
   options.use_multi_echo_laser_scan =
       lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan");
+  options.num_subdivisions_per_laser_scan =
+      lua_parameter_dictionary->GetNonNegativeInt(
+          "num_subdivisions_per_laser_scan");
   options.num_point_clouds =
       lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds");
-
+  CheckTrajectoryOptions(options);
-  CHECK_EQ(options.use_laser_scan + options.use_multi_echo_laser_scan +
-               (options.num_point_clouds > 0),
-           1)
-      << "Configuration error: 'use_laser_scan', "
-         "'use_multi_echo_laser_scan' and 'num_point_clouds' are "
-         "mutually exclusive, but one is required.";
-
   return options;
 }
 
@@ -60,12 +70,15 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
   options->use_odometry = msg.use_odometry;
   options->use_laser_scan = msg.use_laser_scan;
   options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan;
+  options->num_subdivisions_per_laser_scan =
+      msg.num_subdivisions_per_laser_scan;
   options->num_point_clouds = msg.num_point_clouds;
   if (!options->trajectory_builder_options.ParseFromString(
           msg.trajectory_builder_options_proto)) {
     LOG(ERROR) << "Failed to parse protobuf";
     return false;
   }
+  CheckTrajectoryOptions(*options);
   return true;
 }
 
@@ -79,6 +92,7 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
   msg.use_odometry = options.use_odometry;
   msg.use_laser_scan = options.use_laser_scan;
   msg.use_multi_echo_laser_scan = options.use_multi_echo_laser_scan;
+  msg.num_subdivisions_per_laser_scan = options.num_subdivisions_per_laser_scan;
   msg.num_point_clouds = options.num_point_clouds;
   options.trajectory_builder_options.SerializeToString(
       &msg.trajectory_builder_options_proto);

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -36,6 +36,7 @@ struct TrajectoryOptions {
   bool use_odometry;
   bool use_laser_scan;
   bool use_multi_echo_laser_scan;
+  int num_subdivisions_per_laser_scan;
   int num_point_clouds;
 };
 

cartographer_ros/configuration_files/backpack_2d.lua

@@ -26,6 +26,7 @@ options = {
   use_odometry = false,
   use_laser_scan = false,
   use_multi_echo_laser_scan = true,
+  num_subdivisions_per_laser_scan = 10,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,
@@ -34,5 +35,6 @@ options = {
 }
 
 MAP_BUILDER.use_trajectory_builder_2d = true
+TRAJECTORY_BUILDER_2D.scans_per_accumulation = 10
 
 return options

cartographer_ros/configuration_files/backpack_3d.lua

@@ -26,6 +26,7 @@ options = {
   use_odometry = false,
   use_laser_scan = false,
   use_multi_echo_laser_scan = false,
+  num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 2,
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,

cartographer_ros/configuration_files/pr2.lua

@@ -26,6 +26,7 @@ options = {
   use_odometry = false,
   use_laser_scan = true,
   use_multi_echo_laser_scan = false,
+  num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,

cartographer_ros/configuration_files/revo_lds.lua

@@ -26,6 +26,7 @@ options = {
   use_odometry = false,
   use_laser_scan = true,
   use_multi_echo_laser_scan = false,
+  num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -26,6 +26,7 @@ options = {
   use_odometry = true,
   use_laser_scan = true,
   use_multi_echo_laser_scan = false,
+  num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,
   submap_publish_period_sec = 0.3,

cartographer_ros_msgs/msg/TrajectoryOptions.msg

@@ -19,6 +19,7 @@ bool provide_odom_frame
 bool use_odometry
 bool use_laser_scan
 bool use_multi_echo_laser_scan
+int32 num_subdivisions_per_laser_scan
 int32 num_point_clouds
 
 # This is a binary-encoded