首先是MapBuilder的声明与构造
node_main.cc
auto map_builder =
cartographer::mapping::CreateMapBuilder(node_options.map_builder_opt
ions);
Node node(node_options, std::move(map_builder), &tf_buffer,
FLAGS_collect_metrics);
CreateMapBuilder函数
std::unique_ptr<MapBuilderInterface> CreateMapBuilder(
const proto::MapBuilderOptions& options) {
return absl::make_unique<MapBuilder>(options);
}
函数使用 absl::make_unique
来创建一个 MapBuilder
对象,并将 options
作为参数传递给 MapBuilder
的构造函数,然后将其封装在一个 std::unique_ptr<MapBuilderInterface>
中,并返回该指针。
在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),
将map_builder的所有权给转移
map_builder_bridge_是MapBuilderBridge类
再看MapBuilderBridge类的构造
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) {}
将map_builder的所有权在给转移
MapBuilder的构造在上文的工厂函数就创建了
轨迹开始的处理
node_main.cc的StartTrajectoryWithDefaultTopics函数
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
absl::MutexLock lock(&mutex_);
// 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
CHECK(ValidateTrajectoryOptions(options));
// 添加一条轨迹
AddTrajectory(options);
}
Node的AddTrajectory函数
int Node::AddTrajectory(const TrajectoryOptions& options) {
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
expected_sensor_ids = ComputeExpectedSensorIds(options);
// 调用map_builder_bridge的AddTrajectory, 添加一个轨迹
const int trajectory_id =
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
// 新增一个位姿估计器
AddExtrapolator(trajectory_id, options);
// 新生成一个传感器数据采样器
AddSensorSamplers(trajectory_id, options);
// 订阅话题与注册回调函数
LaunchSubscribers(options, trajectory_id);
// 创建了一个3s执行一次的定时器,由于oneshot=true, 所以只执行一次
// 检查设置的topic名字是否在ros中存在, 不存在则报错
wall_timers_.push_back(node_handle_.createWallTimer(
::ros::WallDuration(kTopicMismatchCheckDelaySec), // kTopicMismatchCheckDelaySec = 3s
&Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
// 将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复
for (const auto& sensor_id : expected_sensor_ids) {
subscribed_topics_.insert(sensor_id.id);
}
return trajectory_id;
}
map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
MapBuilderBridge类的AddTrajectory函数、
int MapBuilderBridge::AddTrajectory(
const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
expected_sensor_ids,
const TrajectoryOptions& trajectory_options) {
// Step: 1 开始一条新的轨迹, 返回新轨迹的id,需要传入一个函数
const int trajectory_id = map_builder_->AddTrajectoryBuilder(
expected_sensor_ids, trajectory_options.trajectory_builder_options,
// lambda表达式 local_slam_result_callback_
[this](const int trajectory_id,
const ::cartographer::common::Time time,
const Rigid3d local_pose,
::cartographer::sensor::RangeData range_data_in_local,
const std::unique_ptr<
const ::cartographer::mapping::TrajectoryBuilderInterface::
InsertionResult>) {
// 保存local slam 的结果数据 5个参数实际只用了4个
OnLocalSlamResult(trajectory_id, time, local_pose, range_data_in_local);
});
LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";
// Make sure there is no trajectory with 'trajectory_id' yet.
CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
// Step: 2 为这个新轨迹 添加一个SensorBridge
sensor_bridges_[trajectory_id] = absl::make_unique<SensorBridge>(
trajectory_options.num_subdivisions_per_laser_scan,
trajectory_options.tracking_frame,
node_options_.lookup_transform_timeout_sec,
tf_buffer_,
map_builder_->GetTrajectoryBuilder(trajectory_id)); // CollatedTrajectoryBuilder
// Step: 3 保存轨迹的参数配置
auto emplace_result =
trajectory_options_.emplace(trajectory_id, trajectory_options);
CHECK(emplace_result.second == true);
return trajectory_id;
}
调用了MapBuilder::AddTrajectoryBuilder函数
int MapBuilder::AddTrajectoryBuilder(
const std::set<SensorId>& expected_sensor_ids,
const proto::TrajectoryBuilderOptions& trajectory_options,
LocalSlamResultCallback local_slam_result_callback) {
// CollatedTrajectoryBuilder 初始化
trajectory_builders_.push_back(absl::make_unique<CollatedTrajectoryB
uilder>(
trajectory_options,
sensor_collator_.get(), // sensor::Collator
trajectory_id,
expected_sensor_ids,
CreateGlobalTrajectoryBuilder2D(
std::move(local_trajectory_builder), trajectory_id,
static_cast<PoseGraph2D*>(pose_graph_.get()),
local_slam_result_callback,
pose_graph_odometry_motion_filter)));
return trajectory_id;
}
// 返回指向 CollatedTrajectoryBuilder 的指针
mapping::TrajectoryBuilderInterface *GetTrajectoryBuilder(
int trajectory_id) const override {
return trajectory_builders_.at(trajectory_id).get();
}