From 8bcb34f4caacbb1d44753398e0d5fe9418d4b84d Mon Sep 17 00:00:00 2001 From: Linh Nguyen Date: Tue, 13 Sep 2022 20:32:09 +0800 Subject: [PATCH] Add option to ignore out of order sensor message (#1737) Check IMU and Odometry data before extrapolation Signed-off-by: Linh Nguyen --- .../cartographer_ros/map_builder_bridge.cc | 1 + cartographer_ros/cartographer_ros/node.cc | 6 ++- .../cartographer_ros/sensor_bridge.cc | 39 ++++++++++++++++++- .../cartographer_ros/sensor_bridge.h | 9 ++++- .../cartographer_ros/trajectory_options.cc | 6 +++ .../cartographer_ros/trajectory_options.h | 1 + 6 files changed, 57 insertions(+), 5 deletions(-) diff --git a/cartographer_ros/cartographer_ros/map_builder_bridge.cc b/cartographer_ros/cartographer_ros/map_builder_bridge.cc index c89a4f0..65925e3 100644 --- a/cartographer_ros/cartographer_ros/map_builder_bridge.cc +++ b/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( 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)); diff --git a/cartographer_ros/cartographer_ros/node.cc b/cartographer_ros/cartographer_ros/node.cc index 067ab5a..a7a0c14 100644 --- a/cartographer_ros/cartographer_ros/node.cc +++ b/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); diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.cc b/cartographer_ros/cartographer_ros/sensor_bridge.cc index 3254d98..9dc256d 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.cc +++ b/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 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 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 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(), diff --git a/cartographer_ros/cartographer_ros/sensor_bridge.h b/cartographer_ros/cartographer_ros/sensor_bridge.h index 807d816..400ac4a 100644 --- a/cartographer_ros/cartographer_ros/sensor_bridge.h +++ b/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, - double lookup_transform_timeout_sec, tf2_ros::Buffer* tf_buffer, + int num_subdivisions_per_laser_scan, bool ignore_out_of_order_messages, + 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 sensor_to_previous_subdivision_time_; + std::map latest_sensor_time_; const TfBridge tf_bridge_; ::cartographer::mapping::TrajectoryBuilderInterface* const trajectory_builder_; diff --git a/cartographer_ros/cartographer_ros/trajectory_options.cc b/cartographer_ros/cartographer_ros/trajectory_options.cc index 143f5b2..642a531 100644 --- a/cartographer_ros/cartographer_ros/trajectory_options.cc +++ b/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; } diff --git a/cartographer_ros/cartographer_ros/trajectory_options.h b/cartographer_ros/cartographer_ros/trajectory_options.h index 93817b7..e4b57c4 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_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;