From 3fab4ad6b619d13948fbf5cb1c363d6ff4c57f57 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Juraj=20Or=C5=A1uli=C4=87?= Date: Fri, 29 Sep 2017 09:04:41 +0200 Subject: [PATCH] Adds per-sensor fixed ratio samplers (#499) --- cartographer_ros/cartographer_ros/node.cc | 27 +++++++++++++++++++ cartographer_ros/cartographer_ros/node.h | 16 +++++++++++ .../cartographer_ros/trajectory_options.cc | 12 +++++++++ .../cartographer_ros/trajectory_options.h | 3 +++ .../configuration_files/backpack_2d.lua | 3 +++ .../configuration_files/backpack_3d.lua | 3 +++ cartographer_ros/configuration_files/pr2.lua | 3 +++ .../configuration_files/revo_lds.lua | 3 +++ .../configuration_files/taurob_tracker.lua | 3 +++ .../msg/TrajectoryOptions.msg | 3 +++ 10 files changed, 76 insertions(+) diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index be37763..9c62278 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/cartographer_ros/cartographer_ros/node.cc @@ -152,6 +152,16 @@ void Node::AddExtrapolator(const int trajectory_id, gravity_time_constant)); } +void Node::AddSensorSamplers(const int trajectory_id, + const TrajectoryOptions& options) { + CHECK(sensor_samplers_.count(trajectory_id) == 0); + sensor_samplers_.emplace(std::piecewise_construct, + std::forward_as_tuple(trajectory_id), + std::forward_as_tuple(options.rangefinder_sampling_ratio, + options.odometry_sampling_ratio, + options.imu_sampling_ratio)); +} + void Node::PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event) { carto::common::MutexLocker lock(&mutex_); for (const auto& entry : map_builder_bridge_.GetTrajectoryStates()) { @@ -272,6 +282,7 @@ int Node::AddTrajectory(const TrajectoryOptions& options, const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); + AddSensorSamplers(trajectory_id, options); LaunchSubscribers(options, topics, trajectory_id); is_active_trajectory_[trajectory_id] = true; subscribed_topics_.insert(expected_sensor_ids.begin(), @@ -388,6 +399,7 @@ int Node::AddOfflineTrajectory( const int trajectory_id = map_builder_bridge_.AddTrajectory(expected_sensor_ids, options); AddExtrapolator(trajectory_id, options); + AddSensorSamplers(trajectory_id, options); is_active_trajectory_[trajectory_id] = true; return trajectory_id; } @@ -461,6 +473,9 @@ void Node::HandleOdometryMessage(const int trajectory_id, const string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) { + return; + } auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg); if (odometry_data_ptr != nullptr) { @@ -472,6 +487,9 @@ void Node::HandleOdometryMessage(const int trajectory_id, void Node::HandleImuMessage(const int trajectory_id, const string& sensor_id, const sensor_msgs::Imu::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) { + return; + } auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id); auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg); if (imu_data_ptr != nullptr) { @@ -484,6 +502,9 @@ void Node::HandleLaserScanMessage(const int trajectory_id, const string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleLaserScanMessage(sensor_id, msg); } @@ -492,6 +513,9 @@ void Node::HandleMultiEchoLaserScanMessage( int trajectory_id, const string& sensor_id, const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } map_builder_bridge_.sensor_bridge(trajectory_id) ->HandleMultiEchoLaserScanMessage(sensor_id, msg); } @@ -500,6 +524,9 @@ void Node::HandlePointCloud2Message( const int trajectory_id, const string& sensor_id, const sensor_msgs::PointCloud2::ConstPtr& msg) { carto::common::MutexLocker lock(&mutex_); + if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) { + return; + } map_builder_bridge_.sensor_bridge(trajectory_id) ->HandlePointCloud2Message(sensor_id, msg); } diff --git a/cartographer_ros/cartographer_ros/node.h b/cartographer_ros/cartographer_ros/node.h index fc9274d..f4b34b8 100644 --- a/cartographer_ros/cartographer_ros/node.h +++ b/cartographer_ros/cartographer_ros/node.h @@ -23,6 +23,7 @@ #include #include +#include "cartographer/common/fixed_ratio_sampler.h" #include "cartographer/common/mutex.h" #include "cartographer/mapping/pose_extrapolator.h" #include "cartographer_ros/map_builder_bridge.h" @@ -115,6 +116,7 @@ class Node { int trajectory_id); void PublishSubmapList(const ::ros::WallTimerEvent& timer_event); void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options); + void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options); void PublishTrajectoryStates(const ::ros::WallTimerEvent& timer_event); void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event); void PublishConstraintList(const ::ros::WallTimerEvent& timer_event); @@ -138,8 +140,22 @@ class Node { std::vector<::ros::ServiceServer> service_servers_; ::ros::Publisher scan_matched_point_cloud_publisher_; + struct TrajectorySensorSamplers { + TrajectorySensorSamplers(double rangefinder_sampling_ratio, + double odometry_sampling_ratio, + double imu_sampling_ratio) + : rangefinder_sampler(rangefinder_sampling_ratio), + odometry_sampler(odometry_sampling_ratio), + imu_sampler(imu_sampling_ratio) {} + + ::cartographer::common::FixedRatioSampler rangefinder_sampler; + ::cartographer::common::FixedRatioSampler odometry_sampler; + ::cartographer::common::FixedRatioSampler imu_sampler; + }; + // These are keyed with 'trajectory_id'. std::map extrapolators_; + std::unordered_map sensor_samplers_; std::unordered_map> subscribers_; std::unordered_set subscribed_topics_; std::unordered_map is_active_trajectory_ GUARDED_BY(mutex_); diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index 4ed8d0f..a4eb58b 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/cartographer_ros/cartographer_ros/trajectory_options.cc @@ -59,6 +59,12 @@ TrajectoryOptions CreateTrajectoryOptions( "num_subdivisions_per_laser_scan"); options.num_point_clouds = lua_parameter_dictionary->GetNonNegativeInt("num_point_clouds"); + options.rangefinder_sampling_ratio = + lua_parameter_dictionary->GetDouble("rangefinder_sampling_ratio"); + options.odometry_sampling_ratio = + lua_parameter_dictionary->GetDouble("odometry_sampling_ratio"); + options.imu_sampling_ratio = + lua_parameter_dictionary->GetDouble("imu_sampling_ratio"); CheckTrajectoryOptions(options); return options; } @@ -75,6 +81,9 @@ bool FromRosMessage(const cartographer_ros_msgs::TrajectoryOptions& msg, options->num_subdivisions_per_laser_scan = msg.num_subdivisions_per_laser_scan; options->num_point_clouds = msg.num_point_clouds; + options->rangefinder_sampling_ratio = msg.rangefinder_sampling_ratio; + options->odometry_sampling_ratio = msg.odometry_sampling_ratio; + options->imu_sampling_ratio = msg.imu_sampling_ratio; if (!options->trajectory_builder_options.ParseFromString( msg.trajectory_builder_options_proto)) { LOG(ERROR) << "Failed to parse protobuf"; @@ -96,6 +105,9 @@ cartographer_ros_msgs::TrajectoryOptions ToRosMessage( 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; + msg.rangefinder_sampling_ratio = options.rangefinder_sampling_ratio; + msg.odometry_sampling_ratio = options.odometry_sampling_ratio; + msg.imu_sampling_ratio = options.imu_sampling_ratio; options.trajectory_builder_options.SerializeToString( &msg.trajectory_builder_options_proto); return msg; diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index 25c735d..e3063ec 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.h +++ b/cartographer_ros/cartographer_ros/trajectory_options.h @@ -38,6 +38,9 @@ struct TrajectoryOptions { int num_multi_echo_laser_scans; int num_subdivisions_per_laser_scan; int num_point_clouds; + double rangefinder_sampling_ratio; + double odometry_sampling_ratio; + double imu_sampling_ratio; }; TrajectoryOptions CreateTrajectoryOptions( diff --git a/cartographer_ros/configuration_files/backpack_2d.lua b/cartographer_ros/configuration_files/backpack_2d.lua index 79c6cbe..997c55d 100644 --- a/cartographer_ros/configuration_files/backpack_2d.lua +++ b/cartographer_ros/configuration_files/backpack_2d.lua @@ -32,6 +32,9 @@ options = { submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + imu_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/backpack_3d.lua b/cartographer_ros/configuration_files/backpack_3d.lua index 0f54498..70b49c5 100644 --- a/cartographer_ros/configuration_files/backpack_3d.lua +++ b/cartographer_ros/configuration_files/backpack_3d.lua @@ -32,6 +32,9 @@ options = { submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + imu_sampling_ratio = 1., } TRAJECTORY_BUILDER_3D.scans_per_accumulation = 160 diff --git a/cartographer_ros/configuration_files/pr2.lua b/cartographer_ros/configuration_files/pr2.lua index 3a392a7..e770d41 100644 --- a/cartographer_ros/configuration_files/pr2.lua +++ b/cartographer_ros/configuration_files/pr2.lua @@ -32,6 +32,9 @@ options = { submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + imu_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/revo_lds.lua b/cartographer_ros/configuration_files/revo_lds.lua index b5cd375..2623ac1 100644 --- a/cartographer_ros/configuration_files/revo_lds.lua +++ b/cartographer_ros/configuration_files/revo_lds.lua @@ -32,6 +32,9 @@ options = { submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + imu_sampling_ratio = 1., } MAP_BUILDER.use_trajectory_builder_2d = true diff --git a/cartographer_ros/configuration_files/taurob_tracker.lua b/cartographer_ros/configuration_files/taurob_tracker.lua index 996fb15..f71b50f 100644 --- a/cartographer_ros/configuration_files/taurob_tracker.lua +++ b/cartographer_ros/configuration_files/taurob_tracker.lua @@ -32,6 +32,9 @@ options = { submap_publish_period_sec = 0.3, pose_publish_period_sec = 5e-3, trajectory_publish_period_sec = 30e-3, + rangefinder_sampling_ratio = 1., + odometry_sampling_ratio = 1., + imu_sampling_ratio = 1., } TRAJECTORY_BUILDER_3D.scans_per_accumulation = 180 diff --git a/cartographer_ros_msgs/msg/TrajectoryOptions.msg b/cartographer_ros_msgs/msg/TrajectoryOptions.msg index 4c11426..95e62fe 100644 --- a/cartographer_ros_msgs/msg/TrajectoryOptions.msg +++ b/cartographer_ros_msgs/msg/TrajectoryOptions.msg @@ -21,6 +21,9 @@ int32 num_laser_scans int32 num_multi_echo_laser_scans int32 num_subdivisions_per_laser_scan int32 num_point_clouds +float64 rangefinder_sampling_ratio +float64 odometry_sampling_ratio +float64 imu_sampling_ratio # This is a binary-encoded # 'cartographer.mapping.proto.TrajectoryBuilderOptions' proto.