trajectory_id 轨迹id
sensor_id 里程计的topic名字
msg 里程计的ros格式的数据
void Node::HandleOdometryMessage(const int trajectory_id,
const std::string& sensor_id,
const nav_msgs::Odometry::ConstPtr& msg) {
absl::MutexLock 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);
// extrapolators_使用里程计数据进行位姿预测
if (odometry_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}
例如(odom的回调函数):
absl::MutexLock lock(&mutex_);(上锁)
在node.h中有如下这个成员,因为回调函数用到了所以上锁
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);
if (!sensor_samplers_.at(trajectory_id).odometry_sampler.Pulse()) {
return;
}
查看有没有数据;
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
获取SensorBridge类的指针,调用trajectory_builder_的AddSensorData进行数据的处理
auto odometry_data_ptr = sensor_bridge_ptr->ToOdometryData(msg);
调用函数将数据格式转换
// extrapolators_使用里程计数据进行位姿预测
if (odometry_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
odom准确就可以用,在lua文件中可以设置。