12345qiupeng
/
cartographer_ros
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

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>
master
Linh Nguyen 2022-09-13 20:32:09 +08:00 committed by GitHub
parent c06879b635
commit 8bcb34f4ca
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 57 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
cartographer_ros/cartographer_ros/map_builder_bridge.cc
View File

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

6
cartographer_ros/cartographer_ros/node.cc
View File

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

39
cartographer_ros/cartographer_ros/sensor_bridge.cc
View File

@ -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>(),

9
cartographer_ros/cartographer_ros/sensor_bridge.h
View File

@ -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<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_;

6
cartographer_ros/cartographer_ros/trajectory_options.cc
View File

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

1
cartographer_ros/cartographer_ros/trajectory_options.h
View File

@ -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;