C++ Cartographer源码中关于传感器的数据传递实现
上一节我们看了添加轨迹的相关处理，发现在此调用了LaunchSubscribers这个函数来订阅相关传感器数据. 这一节看看这些订阅的传感器数据的流动过程. 咱们进入Node::LaunchSubscribers看看
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id);可以看到, 传入的参数只有当前轨迹id和配置参数. 咱们看看激光雷达和超声雷达的内容
激光雷达,超声雷达与点云数据处理与传递
存在3中雷达数据, 一种是单线雷达点云(LaserScan), 一种是多回声波雷达点云(MultiEchoLaserScanMessage),一种是点云(PointCloud2)可以处理多线雷达,
// laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
for (const std::string& topic :
ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::LaserScan>(
&Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
this),
topic});
}
// multi_echo_laser_scans的订阅与注册回调函数
for (const std::string& topic : ComputeRepeatedTopicNames(
kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
&Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
&node_handle_, this),
topic});
}以激光雷达为例, 进入HandleLaserScanMessage
// 调用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);
}注释已经写了一部分内容了,除此之外还调用了sensor_bridge的同名函数HandleLaserScanMessage, 看参数表示这个激光雷达的数据是某个轨迹id的某个sensorid的信息.
在进到这里看看. 在sensor_bridge.cc中实现
// 处理LaserScan数据, 先转成点云,再传入trajectory_builder_
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}ToPointCloudWithIntensities把ros格式的LaserScan转化成carto格式的PointCloudWithIntensities,
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>返回值是一个打包好的std::tuple, 内容是carto格式的点云和时间戳, 然后再由std::tie解压成变量point_cloud和time, 传入真正的数据处理函数- HandleLaserScan. 进到这个函数再看看
const size_t start_index =
points.points.size() * i / num_subdivisions_per_laser_scan_;
const size_t end_index =
points.points.size() * (i + 1) / num_subdivisions_per_laser_scan_;这一块标会一帧雷达数据被分成多少部分处理. 这个分块函数还是挺有意思的, 学习一下自己以后也可以巧妙的均匀切分数据了.
接下来就是检查分割后点云是否有错: 通过时间先后, 因为上一段分块的点云的末尾的一个点的时间戳不可能比这一块点云的第一个点的时间戳还早,否则就是错的.
if (it != sensor_to_previous_subdivision_time_.end() &&
// 上一段点云的时间不应该大于等于这一段点云的时间
it->second >= subdivision_time) {
LOG(WARNING) << "Ignored subdivision of a LaserScan message from sensor "
<< sensor_id << " because previous subdivision time "
<< it->second << " is not before current subdivision time "
<< subdivision_time;
continue;
}
// 更新对应sensor_id的时间戳
sensor_to_previous_subdivision_time_[sensor_id] = subdivision_time;
// 检查点云的时间
for (auto& point : subdivision) {
point.time -= time_to_subdivision_end;
}
CHECK_EQ(subdivision.back().time, 0.f);然后调用当前类的HandleRangefinder方法,处理分段后的点云.
HandleRangefinder() 函数将点云点云从雷达坐标系下转到tracking_frame坐标系系下, 并转成
TimedPointCloudData格式的点云, 然后才传入到cartographer中.
值得注意的是,不论是激光雷达还是超声雷达,最终都调用的是SensorBridge::HandleLaserScan.
咱们再看看最后一种激光传感器类型-点云,不同于激光雷达和超声雷达, 这个Cartographer定义的传感器类型接受直接的点云数据, 所以免去了数据分割等数据处理. 他调用了SensorBridge::HandlePointCloud2Message这个方法, 这个方法同样调用了HandleRangefinder. 所以HandleRangefinder这个函数实现了各种雷达与点云传感器类型的统一.可以看到调用关系上点云少了一步.
里程计与IMU数据的走向
咱们看看Node::HandleImuMessage,
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的数据走向有2个:
1. 传入PoseExtrapolator,用于位姿预测与重力方向的确定,
2. 传入SensorBridge,使用其传感器处理函数进行imu数据处理
IMU数据通过sensor_bridge的ToImuData转化为了位姿推测器的imudata, 然后传递给了位姿推测器的AddImuData, 用于位姿预测. 同样, HandleImuMessage也采用了ToImuData转化了imu的data, 然后调用trajectory_builder_的AddSensorData添加数据到轨迹中, 实现定位.
在ToImuData方法中:
const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
time, CheckNoLeadingSlash(msg->header.frame_id));这个是通过tf得到传感器相对于机器人坐标系的转换关系, 由于imu的hz是很高的(1000hz左右), 所以机器人一般以IMU所在位子为机器人的坐标, 以减小计算量.
里程计和IMU数据是一样的, 都是两个数据走向, 参照IMU.
GPS与landmark数据走向
landmark从Node类中的回调函数进来,只有一个走向,直接传入
SensorBridge::HandleLandmarkMessage()函数中,通过tf转换到机器人坐标系下后,再从SensorBridge传递给cartographer.
GPS数据和landmark数据一样, 只有一个走向, 和landmark不同的是, GPS数据需要进行坐标转换,因为GPS得到的raw data是lla坐标, 需要转化为ECEF坐标, 然后计算两帧GPS数据之间的相对坐标转换. 程序如下:
// 通过这个坐标变换 乘以 之后的gps数据,就相当于减去了一个固定的坐标,从而得到了gps数据间的相对坐标变换
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::FixedFramePoseData{
time, absl::optional<Rigid3d>(Rigid3d::Translation(
ecef_to_local_frame_.value() *
LatLongAltToEcef(msg->latitude, msg->longitude,
msg->altitude)))});总结
综上所述, 所有的数据都要通过处理转化为Cartographer能够接受的数据结构, 然后通过调用trajectory_builder_的addsensordata方法, 把传感器数据给到Cartographer算法部. 这一部分没啥难点.