先从node_main.cc文件开始看起
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);
构造了map_builder这个类
// 工厂函数
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options) {
return absl::make_unique<MapBuilder>(options);
}
使用了工厂函数,这里的MapBuilder是MapBuilderInterface基类的一个子类。
继续读node_main.cc的代码,可以发现构造了node类
// Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
Node node(node_options, std::move(map_builder), &tf_buffer,
//FLAGS_collect_metrics false
FLAGS_collect_metrics);
接下来看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)
: node_options_(node_options),
map_builder_bridge_(node_options_, std::move(map_builder), tf_buffer)
使用初始化的方法构造,在构造过程中初始化了map_builder_bridge这个类
MapBuilderBridge::MapBuilderBridge(
const NodeOptions& node_options,
std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
tf2_ros::Buffer* const tf_buffer)
: node_options_(node_options),
map_builder_(std::move(map_builder)),
tf_buffer_(tf_buffer)
MapBuilderBridge的初始化,过程中又构造了map_builder这个类
MapBuilder::MapBuilder(const proto::MapBuilderOptions& options)
: options_(options), thread_pool_(options.num_background_threads()) { // param: num_background_threads
CHECK(options.use_trajectory_builder_2d() ^
options.use_trajectory_builder_3d());
// 2d位姿图(后端)的初始化
if (options.use_trajectory_builder_2d()) {
pose_graph_ = absl::make_unique<PoseGraph2D>(
options_.pose_graph_options(),
absl::make_unique<optimization::OptimizationProblem2D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
}
// 3d位姿图(后端)的初始化
if (options.use_trajectory_builder_3d()) {
pose_graph_ = absl::make_unique<PoseGraph3D>(
options_.pose_graph_options(),
absl::make_unique<optimization::OptimizationProblem3D>(
options_.pose_graph_options().optimization_problem_options()),
&thread_pool_);
}
// 在 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>();
}
}
在构造map_builder时,触发了构造thread_pool线程池这个类
// 根据传入的数字, 进行线程池的构造, DoWork()函数开始了一个始终执行的for循环
ThreadPool::ThreadPool(int num_threads) {
CHECK_GT(num_threads, 0) << "ThreadPool requires a positive num_threads!";
absl::MutexLock locker(&mutex_);
for (int i = 0; i != num_threads; ++i) {
pool_.emplace_back([this]() { ThreadPool::DoWork(); });
}
}
线程池的实现
// 开始一个不停止的for循环, 如果任务队列不为空, 就执行第一个task
void ThreadPool::DoWork() {
#ifdef __linux__ //预编译指令,表示如果是在linux环境下就执行下面的指令
// This changes the per-thread nice level of the current thread on Linux. We
// do this so that the background work done by the thread pool is not taking
// away CPU resources from more important foreground threads.
/*
代码调整当前线程的优先级,通过 nice() 函数将当前线程的 nice 值设为 10。在 Linux 系统中,nice 值表示进程的优先级,
取值范围是 -20(最高优先级)到 19(最低优先级)。这里将线程优先级设置较高,
确保后台线程不会占用太多 CPU 资源,以免影响更重要的前台线程
*/
CHECK_NE(nice(10), -1);
#endif
const auto predicate = [this]() EXCLUSIVE_LOCKS_REQUIRED(mutex_) {
return !task_queue_.empty() || !running_;
};
// 始终执行, 直到running_为false时停止执行
for (;;) {
std::shared_ptr<Task> task;
{
absl::MutexLock locker(&mutex_);
mutex_.Await(absl::Condition(&predicate));
// map_builder.lua中设置的线程数, 4个线程处理同一个task_queue_
// 如果任务队列不为空, 那就取出第一个task
if (!task_queue_.empty()) {
task = std::move(task_queue_.front());
task_queue_.pop_front();
} else if (!running_) {
return;
}
}
CHECK(task);
CHECK_EQ(task->GetState(), common::Task::DEPENDENCIES_COMPLETED);
// 执行task
Execute(task.get());
}
}