一、前言
前段时间去忙了些杂活项目，调调代码测测算法，这几天闲下来准备硕士开题的事情（SLAM方面能开展的工作大家都大同小异），之前在梳理实验室程序的时候遇到一些阻碍，有一部分是引用的cartographer的东西，师兄把那部分的代码阉的千奇百怪的，只能去找谷歌源代码看看阉之前是什么样子了，顺便也系统的学习一下谷歌维护这个工程的方式。
二、数据接收部分梳理
先看cmakelist，分别在_ros、msg和rviz里面，一共有四个list，工程依赖的东西都比较常规，形成的方式也很暴力，直接把所有.h和.cc文件加到ALL_SRCS里，把所有mian_cc做成可执行文件，打开_ros一看，三十多个c文件，再打开launch文件一看，又是十几个launch文件，痛苦面具。轮番观察了一下发现主要用的是下面几个节点
<launch>
<param name="robot_description"
textfile="$(find cartographer_ros)/urdf/backpack_2d.urdf" />
<node name="robot_state_publisher" pkg="robot_state_publisher"
type="robot_state_publisher" />
<node name="cartographer_node" pkg="cartographer_ros"
type="cartographer_node" args="
-configuration_directory $(find cartographer_ros)/configuration_files
-configuration_basename backpack_2d.lua"
output="screen">
<remap from="echoes" to="horizontal_laser_2d" />
</node>
<node name="cartographer_occupancy_grid_node" pkg="cartographer_ros"
type="cartographer_occupancy_grid_node" args="-resolution 0.05" />
</launch>比较常规的用了robot_description、robot_state_publisher，启的节点主要有cartographer_node和cartographer_occupancy_grid_node，前者是前端里程计，后者是生成占据栅格地图的节点，本文先主要分析2d建图中的里程计部分，后面的内容有机会再去学习。不过2d和3d启动的节点都一样，只是传进去的para不同。
找到node_mian.cc
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
google::ParseCommandLineFlags(&argc, &argv, true);
CHECK(!FLAGS_configuration_directory.empty())
<< "-configuration_directory is missing.";
CHECK(!FLAGS_configuration_basename.empty())
<< "-configuration_basename is missing.";
::ros::init(argc, argv, "cartographer_node");
::ros::start();
cartographer_ros::ScopedRosLogSink ros_log_sink;
cartographer_ros::Run();
::ros::shutdown();
}公式化定义了几个量，检查了一些config文件，构造了ros_log_sink，然后运行了Run函数。ros_log_sink顺着找过去看了一下，主要是和glog相关的，用来打印程序输出的错误问题等，Run函数比较重要
void Run() {
//tf2监听对象，最长缓冲时间10s
constexpr double kTfBufferCacheTimeInSeconds = 10.;
tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
tf2_ros::TransformListener tf(tf_buffer);
//分别在两个.h文件内,将config文件内的para读取到程序里边
NodeOptions node_options;
TrajectoryOptions trajectory_options;
std::tie(node_options, trajectory_options) =
LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);
//这里调用的是cartographer/mapping/map_builder.cc里面的类，将node_options.map_builder_options传入到map_builder
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
//构造node，具体内容在node.cc中,93行
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
//读配置文件
if (!FLAGS_load_state_filename.empty()) {
node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
}
//默认为true，主进程，通过接受默认话题构造轨迹
if (FLAGS_start_trajectory_with_default_topics) {
node.StartTrajectoryWithDefaultTopics(trajectory_options);
}
::ros::spin();
//结束建立轨迹
node.FinishAllTrajectories();
//全局优化
node.RunFinalOptimization();
if (!FLAGS_save_state_filename.empty()) {
node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}
}初始化之后，主要由构造函数Node node起作用，运行程序的巨大部分内容，然后运行FinishAllTrajectories和RunFinalOptimization两个函数。
关于node的构造：
Node::Node(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer, const bool collect_metrics)
//这里构建的两个东西分别是NodeOptions类和MapBuilderBridge类
: node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer) {
//线程锁
absl::MutexLock lock(&mutex_);
//默认是0，不记录metrics_registry_中的信息
if (collect_metrics) {
metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
carto::metrics::RegisterAllMetrics(metrics_registry_.get());
}
//publisher
submap_list_publisher_ =
node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
kSubmapListTopic, kLatestOnlyPublisherQueueSize);
trajectory_node_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);
landmark_poses_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
constraint_list_publisher_ =
node_handle_.advertise<::visualization_msgs::MarkerArray>(
kConstraintListTopic, kLatestOnlyPublisherQueueSize);
if (node_options_.publish_tracked_pose) {
tracked_pose_publisher_ =
node_handle_.advertise<::geometry_msgs::PoseStamped>(
kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
}
service_servers_.push_back(node_handle_.advertiseService(
kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));
service_servers_.push_back(node_handle_.advertiseService(
kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kFinishTrajectoryServiceName, &Node::HandleFinishTrajectory, this));
service_servers_.push_back(node_handle_.advertiseService(
kWriteStateServiceName, &Node::HandleWriteState, this));
service_servers_.push_back(node_handle_.advertiseService(
kGetTrajectoryStatesServiceName, &Node::HandleGetTrajectoryStates, this));
service_servers_.push_back(node_handle_.advertiseService(
kReadMetricsServiceName, &Node::HandleReadMetrics, this));
scan_matched_point_cloud_publisher_ =
node_handle_.advertise<sensor_msgs::PointCloud2>(
kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.submap_publish_period_sec),
&Node::PublishSubmapList, this));
if (node_options_.pose_publish_period_sec > 0) {
publish_local_trajectory_data_timer_ = node_handle_.createTimer(
::ros::Duration(node_options_.pose_publish_period_sec),
&Node::PublishLocalTrajectoryData, this);
}
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishTrajectoryNodeList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(node_options_.trajectory_publish_period_sec),
&Node::PublishLandmarkPosesList, this));
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kConstraintPublishPeriodSec),
&Node::PublishConstraintList, this));
}主要是构建了一些publisher，service和walltime，把后面计算出来的位姿和轨迹等东西发出来。
比较大的一个程序调用是StartTrajectoryWithDefaultTopics，在node.cc里面
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
CHECK(ValidateTrajectoryOptions(options));
AddTrajectory(options);
}
bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
return options.trajectory_builder_options
.has_trajectory_builder_2d_options();
}
if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
return options.trajectory_builder_options
.has_trajectory_builder_3d_options();
}
return false;
}CHECK里面读的是lua文件（主要是frame和一些传感器的参数），确定是2d还是3d的地图建立模式，然后进入最大的程序AddTrajectory!
int Node::AddTrajectory(const TrajectoryOptions& options) {
//构建expected_sensor_ids，ComputeExpectedSensorIds函数返回的是订阅的话题名称，包括要订阅的雷达的数量、话题名称和imu、里程计等的话题名称
//小小的嘴一句，这里谷歌写的东西感觉就是在脱裤子放屁，几个函数几个文件调来调去，放在学院派就是一行代码的事情
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
//这里不是自己调自己，map_builder_bridge_类定义在map_builder_bridge.h文件里，也是把sensor_id写到trajectory_id里了
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
//对extrapolators_操作，将trajectory_id和options内参数写入extrapolators_
AddExtrapolator(trajectory_id, options);
//和AddExtrapolator类似，对sensor_samplers_进行插入操作
AddSensorSamplers(trajectory_id, options);
//创建subscriber，调用回调函数，是这个大函数里最大的一块
LaunchSubscribers(options, trajectory_id);
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec),
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}里面有三个比较大的函数
void Node::AddExtrapolator(const int trajectory_id,
const TrajectoryOptions& options) {
constexpr double kExtrapolationEstimationTimeSec = 0.001; // 1 ms
//定义在node.h里 std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;确保extrapolators_内部没有trajectory_id
CHECK(extrapolators_.count(trajectory_id) == 0);
//选择2d还是3d
const double gravity_time_constant =
node_options_.map_builder_options.use_trajectory_builder_3d()
? options.trajectory_builder_options.trajectory_builder_3d_options()
.imu_gravity_time_constant()
: options.trajectory_builder_options.trajectory_builder_2d_options()
.imu_gravity_time_constant();
//对extrapolators_进行插入操作
//c++11: map::emplace() 用于通过在容器中插入新元素来扩展map容器
extrapolators_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
gravity_time_constant));
}
//和AddExtrapolator类似，对sensor_samplers_进行插入操作
void Node::AddSensorSamplers(const int trajectory_id,
const TrajectoryOptions& options) {
CHECK(sensor_samplers_.count(trajectory_id) == 0);
sensor_samplers_.emplace(
std::piecewise_construct, std::forward_as_tuple(trajectory_id),
std::forward_as_tuple(
options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
options.landmarks_sampling_ratio));
}前两个函数比较类似，都是把options里的参数写入到extrapolators_和sensor_samplers_里面，然后第三个函数比较重要，建立了全部的subscribers，调用了回调函数，从这里开始整个工程才算正式启动了
//创建subscriber，调用回调函数，是这个大函数里最大的一块
void Node::LaunchSubscribers(const TrajectoryOptions& options,
const int trajectory_id) {
//订阅话题的形式基本一致，从options中取出对应话题的字符串，然后调用对应的回调函数
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});
}
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});
}
for (const std::string& topic :
ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::PointCloud2>(
&Node::HandlePointCloud2Message, trajectory_id, topic,
&node_handle_, this),
topic});
}
// For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
// required.
if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
(node_options_.map_builder_options.use_trajectory_builder_2d() &&
options.trajectory_builder_options.trajectory_builder_2d_options()
.use_imu_data())) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
trajectory_id, kImuTopic,
&node_handle_, this),
kImuTopic});
}
if (options.use_odometry) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
trajectory_id, kOdometryTopic,
&node_handle_, this),
kOdometryTopic});
}
if (options.use_nav_sat) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<sensor_msgs::NavSatFix>(
&Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
&node_handle_, this),
kNavSatFixTopic});
}
if (options.use_landmarks) {
subscribers_[trajectory_id].push_back(
{SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
&Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
&node_handle_, this),
kLandmarkTopic});
}
}
以HandleLaserScanMessage为例分析传入雷达点云话题，这里函数调用跳来跳去简直让人头皮发麻，不过在跳来跳去的过程中，看到后面的HandleMultiEchoLaserScanMessage、HandlePointCloud2Message的回调函数了，想来雷达点云的调用也是大同小异的，只是传入的点云类型不一样，调用的时候变量名啥的不太一样，从这里也能很明显的看出cartographer对于数据处理的严谨，以及繁杂。
//node.cc中的回调函数
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;
}
//调用sensor_bridge中的函数，将点云信息处理后，带上时间戳、frame等信息，装入map_builder_bridge_
map_builder_bridge_.sensor_bridge(trajectory_id)
->HandleLaserScanMessage(sensor_id, msg);
}
//sensor_bridge.cc中的函数
void SensorBridge::HandleLaserScanMessage(
const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
carto::sensor::PointCloudWithIntensities point_cloud;
carto::common::Time time;
//调用msg_conversion.cc中的函数，
std::tie(point_cloud, time) = ToPointCloudWithIntensities(*msg);
HandleLaserScan(sensor_id, time, msg->header.frame_id, point_cloud);
}
//msg_conversion.cc中的函数
std::tuple<::cartographer::sensor::PointCloudWithIntensities,
::cartographer::common::Time>
ToPointCloudWithIntensities(const sensor_msgs::LaserScan& msg) {
return LaserScanToPointCloudWithIntensities(msg);
}
// For sensor_msgs::LaserScan and sensor_msgs::MultiEchoLaserScan.
//处理点云信息，最后都装到point_cloud里
template <typename LaserMessageType>
std::tuple<PointCloudWithIntensities, ::cartographer::common::Time>
LaserScanToPointCloudWithIntensities(const LaserMessageType& msg) {
//检查最小点云距离大于0
CHECK_GE(msg.range_min, 0.f);
//检查最大点云距离大于最小点云距离
CHECK_GE(msg.range_max, msg.range_min);
//检查角度分辨率,点云起始角度与结束角度
if (msg.angle_increment > 0.f) {
CHECK_GT(msg.angle_max, msg.angle_min);
} else {
CHECK_GT(msg.angle_min, msg.angle_max);
}
PointCloudWithIntensities point_cloud;
//点云起始角度
float angle = msg.angle_min;
//这里的ranges类比pcl里面的points[i]
for (size_t i = 0; i < msg.ranges.size(); ++i) {
const auto& echoes = msg.ranges[i];
//引于 https://gaoyichao.com/Xiaotu/?book=Cartographer%E6%BA%90%E7%A0%81%E8%A7%A3%E8%AF%BB&title=%E4%BD%BF%E7%94%A8SensorBridge%E8%BD%AC%E6%8D%A2%E6%BF%80%E5%85%89%E4%BC%A0%E6%84%9F%E5%99%A8%E6%95%B0%E6%8D%AE
//HasEcho有两个重载版本，分别应用于LaserScan和MultiEchoLaserScan，对于LaserScan的ROS消息这个函数将总是返回true，
//对于MultiEchoLaserScan则检查字段ranges是否为空。 如果存在测量数据，则通过函数GetFirstEcho获取第一个扫描数据。
//对于LaserScan直接返回测量数据，对于MultiEchoLaserScan则返回第一个测量数据。
if (HasEcho(echoes)) {
const float first_echo = GetFirstEcho(echoes);
//检查一下距离范围，没问题的话就计算旋转角，
if (msg.range_min <= first_echo && first_echo <= msg.range_max) {
//构造一个沿机器人Z轴（向上）转动的转动矢量，旋转角度为点云起始角度（最小角度）
const Eigen::AngleAxisf rotation(angle, Eigen::Vector3f::UnitZ());
//通过first_echo构建一个X轴方向上的运动矢量（向前），并左乘旋转矩阵就可以得到扫描点在机器人坐标系下的坐标
//msg.time_increment是扫描间隔，乘i为时间戳
const cartographer::sensor::TimedRangefinderPoint point{
rotation * (first_echo * Eigen::Vector3f::UnitX()),
i * msg.time_increment};
point_cloud.points.push_back(point);
//如果有intensity信息的话一并写入
if (msg.intensities.size() > 0) {
CHECK_EQ(msg.intensities.size(), msg.ranges.size());
const auto& echo_intensities = msg.intensities[i];
CHECK(HasEcho(echo_intensities));
point_cloud.intensities.push_back(GetFirstEcho(echo_intensities));
} else {
point_cloud.intensities.push_back(0.f);
}
}
}
//切换到下一个测量数据
angle += msg.angle_increment;
}
//这里的处理相当于把起始时间从世界时间换到这一组扫描数据开始的时间，方便处理
::cartographer::common::Time timestamp = FromRos(msg.header.stamp);
if (!point_cloud.points.empty()) {
const double duration = point_cloud.points.back().time;
timestamp += cartographer::common::FromSeconds(duration);
for (auto& point : point_cloud.points) {
point.time -= duration;
}
}
//返回点云数据
return std::make_tuple(point_cloud, timestamp);
}
//之后又回到sensor_bridge.cc里
void SensorBridge::HandleLaserScan(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id,
const carto::sensor::PointCloudWithIntensities& points) {
if (points.points.empty()) {
return;
}
CHECK_LE(points.points.back().time, 0.f);
// TODO(gaschler): Use per-point time instead of subdivisions.
//num_subdivisions_per_laser_scan写在lua文件里，2d是10,3d是1,2d的目的是把点云拆成若干段来处理
//这里按2d分析
for (int i = 0; i != num_subdivisions_per_laser_scan_; ++i) {
//将点云均等拆成10段
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_;
carto::sensor::TimedPointCloud subdivision(
points.points.begin() + start_index, points.points.begin() + end_index);
if (start_index == end_index) {
continue;
}
//数据写入subdivision的时间是点云段中最后一个数据的时间
const double time_to_subdivision_end = subdivision.back().time;
// `subdivision_time` is the end of the measurement so sensor::Collator will
// send all other sensor data first.
const carto::common::Time subdivision_time =
time + carto::common::FromSeconds(time_to_subdivision_end);
//谷歌真的喜欢拿it当临时变量
//如果有点云的时间戳超过了subdivision_time
auto it = sensor_to_previous_subdivision_time_.find(sensor_id);
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_to_previous_subdivision_time_[sensor_id] = subdivision_time;
//这一段点云内的点，时间戳都减去time_to_subdivision_end
for (auto& point : subdivision) {
point.time -= time_to_subdivision_end;
}
CHECK_EQ(subdivision.back().time, 0.f);
HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}
}
void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
if (!ranges.empty()) {
CHECK_LE(ranges.back().time, 0.f);
}
//查找雷达坐标系相对于机器人坐标系的坐标转换
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_里
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time, sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())});
}
}实在是没想到这个点云的处理过程这么长，究其原因应该是谷歌为了把几种来源不同的点云数据都统一格式统一处理，集中送到 trajectory_builder_里面，明显是做产品的思路，让人敬畏的工作量。
然后再看一下比较重要的imu数据的处理
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;
}
//从trajectory_id里取出想订阅的话题
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
//ToImuData检查了一下有没有角速度，加速度，已经tf转换是否正常
//返回的有时间戳、变化到机器人坐标系下的角速度、线速度
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
if (imu_data_ptr != nullptr &&
!sensor_bridge_ptr->IgnoreMessage(sensor_id, imu_data_ptr->time)) {
//把变换过后的imu信息装入extrapolators_
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
//保证接受到的imu信息是最新的
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}比雷达的数据处理要简单的多，主要完成了一个坐标变换，然后保证了数据是最新的。
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;
}
//从trajectory_id里取出想订阅的话题
auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
//ToImuData检查了一下有没有角速度，加速度，以及tf转换是否正常（没有偏离机器人太远）
//返回的有时间戳、变化到机器人坐标系下的角速度、线速度
auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
if (imu_data_ptr != nullptr &&
!sensor_bridge_ptr->IgnoreMessage(sensor_id, imu_data_ptr->time)) {
//把变换过后的imu信息装入extrapolators_
//这里我看到另外一版的函数，调用的是trajectory_builder_里面的AddSensorData，和激光点云最后的处理类似
//这里调用的是pose_extrapolator.cc里面的AddImuData函数,把imu数据送到imu_data_里了
extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
}
//保证接受到的imu信息是最新的
sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}之后看了一下里程计等东西的sub函数，和imu大同小异，主要是调用的extrapolators_里面的AddXXXXXData的名字不一样，就不在这里再做赘述了
三、后话
至此数据接收部分就结束了，实在是没想到这个雷达点云的处理函数能跳来跳去跳这么多，本来最开始的目的是找cartographer的帧间匹配部分的代码来看的，稍微看上头了浪费了一天多的时间，后面的部分应该也会再作学习总结，希望最近没有什么别的杂活项目吧
747
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
