Cartographer源码阅读---Sensor的数据走向
整个Cartographer源码阅读是很枯燥的, 但绝对是可以学到东西的! 坚持每天记录, 加油!
在上一节我们详细了解了MapBuilder类, 发现其构造函数, 以及AddTrajectory中有使用到SensorBridge这个类还有sensor_collator_这个变量, 并且似乎是用这个类进行传感器数据的传递的. 当然啦, 如果想建立一个完整的轨迹(Trajectory)和SLAM功能, 我们肯定需要有传感器的数据灌入的.
在MapBuilder的入口类-MapBuilderBridge中, 可以看到一个变量sensor_bridges_
std::unordered_map> 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(),carto::sensor::TransformTimedPointCloud(ranges, sensor_to_tracking->cast())} ); // 强度始终为空}
} 前几节也讲过这部分SensorBridge::HandleLaserScanMessage调用了SensorBridge::HandleRangefinder, 把carto::sensor::TimedPointCloudData这个数据类型和sensor_id通过trajectory_builder_的AddSensorData把点云类型传递给CollatedTrajectoryBuilder的AddSensorData. 为啥是CollatedTrajectoryBuilder的AddSensorData呢?这块我也想了很久. 咱们先去sensor_bridge.h中看
::cartographer::mapping::TrajectoryBuilderInterface* consttrajectory_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
map_builder_->GetTrajectoryBuilder(trajectory_id)这个map_builder_是MapBuilderBridge构造函数map_builder_(std::move(map_builder)), 而这个map_builder也是父类定义,没啥参考价值
std::unique_ptr 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 CreateMapBuilder(const proto::MapBuilderOptions& options) {return absl::make_unique(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
trajectory_builders_.push_back(absl::make_unique(trajectory_options, sensor_collator_.get(), trajectory_id,expected_sensor_ids,// 将3D前端与3D位姿图打包在一起, 传入CollatedTrajectoryBuilderCreateGlobalTrajectoryBuilder3D(std::move(local_trajectory_builder), trajectory_id,static_cast(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 data) {sensor_collator_->AddSensorData(trajectory_id_, std::move(data));
} 再看看sensor::MakeDispatchable, 在dispatchable.h文件中
// 根据传入的data的数据类型,自动推断DataType, 实现一个函数处理不同类型的传感器数据
template
std::unique_ptr> MakeDispatchable(const std::string &sensor_id, const DataType &data) {return absl::make_unique>(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 默认为falseif (options.collate_by_trajectory()) {sensor_collator_ = absl::make_unique();} else {// sensor_collator_初始化, 实际使用这个sensor_collator_ = absl::make_unique();} 一般collate_by_trajectory设置为false, 所以是absl::make_unique
// sensor::Collator的初始化sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_id_strings,[this](const std::string& sensor_id, std::unique_ptr 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) {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) {auto it = queues_.find(queue_key);// 如果queue_key不在queues_中, 就忽略dataif (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 queues_; // 多个数据队列 所以it->second就是Queue, 而Queue是个定义在OrderedMultiQueue的结构体
struct Queue {common::BlockingQueue> 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.// 如果队列未满, 则返回truebool QueueNotFullCondition() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {return queue_size_ == kInfiniteQueueSize || deque_.size() < queue_size_;}除了阻塞作用, 最大的所用就是把数据压入deque_, 咱们再看看这个deque_
template
...
std::deque 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();} // end for// 正常情况, 数据时间都超过common_start_timeif (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)>; 而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& 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) 带了个默认参数sensor_id[callback, sensor_id](std::unique_ptr 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) {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& expected_sensor_ids,std::unique_ptr wrapped_trajectory_builder) ...
{...// sensor::Collator的初始化sensor_collator_->AddTrajectory(trajectory_id, expected_sensor_id_strings,[this](const std::string& sensor_id, std::unique_ptr 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 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( trajectory_options, sensor_collator_.get(), trajectory_id, expected_sensor_ids,// 将2D前端与2D位姿图打包在一起, 传入CollatedTrajectoryBuilderCreateGlobalTrajectoryBuilder2D( //全局轨迹构建器//CreateGlobalTrajectoryBuilder2D是global_trajectory_builderd的方法,//继承自TrajectoryBuilderInterface,和CollatedTrajectoryBuilder一个父类std::move(local_trajectory_builder), //前端构建器trajectory_id, //static_cast(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的整个前后端.
中间的处理用到了很多父子类的互相调用, 一层套一层, 十分复杂, 要认真看才能懂里面的数据流.
手打不易,求点赞收藏!