Add option to ignore out of order sensor message (#1737)

Check IMU and Odometry data before extrapolation

Signed-off-by: Linh Nguyen <linh.nguyen@enway.ai>

cartographer_ros/cartographer_ros/map_builder_bridge.cc

@@ -138,6 +138,7 @@ int MapBuilderBridge::AddTrajectory(
   CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
   sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
       trajectory_options.num_subdivisions_per_laser_scan,
+      trajectory_options.ignore_out_of_order_messages,
       trajectory_options.tracking_frame,
       node_options_.lookup_transform_timeout_sec, tf_buffer_,
       map_builder_->GetTrajectoryBuilder(trajectory_id));

cartographer_ros/cartographer_ros/node.cc

@@ -771,7 +771,8 @@ void Node::HandleOdometryMessage(const int trajectory_id,
   }
   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) {
+  if (odometry_data_ptr != nullptr &&
+      !sensor_bridge_ptr->IgnoreMessage(sensor_id, odometry_data_ptr->time)) {
     extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
   }
   sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
@@ -808,7 +809,8 @@ void Node::HandleImuMessage(const int trajectory_id,
   }
   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) {
+  if (imu_data_ptr != nullptr &&
+      !sensor_bridge_ptr->IgnoreMessage(sensor_id, imu_data_ptr->time)) {
     extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
   }
   sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);

cartographer_ros/cartographer_ros/sensor_bridge.cc

@@ -41,10 +41,11 @@ const std::string& CheckNoLeadingSlash(const std::string& frame_id) {
 
 SensorBridge::SensorBridge(
     const int num_subdivisions_per_laser_scan,
-    const std::string& tracking_frame,
+    const bool ignore_out_of_order_messages, const std::string& tracking_frame,
     const double lookup_transform_timeout_sec, tf2_ros::Buffer* const tf_buffer,
     carto::mapping::TrajectoryBuilderInterface* const trajectory_builder)
     : num_subdivisions_per_laser_scan_(num_subdivisions_per_laser_scan),
+      ignore_out_of_order_messages_(ignore_out_of_order_messages),
       tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
       trajectory_builder_(trajectory_builder) {}
 
@@ -61,11 +62,31 @@ std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
           time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
 }
 
+bool SensorBridge::IgnoreMessage(const std::string& sensor_id,
+                                 cartographer::common::Time sensor_time) {
+  if (!ignore_out_of_order_messages_) {
+    return false;
+  }
+  auto it = latest_sensor_time_.find(sensor_id);
+  if (it == latest_sensor_time_.end()) {
+    return false;
+  }
+  return sensor_time <= it->second;
+}
+
 void SensorBridge::HandleOdometryMessage(
     const std::string& sensor_id, const nav_msgs::Odometry::ConstPtr& msg) {
   std::unique_ptr<carto::sensor::OdometryData> odometry_data =
       ToOdometryData(msg);
   if (odometry_data != nullptr) {
+    if (IgnoreMessage(sensor_id, odometry_data->time)) {
+      LOG(WARNING) << "Ignored odometry message from sensor " << sensor_id
+                   << " because sensor time " << odometry_data->time
+                   << " is not before last odometry message time "
+                   << latest_sensor_time_[sensor_id];
+      return;
+    }
+    latest_sensor_time_[sensor_id] = odometry_data->time;
     trajectory_builder_->AddSensorData(
         sensor_id,
         carto::sensor::OdometryData{odometry_data->time, odometry_data->pose});
@@ -146,6 +167,14 @@ void SensorBridge::HandleImuMessage(const std::string& sensor_id,
                                     const sensor_msgs::Imu::ConstPtr& msg) {
   std::unique_ptr<carto::sensor::ImuData> imu_data = ToImuData(msg);
   if (imu_data != nullptr) {
+    if (IgnoreMessage(sensor_id, imu_data->time)) {
+      LOG(WARNING) << "Ignored IMU message from sensor " << sensor_id
+                   << " because sensor time " << imu_data->time
+                   << " is not before last IMU message time "
+                   << latest_sensor_time_[sensor_id];
+      return;
+    }
+    latest_sensor_time_[sensor_id] = imu_data->time;
     trajectory_builder_->AddSensorData(
         sensor_id,
         carto::sensor::ImuData{imu_data->time, imu_data->linear_acceleration,
@@ -232,6 +261,14 @@ void SensorBridge::HandleRangefinder(
   const auto sensor_to_tracking =
       tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
   if (sensor_to_tracking != nullptr) {
+    if (IgnoreMessage(sensor_id, time)) {
+      LOG(WARNING) << "Ignored Rangefinder message from sensor " << sensor_id
+                   << " because sensor time " << time
+                   << " is not before last Rangefinder message time "
+                   << latest_sensor_time_[sensor_id];
+      return;
+    }
+    latest_sensor_time_[sensor_id] = time;
     trajectory_builder_->AddSensorData(
         sensor_id, carto::sensor::TimedPointCloudData{
                        time, sensor_to_tracking->translation().cast<float>(),

cartographer_ros/cartographer_ros/sensor_bridge.h

@@ -43,8 +43,9 @@ namespace cartographer_ros {
 class SensorBridge {
  public:
   explicit SensorBridge(
-      int num_subdivisions_per_laser_scan, const std::string& tracking_frame,
+      int num_subdivisions_per_laser_scan, bool ignore_out_of_order_messages,
-      double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer,
+      const std::string& tracking_frame, double lookup_transform_timeout_sec,
+      tf2_ros::Buffer* tf_buffer,
       ::cartographer::mapping::TrajectoryBuilderInterface* trajectory_builder);
 
   SensorBridge(const SensorBridge&) = delete;
@@ -73,6 +74,8 @@ class SensorBridge {
                                 const sensor_msgs::PointCloud2::ConstPtr& msg);
 
   const TfBridge& tf_bridge() const;
+  bool IgnoreMessage(const std::string& sensor_id,
+                     ::cartographer::common::Time sensor_time);
 
  private:
   void HandleLaserScan(
@@ -85,8 +88,10 @@ class SensorBridge {
                          const ::cartographer::sensor::TimedPointCloud& ranges);
 
   const int num_subdivisions_per_laser_scan_;
+  const bool ignore_out_of_order_messages_;
   std::map<std::string, cartographer::common::Time>
       sensor_to_previous_subdivision_time_;
+  std::map<std::string, cartographer::common::Time> latest_sensor_time_;
   const TfBridge tf_bridge_;
   ::cartographer::mapping::TrajectoryBuilderInterface* const
       trajectory_builder_;

cartographer_ros/cartographer_ros/trajectory_options.cc

@@ -76,6 +76,12 @@ TrajectoryOptions CreateTrajectoryOptions(
       lua_parameter_dictionary->GetDouble("imu_sampling_ratio");
   options.landmarks_sampling_ratio =
       lua_parameter_dictionary->GetDouble("landmarks_sampling_ratio");
+  if (lua_parameter_dictionary->HasKey("ignore_out_of_order")) {
+    options.ignore_out_of_order_messages =
+        lua_parameter_dictionary->GetBool("ignore_out_of_order");
+  } else {
+    options.ignore_out_of_order_messages = false;
+  }
   CheckTrajectoryOptions(options);
   return options;
 }

cartographer_ros/cartographer_ros/trajectory_options.h

@@ -36,6 +36,7 @@ struct TrajectoryOptions {
   bool use_nav_sat;
   bool use_landmarks;
   bool publish_frame_projected_to_2d;
+  bool ignore_out_of_order_messages;
   int num_laser_scans;
   int num_multi_echo_laser_scans;
   int num_subdivisions_per_laser_scan;