里程计的回调函数为:
/**
* @brief 处理里程计数据,里程计的数据走向有2个
* 第1个是传入PoseExtrapolator,用于位姿预测
* 第2个是传入SensorBridge,使用其传感器处理函数进行里程计数据处理
*
* @param[in] trajectory_id 轨迹id
* @param[in] sensor_id 里程计的topic名字
* @param[in] 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;
}
//如果采样器在工作,首先通过map_builder_bridge_.sensor_bridge传入一个轨迹id并获取一个指针sensor_bridge_ptr
//然后拿着这个指针去调用ToOdometryData(msg)函数,传入参数为msg里程计参数。得到转换好的数据odometry_data_ptr
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) {
//然后将转换好的数据传入位姿推测器这个类使用AddOdometryData函数
extrapolators_.at(trajectory_id).AddOdometryData(*odometry_data_ptr);
}
//原始数据传入HandleOdometryMessage处理
sensor_bridge_ptr->HandleOdometryMessage(sensor_id, msg);
}
这里里程计数据有两个走向:一个是走向位姿估计器,一个是给到HandleOdometryMessage使用传感器处理函数进行里程计数据处理。
/**
* @brief 处理imu数据,imu的数据走向有2个
* 第1个是传入PoseExtrapolator,用于位姿预测与重力方向的确定
* 第2个是传入SensorBridge,使用其传感器处理函数进行imu数据处理
*
* @param[in] trajectory_id 轨迹id
* @param[in] sensor_id imu的topic名字
* @param[in] msg imu的ros格式的数据
*/
void Node::HandleImuMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::Imu::ConstPtr& msg) {
absl::MutexLock 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);
// extrapolators_使用里程计数据进行位姿预测
if (imu_data_ptr != nullptr) {
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}
里程计的数据走向与IMU的数据走向很相似,两个基本上是一样的。所以这两个传感器都有两个数据走向。
node里面的位姿推测器与前端的进行位姿估计的位姿推测器是独立的。node类里面的位姿推测器的作用主要是融合IMU和里程计输出来一个推测出来的位姿。根据参数决定是否拿着这个结果去发布tf。这里的参数就是对应着lua文件中的use-pose-extrapolator=false这个参数。当这个参数设置为true时,算法在使用tf变换时会使用位姿推测器推测出来的位姿。
注意这是一个先验的位姿,是通过IMU与里程计推测出来的位姿,没有经过激光数据验证的,所以不一定完全准确。因此一般情况下我们不使用这个位姿进行tf发布。当然如果你的里程计跟IMU特别特别准确的话可以设置位true使用。
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleNavSatFixMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::NavSatFix::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).fixed_frame_pose_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleNavSatFixMessage(sensor_id, msg);
}
这里跟前面两个不太一样,数据走向只有一条路到HandleNavSatFixMessage(sensor_id, msg)。
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLandmarkMessage(
const int trajectory_id, const std::string& sensor_id,
const cartographer_ros_msgs::LandmarkList::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).landmark_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLandmarkMessage(sensor_id, msg);
}
Landmark数据跟GPS相同,也是只经历一个处理。
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleLaserScanMessage(const int trajectory_id,
const std::string& sensor_id,
const sensor_msgs::LaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
// 根据配置,是否将传感器数据跳过
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandleMultiEchoLaserScanMessage(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleMultiEchoLaserScanMessage(sensor_id, msg);
}
// 调用SensorBridge的传感器处理函数进行数据处理
void Node::HandlePointCloud2Message(
const int trajectory_id, const std::string& sensor_id,
const sensor_msgs::PointCloud2::ConstPtr& msg) {
absl::MutexLock lock(&mutex_);
if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
return;
}
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandlePointCloud2Message(sensor_id, msg);
}
后面3-7的数据处理基本上是一样的,就是数据类型的传入有点区别然后处理的函数有点区别而已。
另外点云输入是传入到sensor_bridge函数中,这个函数在map_builder_bridge.h中:
// 获取对应轨迹id的SensorBridge的指针
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
return sensor_bridges_.at(trajectory_id).get();
}
该函数传入的是一个轨迹ID,输出一个指向sensor_bridge的指针,然后后面通过指针指向新的函数。这里思考一个问题:如果不用指针能不能指到这个函数?区别是什么?