From c58a262d5694138d579440dd78695b149b0662f8 Mon Sep 17 00:00:00 2001 From: Wolfgang Hess Date: Wed, 19 Jul 2017 14:15:11 +0200 Subject: [PATCH] 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. --- .../configuration_files_test.cc | 1 + .../cartographer_ros/map_builder_bridge.cc | 1 + .../cartographer_ros/node_options.h | 1 - .../cartographer_ros/sensor_bridge.cc | 36 +++++++++++++++---- .../cartographer_ros/sensor_bridge.h | 12 +++++-- .../cartographer_ros/trajectory_options.cc | 32 ++++++++++++----- .../cartographer_ros/trajectory_options.h | 1 + .../configuration_files/backpack_2d.lua | 2 ++ .../configuration_files/backpack_3d.lua | 1 + cartographer_ros/configuration_files/pr2.lua | 1 + .../configuration_files/revo_lds.lua | 1 + .../configuration_files/taurob_tracker.lua | 1 + .../msg/TrajectoryOptions.msg | 1 + 13 files changed, 71 insertions(+), 20 deletions(-) diff --git a/cartographer_ros/cartographer_ros/configuration_files_test.cc b/cartographer_ros/cartographer_ros/configuration_files_test.cc index 38f9034..82a7917 100644 --- a/cartographer_ros/cartographer_ros/configuration_files_test.cc +++ b/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" diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index 6d5f730..e4e54e6 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/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( + trajectory_options.num_subdivisions_per_laser_scan, trajectory_options.tracking_frame, node_options_.lookup_transform_timeout_sec, tf_buffer_, map_builder_.GetTrajectoryBuilder(trajectory_id)); diff --git a/cartographer_ros/cartographer_ros/node_options.h b/cartographer_ros/cartographer_ros/node_options.h index 431d80f..c695140 100644 --- a/cartographer_ros/cartographer_ros/node_options.h +++ b/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 { diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index e1d7f0e..965db39 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/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, - tf2_ros::Buffer* const tf_buffer, + const int num_subdivisions_per_laser_scan, const string& tracking_frame, + 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, - ToPointCloudWithIntensities(*msg).points); + HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, + 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, - ToPointCloudWithIntensities(*msg).points); + HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id, + 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, diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index add78b1..6e728c3 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/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, - tf2_ros::Buffer* tf_buffer, + int num_subdivisions_per_laser_scan, const string& tracking_frame, + 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_; }; diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index 5b0e943..ec91ab4 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/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"); - - 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."; - + CheckTrajectoryOptions(options); 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); diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index a02a130..9595cc6 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/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; }; diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 9279d3c..eba2f09 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/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 diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index bdcf559..7898515 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/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, diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 3c2ba22..0fbf691 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/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, diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index 4be039c..7b84bf0 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/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, diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index 2ca2a84..fb7cc14 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/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, diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg index e010812..8f9b9d9 100644 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ b/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