Adds per-sensor fixed ratio samplers (#499)

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);
 }

cartographer_ros/cartographer_ros/node.h

@@ -23,6 +23,7 @@
 #include <unordered_set>
 #include <vector>
 
+#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<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
+  std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
   std::unordered_map<int, std::vector<::ros::Subscriber>> subscribers_;
   std::unordered_set<std::string> subscribed_topics_;
   std::unordered_map<int, bool> is_active_trajectory_ GUARDED_BY(mutex_);

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;

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(

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

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

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

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

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

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.