From e5c2881b90b28e4343f6c0d3f6c27481526c1ccd Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Fri, 21 Jul 2017 14:07:15 +0200 Subject: [PATCH] Support multiple LaserScan and MultiEchoLaserScan topics. (#435) This fixes #212. --- cartographer_ros/cartographer_ros/node.cc | 22 +++++++------- .../cartographer_ros/offline_node_main.cc | 17 ++++++----- .../cartographer_ros/trajectory_options.cc | 25 ++++++++-------- .../cartographer_ros/trajectory_options.h | 4 +-- .../configuration_files/backpack_2d.lua | 4 +-- .../configuration_files/backpack_3d.lua | 4 +-- cartographer_ros/configuration_files/pr2.lua | 4 +-- .../configuration_files/revo_lds.lua | 4 +-- .../configuration_files/taurob_tracker.lua | 4 +-- .../msg/TrajectoryOptions.msg | 4 +-- .../cartographer_rviz/drawable_submap.cc | 10 +++---- docs/source/configuration.rst | 30 +++++++++++-------- docs/source/ros_api.rst | 22 ++++++++------ 13 files changed, 84 insertions(+), 70 deletions(-) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index ed429ce..888061c 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -225,11 +225,14 @@ std::unordered_set Node::ComputeExpectedTopics( const cartographer_ros_msgs::SensorTopics& topics) { std::unordered_set expected_topics; - if (options.use_laser_scan) { - expected_topics.insert(topics.laser_scan_topic); + for (const string& topic : ComputeRepeatedTopicNames( + topics.laser_scan_topic, options.num_laser_scans)) { + expected_topics.insert(topic); } - if (options.use_multi_echo_laser_scan) { - expected_topics.insert(topics.multi_echo_laser_scan_topic); + for (const string& 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) { - const string topic = topics.laser_scan_topic; + for (const string& topic : ComputeRepeatedTopicNames( + topics.laser_scan_topic, options.num_laser_scans)) { subscribers_[trajectory_id].push_back( node_handle_.subscribe( topic, kInfiniteSubscriberQueueSize, @@ -276,9 +279,9 @@ void Node::LaunchSubscribers(const TrajectoryOptions& options, ->HandleLaserScanMessage(topic, msg); }))); } - - if (options.use_multi_echo_laser_scan) { - const string topic = topics.multi_echo_laser_scan_topic; + for (const string& topic : + ComputeRepeatedTopicNames(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( diff --git a/cartographer_ros/cartographer_ros/offline_node_main.cc b/cartographer_ros/cartographer_ros/offline_node_main.cc index a4f24f6..7137650 100644 --- a/cartographer_ros/cartographer_ros/offline_node_main.cc +++ b/cartographer_ros/cartographer_ros/offline_node_main.cc @@ -115,15 +115,16 @@ void Run(const std::vector& bag_filenames) { .second); }; - // For 2D SLAM, subscribe to exactly one horizontal laser. - if (trajectory_options.use_laser_scan) { - check_insert(kLaserScanTopic); + // Subscribe to all laser scan, multi echo laser scan, and point cloud topics. + for (const string& topic : ComputeRepeatedTopicNames( + kLaserScanTopic, trajectory_options.num_laser_scans)) { + check_insert(topic); } - if (trajectory_options.use_multi_echo_laser_scan) { - check_insert(kMultiEchoLaserScanTopic); + for (const string& topic : ComputeRepeatedTopicNames( + 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& 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); } diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index ec91ab4..2d423da 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/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 + - (options.num_point_clouds > 0), + CHECK_GE(options.num_laser_scans + options.num_multi_echo_laser_scans + + options.num_point_clouds, 1) - << "Configuration error: 'use_laser_scan', " - "'use_multi_echo_laser_scan' and 'num_point_clouds' are " - "mutually exclusive, but one is required."; + << "Configuration error: 'num_laser_scans', " + "'num_multi_echo_laser_scans' and 'num_point_clouds' are " + "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.use_multi_echo_laser_scan = - lua_parameter_dictionary->GetBool("use_multi_echo_laser_scan"); + options.num_laser_scans = + lua_parameter_dictionary->GetNonNegativeInt("num_laser_scans"); + 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->use_multi_echo_laser_scan = msg.use_multi_echo_laser_scan; + options->num_laser_scans = msg.num_laser_scans; + 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.use_multi_echo_laser_scan = options.use_multi_echo_laser_scan; + msg.num_laser_scans = options.num_laser_scans; + 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( diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index 9595cc6..6590958 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/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; - bool use_multi_echo_laser_scan; + int num_laser_scans; + int num_multi_echo_laser_scans; int num_subdivisions_per_laser_scan; int num_point_clouds; }; diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index eba2f09..79c6cbe 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/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, - use_multi_echo_laser_scan = true, + num_laser_scans = 0, + num_multi_echo_laser_scans = 1, num_subdivisions_per_laser_scan = 10, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 7898515..640f6ae 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/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, - use_multi_echo_laser_scan = false, + num_laser_scans = 0, + num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, num_point_clouds = 2, lookup_transform_timeout_sec = 0.2, diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 0fbf691..3a392a7 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/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, - use_multi_echo_laser_scan = false, + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 7b84bf0..b5cd375 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/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, - use_multi_echo_laser_scan = false, + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index fb7cc14..51743f1 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/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, - use_multi_echo_laser_scan = false, + num_laser_scans = 1, + num_multi_echo_laser_scans = 0, num_subdivisions_per_laser_scan = 1, num_point_clouds = 0, lookup_transform_timeout_sec = 0.2, diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg index 8f9b9d9..4c11426 100644 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ b/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 -bool use_multi_echo_laser_scan +int32 num_laser_scans +int32 num_multi_echo_laser_scans int32 num_subdivisions_per_laser_scan int32 num_point_clouds diff --git a/cartographer_rviz/cartographer_rviz/drawable_submap.cc b/cartographer_rviz/cartographer_rviz/drawable_submap.cc index 645d8ed..b5d529c 100644 --- a/cartographer_rviz/cartographer_rviz/drawable_submap.cc +++ b/cartographer_rviz/cartographer_rviz/drawable_submap.cc @@ -64,11 +64,11 @@ Ogre::Quaternion ToOgre(const Eigen::Quaterniond& q) { } // namespace -DrawableSubmap::DrawableSubmap( - const ::cartographer::mapping::SubmapId& id, - ::rviz::DisplayContext* const display_context, - ::rviz::Property* const submap_category, const bool visible, - const float pose_axes_length, const float pose_axes_radius) +DrawableSubmap::DrawableSubmap(const ::cartographer::mapping::SubmapId& id, + ::rviz::DisplayContext* const display_context, + ::rviz::Property* const submap_category, + const bool visible, const float pose_axes_length, + const float pose_axes_radius) : id_(id), display_context_(display_context), scene_node_(display_context->getSceneManager() diff --git a/docs/source/configuration.rst b/docs/source/configuration.rst index 8e0b209..d473de4 100644 --- a/docs/source/configuration.rst +++ b/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 - If enabled, the node subscribes to `sensor_msgs/LaserScan`_ on the "scan" - topic. If 2D SLAM is used, either this or *use_multi_echo_laser_scan* - must be enabled. +num_laser_scans + Number of laser scan topics to subscribe to. Subscribes to + `sensor_msgs/LaserScan`_ on the "scan" topic for one laser scanner, or topics + "scan_1", "scan_2", etc. for multiple laser scanners. -use_multi_echo_laser_scan - If enabled, the node subscribes to `sensor_msgs/MultiEchoLaserScan`_ on the - "echoes" topic. If 2D SLAM is used, either this or *use_laser_scan* - must be enabled. +num_multi_echo_laser_scans + Number of multi-echo laser scan topics to subscribe to. Subscribes to + `sensor_msgs/MultiEchoLaserScan`_ on the "echoes" topic for one laser scanner, + 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 - using 3D SLAM. Subscribes to `sensor_msgs/PointCloud2`_ on the "points2" - topic for one laser, or topics "points2_1", "points2_2", etc for multiple - lasers. + Number of point cloud topics to subscribe to. Subscribes to + `sensor_msgs/PointCloud2`_ on the "points2" topic for one rangefinder, or + topics "points2_1", "points2_2", etc. for multiple rangefinders. lookup_transform_timeout_sec Timeout in seconds to use for looking up transforms using `tf2`_. diff --git a/docs/source/ros_api.rst b/docs/source/ros_api.rst index af2361c..5351ae8 100644 --- a/docs/source/ros_api.rst +++ b/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 - be used as input for SLAM. + If *num_laser_scans* is set to 1 in the :doc:`configuration`, this topic will + 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 - topic will be used as input for SLAM. Only the first echo is used. + 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. 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 - :doc:`configuration`, this topic will be used as input for SLAM. If - *num_point_clouds* is greater than 1, multiple numbered points2 topics (i.e. - points2_1, points2_2, points2_3, ... up to and including *num_point_clouds*) - will be used as inputs for SLAM. + If *num_point_clouds* is set to 1 in the :doc:`configuration`, this topic will + be used as input for SLAM. If *num_point_clouds* is greater than 1, multiple + numbered points2 topics (i.e. points2_1, points2_2, points2_3, ... up to and + including *num_point_clouds*) will be used as inputs for SLAM. The following additional sensor data topics may also be provided.