Support multiple LaserScan and MultiEchoLaserScan topics. (#435)

This fixes #212.

cartographer_ros/cartographer_ros/node.cc

@@ -225,11 +225,14 @@ std::unordered_set<string> Node::ComputeExpectedTopics(
     const cartographer_ros_msgs::SensorTopics& topics) {
   std::unordered_set<string> expected_topics;
 
-  if (options.use_laser_scan) {
+  for (const string& topic : ComputeRepeatedTopicNames(
-    expected_topics.insert(topics.laser_scan_topic);
+           topics.laser_scan_topic, options.num_laser_scans)) {
+    expected_topics.insert(topic);
   }
-  if (options.use_multi_echo_laser_scan) {
+  for (const string& topic :
-    expected_topics.insert(topics.multi_echo_laser_scan_topic);
+       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
+                                 options.num_multi_echo_laser_scans)) {
+    expected_topics.insert(topic);
   }
   for (const string& topic : ComputeRepeatedTopicNames(
            topics.point_cloud2_topic, options.num_point_clouds)) {
@@ -264,8 +267,8 @@ int Node::AddTrajectory(const TrajectoryOptions& options,
 void Node::LaunchSubscribers(const TrajectoryOptions& options,
                              const cartographer_ros_msgs::SensorTopics& topics,
                              const int trajectory_id) {
-  if (options.use_laser_scan) {
+  for (const string& topic : ComputeRepeatedTopicNames(
-    const string topic = topics.laser_scan_topic;
+           topics.laser_scan_topic, options.num_laser_scans)) {
     subscribers_[trajectory_id].push_back(
         node_handle_.subscribe<sensor_msgs::LaserScan>(
             topic, kInfiniteSubscriberQueueSize,
@@ -276,9 +279,9 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                       ->HandleLaserScanMessage(topic, msg);
                 })));
   }
-
+  for (const string& topic :
-  if (options.use_multi_echo_laser_scan) {
+       ComputeRepeatedTopicNames(topics.multi_echo_laser_scan_topic,
-    const string topic = topics.multi_echo_laser_scan_topic;
+                                 options.num_multi_echo_laser_scans)) {
     subscribers_[trajectory_id].push_back(node_handle_.subscribe<
                                           sensor_msgs::MultiEchoLaserScan>(
         topic, kInfiniteSubscriberQueueSize,
@@ -289,7 +292,6 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options,
                   ->HandleMultiEchoLaserScanMessage(topic, msg);
             })));
   }
-
   for (const string& topic : ComputeRepeatedTopicNames(
            topics.point_cloud2_topic, options.num_point_clouds)) {
     subscribers_[trajectory_id].push_back(node_handle_.subscribe(

cartographer_ros/cartographer_ros/offline_node_main.cc

@@ -115,15 +115,16 @@ void Run(const std::vector<string>& bag_filenames) {
               .second);
   };
 
-  // For 2D SLAM, subscribe to exactly one horizontal laser.
+  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.
-  if (trajectory_options.use_laser_scan) {
+  for (const string& topic : ComputeRepeatedTopicNames(
-    check_insert(kLaserScanTopic);
+           kLaserScanTopic, trajectory_options.num_laser_scans)) {
+    check_insert(topic);
   }
-  if (trajectory_options.use_multi_echo_laser_scan) {
+  for (const string& topic : ComputeRepeatedTopicNames(
-    check_insert(kMultiEchoLaserScanTopic);
+           kMultiEchoLaserScanTopic,
+           trajectory_options.num_multi_echo_laser_scans)) {
+    check_insert(topic);
   }
-
-  // Subscribe to all point cloud topics.
   for (const string& topic : ComputeRepeatedTopicNames(
            kPointCloud2Topic, trajectory_options.num_point_clouds)) {
     check_insert(topic);
@@ -139,7 +140,7 @@ void Run(const std::vector<string>& bag_filenames) {
     check_insert(kImuTopic);
   }
 
-  // For both 2D and 3D SLAM, odometry is optional.
+  // Odometry is optional.
   if (trajectory_options.use_odometry) {
     check_insert(kOdometryTopic);
   }

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -24,12 +24,12 @@ 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 +
+  CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans +
-               (options.num_point_clouds > 0),
+               options.num_point_clouds,
            1)
-      << "Configuration error: 'use_laser_scan', "
+      << "Configuration error: 'num_laser_scans', "
-         "'use_multi_echo_laser_scan' and 'num_point_clouds' are "
+         "'num_multi_echo_laser_scans' and 'num_point_clouds' are "
-         "mutually exclusive, but one is required.";
+         "all zero, but at least one is required.";
 }
 
 }  // namespace
@@ -49,9 +49,10 @@ TrajectoryOptions CreateTrajectoryOptions(
   options.provide_odom_frame =
       lua_parameter_dictionary->GetBool("provide_odom_frame");
   options.use_odometry = lua_parameter_dictionary->GetBool("use_odometry");
-  options.use_laser_scan = lua_parameter_dictionary->GetBool("use_laser_scan");
+  options.num_laser_scans =
-  options.use_multi_echo_laser_scan =
+      lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans");
-      lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan");
+  options.num_multi_echo_laser_scans =
+      lua_parameter_dictionary->GetNonNegativeInt("num_multi_echo_laser_scans");
   options.num_subdivisions_per_laser_scan =
       lua_parameter_dictionary->GetNonNegativeInt(
           "num_subdivisions_per_laser_scan");
@@ -68,8 +69,8 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg,
   options->odom_frame = msg.odom_frame;
   options->provide_odom_frame = msg.provide_odom_frame;
   options->use_odometry = msg.use_odometry;
-  options->use_laser_scan = msg.use_laser_scan;
+  options->num_laser_scans = msg.num_laser_scans;
-  options->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan;
+  options->num_multi_echo_laser_scans = msg.num_multi_echo_laser_scans;
   options->num_subdivisions_per_laser_scan =
       msg.num_subdivisions_per_laser_scan;
   options->num_point_clouds = msg.num_point_clouds;
@@ -90,8 +91,8 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage(
   msg.odom_frame = options.odom_frame;
   msg.provide_odom_frame = options.provide_odom_frame;
   msg.use_odometry = options.use_odometry;
-  msg.use_laser_scan = options.use_laser_scan;
+  msg.num_laser_scans = options.num_laser_scans;
-  msg.use_multi_echo_laser_scan = options.use_multi_echo_laser_scan;
+  msg.num_multi_echo_laser_scans = options.num_multi_echo_laser_scans;
   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(

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -34,8 +34,8 @@ struct TrajectoryOptions {
   string odom_frame;
   bool provide_odom_frame;
   bool use_odometry;
-  bool use_laser_scan;
+  int num_laser_scans;
-  bool use_multi_echo_laser_scan;
+  int num_multi_echo_laser_scans;
   int num_subdivisions_per_laser_scan;
   int num_point_clouds;
 };

cartographer_ros/configuration_files/backpack_2d.lua

@@ -24,8 +24,8 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
-  use_laser_scan = false,
+  num_laser_scans = 0,
-  use_multi_echo_laser_scan = true,
+  num_multi_echo_laser_scans = 1,
   num_subdivisions_per_laser_scan = 10,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,

cartographer_ros/configuration_files/backpack_3d.lua

@@ -24,8 +24,8 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
-  use_laser_scan = false,
+  num_laser_scans = 0,
-  use_multi_echo_laser_scan = false,
+  num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 2,
   lookup_transform_timeout_sec = 0.2,

cartographer_ros/configuration_files/pr2.lua

@@ -24,8 +24,8 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
-  use_laser_scan = true,
+  num_laser_scans = 1,
-  use_multi_echo_laser_scan = false,
+  num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,

cartographer_ros/configuration_files/revo_lds.lua

@@ -24,8 +24,8 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = true,
   use_odometry = false,
-  use_laser_scan = true,
+  num_laser_scans = 1,
-  use_multi_echo_laser_scan = false,
+  num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,

cartographer_ros/configuration_files/taurob_tracker.lua

@@ -24,8 +24,8 @@ options = {
   odom_frame = "odom",
   provide_odom_frame = false,
   use_odometry = true,
-  use_laser_scan = true,
+  num_laser_scans = 1,
-  use_multi_echo_laser_scan = false,
+  num_multi_echo_laser_scans = 0,
   num_subdivisions_per_laser_scan = 1,
   num_point_clouds = 0,
   lookup_transform_timeout_sec = 0.2,

cartographer_ros_msgs/msg/TrajectoryOptions.msg

@@ -17,8 +17,8 @@ string published_frame
 string odom_frame
 bool provide_odom_frame
 bool use_odometry
-bool use_laser_scan
+int32 num_laser_scans
-bool use_multi_echo_laser_scan
+int32 num_multi_echo_laser_scans
 int32 num_subdivisions_per_laser_scan
 int32 num_point_clouds
 

cartographer_rviz/cartographer_rviz/drawable_submap.cc

@@ -64,11 +64,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) {
 
 }  // namespace
 
-DrawableSubmap::DrawableSubmap(
+DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id,
-    const ::cartographer::mapping::SubmapId& id,
                                ::rviz::DisplayContext* const display_context,
-    ::rviz::Property* const submap_category, const bool visible,
+                               ::rviz::Property* const submap_category,
-    const float pose_axes_length, const float pose_axes_radius)
+                               const bool visible, const float pose_axes_length,
+                               const float pose_axes_radius)
     : id_(id),
       display_context_(display_context),
       scene_node_(display_context->getSceneManager()

docs/source/configuration.rst

@@ -55,21 +55,27 @@ use_odometry
   If enabled, subscribes to `nav_msgs/Odometry`_ on the topic "odom". Odometry
   must be provided in this case, and the information will be included in SLAM.
 
-use_laser_scan
+num_laser_scans
-  If enabled, the node subscribes to `sensor_msgs/LaserScan`_ on the "scan"
+  Number of laser scan topics to subscribe to. Subscribes to
-  topic. If 2D SLAM is used, either this or *use_multi_echo_laser_scan*
+  `sensor_msgs/LaserScan`_ on the "scan" topic for one laser scanner, or topics
-  must be enabled.
+  "scan_1", "scan_2", etc. for multiple laser scanners.
 
-use_multi_echo_laser_scan
+num_multi_echo_laser_scans
-  If enabled, the node subscribes to  `sensor_msgs/MultiEchoLaserScan`_ on the
+  Number of multi-echo laser scan topics to subscribe to. Subscribes to
-  "echoes" topic. If 2D SLAM is used, either this or *use_laser_scan*
+  `sensor_msgs/MultiEchoLaserScan`_ on the "echoes" topic for one laser scanner,
-  must be enabled.
+  or topics "echoes_1", "echoes_2", etc. for multiple laser scanners.
+
+num_subdivisions_per_laser_scan
+  Number of point clouds to split each received (multi-echo) laser scan into.
+  Subdividing a scan makes it possible to unwarp scans acquired while the
+  scanners are moving. There is a corresponding trajectory builder option to
+  accumulate the subdivided scans into a point cloud that will be used for scan
+  matching.
 
 num_point_clouds
-  Number of 3D lasers to subscribe to. Must be a positive value if and only if
+  Number of point cloud topics to subscribe to. Subscribes to
-  using 3D SLAM. Subscribes to `sensor_msgs/PointCloud2`_ on the "points2"
+  `sensor_msgs/PointCloud2`_ on the "points2" topic for one rangefinder, or
-  topic for one laser, or topics "points2_1", "points2_2", etc for multiple
+  topics "points2_1", "points2_2", etc. for multiple rangefinders.
-  lasers.
 
 lookup_transform_timeout_sec
   Timeout in seconds to use for looking up transforms using `tf2`_.

docs/source/ros_api.rst

@@ -39,20 +39,24 @@ range data is required.
 
 scan (`sensor_msgs/LaserScan`_)
   Supported in 2D and 3D (e.g. using an axially rotating planar laser scanner).
-  If *use_laser_scan* is enabled in the :doc:`configuration`, this topic will
+  If *num_laser_scans* is set to 1 in the :doc:`configuration`, this topic will
-  be used as input for SLAM.
+  be used as input for SLAM. If *num_laser_scans* is greater than 1, multiple
+  numbered scan topics (i.e. scan_1, scan_2, scan_3, ...  up to and including
+  *num_laser_scans*) will be used as inputs for SLAM.
 
 echoes (`sensor_msgs/MultiEchoLaserScan`_)
   Supported in 2D and 3D (e.g. using an axially rotating planar laser scanner).
-  If *use_multi_echo_laser_scan* is enabled in the :doc:`configuration`, this
+  If *num_multi_echo_laser_scans* is set to 1 in the :doc:`configuration`, this
-  topic will be used as input for SLAM. Only the first echo is used.
+  topic will be used as input for SLAM. Only the first echo is used. If
+  *num_multi_echo_laser_scans* is greater than 1, multiple numbered echoes
+  topics (i.e. echoes_1, echoes_2, echoes_3, ...  up to and including
+  *num_multi_echo_laser_scans*) will be used as inputs for SLAM.
 
 points2 (`sensor_msgs/PointCloud2`_)
-  Only supported in 3D. If *num_point_clouds* is set to 1 in the
+  If *num_point_clouds* is set to 1 in the :doc:`configuration`, this topic will
-  :doc:`configuration`, this topic will be used as input for SLAM. If
+  be used as input for SLAM. If *num_point_clouds* is greater than 1, multiple
-  *num_point_clouds* is greater than 1, multiple numbered points2 topics (i.e.
+  numbered points2 topics (i.e. points2_1, points2_2, points2_3, ...  up to and
-  points2_1, points2_2, points2_3, ...  up to and including *num_point_clouds*)
+  including *num_point_clouds*) will be used as inputs for SLAM.
-  will be used as inputs for SLAM.
 
 The following additional sensor data topics may also be provided.