整个Cartographer源码阅读是很枯燥的, 但绝对是可以学到东西的! 坚持每天记录, 加油!
在上一节我们详细了解了MapBuilder类, 发现其构造函数, 以及AddTrajectory中有使用到SensorBridge这个类还有sensor_collator_这个变量, 并且似乎是用这个类进行传感器数据的传递的. 当然啦, 如果想建立一个完整的轨迹(Trajectory)和SLAM功能, 我们肯定需要有传感器的数据灌入的.
在MapBuilder的入口类-MapBuilderBridge中, 可以看到一个变量sensor_bridges_
std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;sensor_bridges_存储了一系列SensorBridge类的实例, 并且在MapBuilderBridge::AddTrajectory
的第二步使用, 作用是为当前轨迹添加一个SensorBridge.
所以这一节我们重点看看SensorBridge这个类. 我们以最重要的sensor类-LaserScan为例子
Node类的HandleLaserScanMessage函数
我们都知道, ros中软实时的程序数据的入口都是从subscriber的回调函数开始. 之前已经讲过了Node的LaunchSubscriber函数, 用来专门启动所有传感器的订阅. 咱们就看看Node类的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);
}我们可以看到, 他最终是调用了MapBuilderBridge类的sensor_bridge的成员函数-HandleLaserScanMessage来处理传入的传感器的数据.
SensorBridge类的HandleLaserScanMessage函数
咱们再去SensorBridge中看看, 以下是主要代码部分
// 处理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);
}
void SensorBridge::HandleRangefinder(
const std::string& sensor_id, const carto::common::Time time,
const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
if (sensor_to_tracking != nullptr) {
trajectory_builder_->AddSensorData(
sensor_id, carto::sensor::TimedPointCloudData{
time,
sensor_to_tracking->translation().cast<float>(),
carto::sensor::TransformTimedPointCloud(
ranges, sensor_to_tracking->cast<float>())} ); // 强度始终为空
}
}前几节也讲过这部分SensorBridge::HandleLaserScanMessage调用了SensorBridge::HandleRangefinder, 把carto::sensor::TimedPointCloudData这个数据类型和sensor_id通过trajectory_builder_的AddSensorData把点云类型传递给CollatedTrajectoryBuilder的AddSensorData. 为啥是CollatedTrajectoryBuilder的AddSensorData呢?这块我也想了很久. 咱们先去sensor_bridge.h中看
::cartographer::mapping::TrajectoryBuilderInterface* const
trajectory_builder_;发现是TrajectoryBuilderInterface, 这明显是个父类啊,没啥意义. 咱们回到SensorBridge的构造函数
/**
* @brief 构造函数, 并且初始化TfBridge
*
* @param[in] num_subdivisions_per_laser_scan 一帧数据分成几次发送
* @param[in] tracking_frame 数据都转换到tracking_frame
* @param[in] lookup_transform_timeout_sec 查找tf的超时时间
* @param[in] tf_buffer tf_buffer
* @param[in] trajectory_builder 轨迹构建器
*/
SensorBridge::SensorBridge(
const int num_subdivisions_per_laser_scan,
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),
tf_bridge_(tracking_frame, lookup_transform_timeout_sec, tf_buffer),
trajectory_builder_(trajectory_builder) {}发现这个trajectory_builder_是SensorBridge构造函数的最后一个参数, 那么这个SensorBridge是在哪构造的呢? 是在map_builder_bridge.cc中, MapBuilderBridge::AddTrajectory的第二步sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>. 我们看到最后一个参数是
map_builder_->GetTrajectoryBuilder(trajectory_id)这个map_builder_是MapBuilderBridge构造函数map_builder_(std::move(map_builder)), 而这个map_builder也是父类定义,没啥参考价值
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder再向前回溯, 看MapBuilderBridge是咋构造的, 发现是Node的构造函数就构造了map_builder_bridge_
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)又要往前回溯, 在node_main.cc中
auto map_builder = cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
Node node(node_options, std::move(map_builder), &tf_buffer, FLAGS_collect_metrics);发现map_builder是cartographer::mapping::CreateMapBuilder给的. 咱们再进CreateMapBuilder, 发现其只是一个工厂函数
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options) {
return absl::make_unique<MapBuilder>(options);
}而这个工厂函数实例化了MapBuilder这个类, 所以map_builder_->GetTrajectoryBuilder(trajectory_id)调用的是MapBuilder的GetTrajectoryBuilder. (有一种峰回路转的感觉), 返回trajectory_builders_的轨迹id为trajectory_id的指针,即:
mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
int trajectory_id) const override {
return trajectory_builders_.at(trajectory_id).get();
}而trajectory_builders_在map_builder.cc中被压入absl::make_unique<CollatedTrajectoryBuilder>, 如下
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
// 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilder
CreateGlobalTrajectoryBuilder3D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph3D*>(pose_graph_.get()),
local_slam_result_callback, pose_graph_odometry_motion_filter)));所以最终SensorBridge的trajectory_builder_实际上是CollatedTrajectoryBuilder的地址, 所以SensorBridge的trajectory_builder_的AddSensorData实际上是把数据添加到了CollatedTrajectoryBuilder里面, 而不是GlobalTrajectoryBuilder或者LocalTrajectoryBuilder(这三个TrajectoryBuilder都继承于TrajectoryBuilderInterface)
这块地方难就难在子类可以用父类代替, 搞不清到底是调用的哪个子类的成员函数.
CollatedTrajectoryBuilder类的AddSensorData函数
既然上面调用的是CollatedTrajectoryBuilder, 那咱们看看CollatedTrajectoryBuilder这个类的AddSensorData
void AddSensorData(
const std::string& sensor_id,
const sensor::TimedPointCloudData& timed_point_cloud_data) override {
AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));
}
void CollatedTrajectoryBuilder::AddData(std::unique_ptr<sensor::Data> data) {
sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
}再看看sensor::MakeDispatchable, 在dispatchable.h文件中
// 根据传入的data的数据类型,自动推断DataType, 实现一个函数处理不同类型的传感器数据
template <typename DataType>
std::unique_ptr<Dispatchable<DataType>> MakeDispatchable(
const std::string &sensor_id, const DataType &data) {
return absl::make_unique<Dispatchable<DataType>>(sensor_id, data);
}这个函数通过模板, 实现了一个函数处理多个类型, 也就是说可以用一个函数去分发上到激光雷达,下到IMU的数据, 值得学习.
CollatedTrajectoryBuilder::AddData又调用sensor_collator_->AddSensorData, 用std::move(data), 把data移动给AddSensorData, 给某个Trajectory加入传感器数据. 而这个sensor_collator_定义如下:
sensor::CollatorInterface* const sensor_collator_;又是用父类代替子类, 在CollatedTrajectoryBuilder的构造函数中实现实例化, 这个sensor_collator_实际上是sensor::Collator, 原因是在map_builder.cc中的MapBuilder构造函数中有如下一段程序
// 在 cartographer/configuration_files/map_builder.lua 中设置
// param: MAP_BUILDER.collate_by_trajectory 默认为false
if (options.collate_by_trajectory()) {
sensor_collator_ = absl::make_unique<sensor::TrajectoryCollator>();
} else {
// sensor_collator_初始化, 实际使用这个
sensor_collator_ = absl::make_unique<sensor::Collator>();
}一般collate_by_trajectory设置为false, 所以是absl::make_unique<sensor::Collator>, 即sensor的Collator
// sensor::Collator的初始化
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_id_strings,
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data)); //传递给GlobalTrajectoryBuilder类相应的函数
});Collator类的AddSensorData函数
咱们进到collator这个类中看看, 发现这个类继承于CollatorInterface, 再看看collator这个类的AddSensorData
// 向数据队列中添加 传感器数据
void Collator::AddSensorData(const int trajectory_id,
std::unique_ptr<Data> data) {
QueueKey queue_key{trajectory_id, data->GetSensorId()};
queue_.Add(std::move(queue_key), std::move(data));
}作用是 向队列中添加传感器数据, 啥是队列?以后将在线程池部分详细说说. 现在简单看看
queue_是Cartographer的任务队列, 用于线程池多任务序列的储存与处理.
// Queue keys are a pair of trajectory ID and sensor identifier.
OrderedMultiQueue queue_;也就是说Collator::AddSensorData负责把data放在任务队列中等待处理并赋一个key, 并不负责处理数据, 所以咱们再往前看看, 看一下OrderedMultiQueue这个类的关于添加数据的成员函数-Add
OrderedMultiQueue类的Add函数
OrderedMultiQueue这个类定义在ordered_multi_queue.cc中, 添加数据是在Add成员函数实现的:
// 向数据队列中添加数据
void OrderedMultiQueue::Add(const QueueKey& queue_key,
std::unique_ptr<Data> data) {
auto it = queues_.find(queue_key);
// 如果queue_key不在queues_中, 就忽略data
if (it == queues_.end()) {
LOG_EVERY_N(WARNING, 1000)
<< "Ignored data for queue: '" << queue_key << "'";
return;
}
// 向数据队列中添加数据
it->second.queue.Push(std::move(data));
// 传感器数据的分发处理
Dispatch();
}可以发现Add就是生产者, 用于生成并传递可用数据.
Dispatch()这个成员函数负责数据分发, 将处于数据队列中的数据根据时间依次传入回调函数. 这个后面再看, 咱们先看看it->second.queue.Push(std::move(data));这个部分.
it这个变量就是queues_最后一个数据,可以理解为最新的一个数据, 而OrderedMultiQueue的queue_和上一小节提到的Collator是不同的, 在OrderedMultiQueue中的queue_是定义为一个std::map
std::map<QueueKey, Queue> queues_; // 多个数据队列所以it->second就是Queue, 而Queue是个定义在OrderedMultiQueue的结构体
struct Queue {
common::BlockingQueue<std::unique_ptr<Data>> queue; // 存储数据的队列
Callback callback; // 本数据队列对应的回调函数
bool finished = false; // 这个queue是否finished
};Push 也就是把data压入Queue这个结构体中,然后生成map形成个对列. 而这个Push不是push_back, 这个Push是Cartographer自己定义的一种压栈方法. 定义在blocking_queue.h中,如下...
BlockingQueue类的Push函数
// Pushes a value onto the queue. Blocks if the queue is full.
// 将值压入队列. 如果队列已满, 则阻塞
void Push(T t) {
// 首先定义判断函数
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return QueueNotFullCondition();
};
// absl::Mutex的更多信息可看: https://www.jianshu.com/p/d2834abd6796
// absl官网: https://abseil.io/about/
// 如果数据满了, 就进行等待
absl::MutexLock lock(&mutex_);
mutex_.Await(absl::Condition(&predicate));
// 将数据加入队列, 移动而非拷贝
deque_.push_back(std::move(t));
}发现Push作用相当于阻塞者, 使用了mutex_.Await和锁用来阻塞数据传入. 看看QueueNotFullCondition这个函数就一目了然了. 当队列为无限大或者小于queue_size_的时候返回true.
// Returns true iff the queue is not full.
// 如果队列未满, 则返回true
bool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;
}除了阻塞作用, 最大的所用就是把数据压入deque_, 咱们再看看这个deque_
template <typename T>
...
std::deque<T> deque_ GUARDED_BY(mutex_);发现它是std::deque这个基础类型,类型决定于模板T, GUARDED_BY(mutex_)表示这个数据在使用的时候必须要上锁, 否则就会报错.
所以Push的作用就是有阻塞作用的push_back, 负责把data压入OrderedMultiQueue的queue_, 并且在队列满的时候阻塞.
OrderedMultiQueue类的Dispatch函数
咱们再回到OrderedMultiQueue类的Add函数的Dispatch中, 看看Dispatch函数有关数据分发的部分
void OrderedMultiQueue::Dispatch() {
while (true) {
const Data* next_data = nullptr;
Queue* next_queue = nullptr;
QueueKey next_queue_key;
// 遍历所有的数据队列, 找到所有数据队列的第一个数据中时间最老的一个数据
for (auto it = queues_.begin(); it != queues_.end();) {
const auto* data = it->second.queue.Peek<Data>();
} // end for
// 正常情况, 数据时间都超过common_start_time
if (next_data->GetTime() >= common_start_time) {
last_dispatched_time_ = next_data->GetTime();
// 将数据传入 callback() 函数进行处理,并将这个数据从数据队列中删除
next_queue->callback(next_queue->queue.Pop());
}
}
}Peek是取出队列最前面的一个数据. callback定义在头文件中, 这个是std::function封装的一个函数
using Callback = std::function<void(std::unique_ptr<Data>)>;而Callback这个函数到底是啥呢? 这个要看OrderedMultiQueue::AddQueue这个成员函数
void OrderedMultiQueue::AddQueue(const QueueKey& queue_key, Callback callback) {
CHECK_EQ(queues_.count(queue_key), 0);
queues_[queue_key].callback = std::move(callback);
}我们看到这个函数把参数传入的callback传入queues_的callback. 那么是谁调用的OrderedMultiQueue的AddQueue这个函数呢? 是Collator的AddTrajectory调用的!
我们在回溯到Collator这个类看看Collator的AddTrajectory成员函数
/**
* @brief 添加轨迹以生成排序的传感器输出, 每个topic设置一个回调函数
*
* @param[in] trajectory_id 新生成的轨迹的id
* @param[in] expected_sensor_ids 需要排序的topic名字的集合
* @param[in] callback 2个参数的回调函数, 实际是CollatedTrajectoryBuilder::HandleCollatedSensorData()函数
*/
void Collator::AddTrajectory(
const int trajectory_id,
const absl::flat_hash_set<std::string>& expected_sensor_ids,
const Callback& callback) {
for (const auto& sensor_id : expected_sensor_ids) {
const auto queue_key = QueueKey{trajectory_id, sensor_id};
queue_.AddQueue(queue_key,
// void(std::unique_ptr<Data> data) 带了个默认参数sensor_id
[callback, sensor_id](std::unique_ptr<Data> data) {
callback(sensor_id, std::move(data));
});
queue_keys_[trajectory_id].push_back(queue_key);
}
}我们看到它调用了queue_的AddQueue, 而queue_就是OrderedMultiQueue的实例化 ,所以这里的AddQueue是OrderedMultiQueue的AddQueue. Callback又是个lambda函数
[callback, sensor_id](std::unique_ptr<Data> data) {
callback(sensor_id, std::move(data));
}这个lambda函数调用的是传入的callback函数, 而这个Collator::AddTrajectory是谁调用的呢?实际上是CollatedTrajectoryBuilder. 在CollatedTrajectoryBuilder的构造函数中就实现了Collator这个类的初始化, 并且调用了AddTrajectory这个函数
CollatedTrajectoryBuilder::CollatedTrajectoryBuilder(
const proto::TrajectoryBuilderOptions& trajectory_options,
sensor::CollatorInterface* const sensor_collator, const int trajectory_id,
const std::set<SensorId>& expected_sensor_ids,
std::unique_ptr<TrajectoryBuilderInterface> wrapped_trajectory_builder) ...
{
...
// sensor::Collator的初始化
sensor_collator_->AddTrajectory(
trajectory_id, expected_sensor_id_strings,
[this](const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
HandleCollatedSensorData(sensor_id, std::move(data)); //传递给GlobalTrajectoryBuilder类相应的函数
});
}所以传入的参数是是HandleCollatedSensorData这个函数.
CollatedTrajectoryBuilder类的HandleCollatedSensorData函数
这个函数才是真正的消费者. 看一下HandleCollatedSensorData传入sensor data的部分:
void CollatedTrajectoryBuilder::HandleCollatedSensorData(
const std::string& sensor_id, std::unique_ptr<sensor::Data> data) {
// 将排序好的数据送入 GlobalTrajectoryBuilder中的AddSensorData()函数中进行使用
data->AddToTrajectoryBuilder(wrapped_trajectory_builder_.get());
}这个函数的作用是处理按照时间顺序分发的传感器数据, 在进去到Data的AddToTrajectoryBuilder里看看
Data这个类又是个基类, 里面有个纯虚函数
virtual void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *trajectory_builder) = 0;这个基类只有一个子类: Dispatchable. 进到这个子类中去看看AddToTrajectoryBuilder.
// 调用传入的trajectory_builder的AddSensorData()
void AddToTrajectoryBuilder(
mapping::TrajectoryBuilderInterface *const trajectory_builder) override {
trajectory_builder->AddSensorData(sensor_id_, data_);
}所以这里的trajectory_builder指的就是CollatedTrajectoryBuilder::HandleCollatedSensorData中调用的wrapped_trajectory_builder_. 而这个wrapped_trajectory_builder_是啥呢?这又要回溯到CollatedTrajectoryBuilder的初始构造中去, 在map_builder.cc中实现
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryBuilder>(
trajectory_options, sensor_collator_.get(), trajectory_id,
expected_sensor_ids,
// 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilder
CreateGlobalTrajectoryBuilder2D( //全局轨迹构建器
//CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,
//继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类
std::move(local_trajectory_builder), //前端构建器
trajectory_id, //
static_cast<PoseGraph2D*>(pose_graph_.get()), //后端位姿图
local_slam_result_callback, pose_graph_odometry_motion_filter)));我们看到wrapped_trajectory_builder_实际上是CreateGlobalTrajectoryBuilder2D, 这个在上回也说到就是GlobalTrajectoryBuilder这个类CreateGlobalTrajectoryBuilder2D, 返回Cartographer的前端和后端.
到这里, 整个Cartographer的传感器数据传递过程也就明了了.
从GlobalTrajectoryBuilder2D开始, 数据才真正走到SLAM的前端与后端部分.
总结
Cartographer中传感器数据的传入从Node类的HandleXXXMessage成员函数开始, 传递给SensorBridge类, 然后调用CollatedTrajectoryBuilder把数据给到Collator类, 由Collator进行消费这模式的处理, 然后再返回给CollatedTrajectoryBuilder完成Cartographer的整个前后端.
中间的处理用到了很多父子类的互相调用, 一层套一层, 十分复杂, 要认真看才能懂里面的数据流.
手打不易,求点赞收藏!
652
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
