01 cartographer_ros进阶

node.cc

node.h和node.cc所包含的内容太多，只能分段解析！

01 构造函数

Node类的初始化是在node_main.cc中的Run函数中构造

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

Node类的构造函数一共完成了四步：

Step 1：声明需要发布的topic
Step 2：声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
Step 3：处理之后的点云的发布器
Step 4：进行定时器与函数的绑定, 定时发布数据

/**
 * @brief
 * 声明ROS的一些topic的发布器, 服务的发布器, 以及将时间驱动的函数与定时器进行绑定
 *
 * @param[in] node_options 配置文件的内容
 * @param[in] map_builder SLAM算法的具体实现
 * @param[in] tf_buffer tf
 * @param[in] collect_metrics 是否启用metrics,默认不启用
 */
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) {
  // 将mutex_上锁, 防止在初始化时数据被更改
  absl::MutexLock lock(&mutex_);

  // 默认不启用
  if (collect_metrics) {
    metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
  }

  // Step: 1 声明需要发布的topic

  // 发布SubmapList
  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_pose
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);
  // 发布约束
  constraint_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kConstraintListTopic, kLatestOnlyPublisherQueueSize);
  // 发布tracked_pose, 默认不发布
  if (node_options_.publish_tracked_pose) {
    tracked_pose_publisher_ =
        node_handle_.advertise<::geometry_msgs::PoseStamped>(
            kTrackedPoseTopic, kLatestOnlyPublisherQueueSize);
  }
  // lx add
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    point_cloud_map_publisher_ =
        node_handle_.advertise<sensor_msgs::PointCloud2>(
            kPointCloudMapTopic, kLatestOnlyPublisherQueueSize, true);
  }

  // Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
  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));

  // Step: 3 处理之后的点云的发布器
  scan_matched_point_cloud_publisher_ =
      node_handle_.advertise<sensor_msgs::PointCloud2>(
          kScanMatchedPointCloudTopic, kLatestOnlyPublisherQueueSize);

  // Step: 4 进行定时器与函数的绑定, 定时发布数据
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),  // 0.3s
      &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),  // 5e-3s
        &Node::PublishLocalTrajectoryData, this);
  }
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(
          node_options_.trajectory_publish_period_sec),  // 30e-3s
      &Node::PublishTrajectoryNodeList, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(
          node_options_.trajectory_publish_period_sec),  // 30e-3s
      &Node::PublishLandmarkPosesList, this));
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kConstraintPublishPeriodSec),  // 0.5s
      &Node::PublishConstraintList, this));
  // lx add
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    wall_timers_.push_back(node_handle_.createWallTimer(
        ::ros::WallDuration(kPointCloudMapPublishPeriodSec),  // 10s
        &Node::PublishPointCloudMap, this));
  }
}

02 开始轨迹

开始轨迹的流程

node_main.cc的Run函数构造了node类，马上利用node类中的成员函数StartTrajectoryWithDefaultTopics开启一条默认的轨迹！

变量start_trajectory_with_default_topics是在node_main.cc中，gflags定义的一些变量中定义的，默认为true

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

  // 如果加载了pbstream文件, 就执行这个函数
  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

  // 使用默认topic 开始轨迹
  if (FLAGS_start_trajectory_with_default_topics) {
    node.StartTrajectoryWithDefaultTopics(trajectory_options);
  }

在node类中的成员函数`StartTrajectoryWithDefaultTopics`中

首先检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息，存在返回true，不存在返回false
然后调用node类中的另一个成员函数AddTrajectory

// 使用默认topic名字开始一条轨迹,也就是开始slam
void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

在node类中的成员函数`AddTrajectory`中

首先调用node的成员函数ComputeExpectedSensorIds中，根据配置文件, 确定所有需要的topic的名字的集合
- lua文件中，在订阅的所有激光扫描、多回波激光扫描和点云主题的基础上，如果对应的话题只有一个传感器, 那订阅的topic就是topic；如果是存在多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推；
```
  num_laser_scans = 0,                      -- 是否使用单线激光数据
  num_multi_echo_laser_scans = 0,           -- 是否使用multi_echo_laser_scans数据
  num_subdivisions_per_laser_scan = 1,      -- 1帧数据被分成几次处理,一般为1
  num_point_clouds = 1,                     -- 是否使用点云数据
```
- 3d slam必须有imu, 2d可有可无, imu的topic的个数只能有一个
- Odometry（里程计）可有可无, topic的个数只能有一个
- gps可有可无, topic的个数只能有一个
- Landmark可有可无, topic的个数只能有一个
然后调用MapBuilderBridge类的成员函数AddTrajectory，传入上一步获得的topic的名字的集合以及TrajectoryOptions的配置参数！
- 这个函数在之后的MapBuilderBridge类的时候会详细注释
新增一个位姿估计器（imu和里程计的一个融合，它只是去做预测，前端匹配之前的先验！）
- 以1ms, 以及重力常数10, 作为参数构造PoseExtrapolator
新添加一个传感器采样器
- 频率为1的时候，表示每一帧点云数据都使用；表示两帧使用一次点云数据
- 从lua文件中获取自己所设置的各个传感器的频率（默认所有传感器的频率都为1）来进行参数构造TrajectorySensorSamplers
订阅话题与注册回调函数
- 实现了laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
- multi_echo_laser_scans的订阅与注册回调函数
- point_clouds 的订阅与注册回调函数
- imu 的订阅与注册回调函数,只有一个imu的topic
- odometry 的订阅与注册回调函数,只有一个odometry的topic
- gps 的订阅与注册回调函数,只有一个gps的topic
- landmarks 的订阅与注册回调函数,只有一个landmarks的topic
  
  这里使用的语法很复杂，用std::unordered_map<int, std::vector<Subscriber>>的变量subscribe_来订阅与注册回调函数，将trajectory_id作为键，一个函数指针和多个其它参数传入进一个函数，用该函数的返回值加上topic一起作为的值进行写入！最后得到的键是trajectory_id ，而值则是一个装有laser_scan，point_clouds，gps等Subscriber类的一个vector！
创建了一个3s执行一次的定时器
- 由于oneshot=true, 所以只执行一次，检查设置的topic名字是否在ros中存在, 不存在则报错
  （出现以上图片的错误的一个解决办法就是去launch文件重新设置remap即可！）
将topic名字保存下来,用于之后的新建轨迹时检查topic名字是否重复

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;
}

03 传感器数据的走向与传递

在开始轨迹的LaunchSubscribers中，注册了各个传感器的回调函数，当每一帧传感器数据传入时，对应的回调函数就会被调用来处理传感器数据！

传感器	回调函数	传感器数据走向
imu	Node::HandleImuMessage	处理imu数据,imu的数据走向有2个： 1）传入PoseExtrapolator，用于位姿预测与重力方向的确定； 2）第2个是传入SensorBridge，使用其传感器处理函数进行imu数据处理
里程计	Node::HandleOdometryMessage	处理里程计数据,里程计的数据走向有2个 1）传入PoseExtrapolator，用于位姿预测 2）传入SensorBridge,使用其传感器处理函数进行里程计数据处理
laser_scan	Node::HandleLaserScanMessage	调用SensorBridge的传感器处理函数进行数据处理
multi_echo_laser_scans	Node::HandleMultiEchoLaserScanMessage	调用SensorBridge的传感器处理函数进行数据处理
point_clouds	Node::HandlePointCloud2Message	调用SensorBridge的传感器处理函数进行数据处理
gps	Node::HandleNavSatFixMessage	调用SensorBridge的传感器处理函数进行数据处理
landmarks	Node::HandleLandmarkMessage	调用SensorBridge的传感器处理函数进行数据处理

可以发现，除了imu和里程计的数据走向多了一个传入PoseExtrapolator，其他传感器都直接传入了SensorBridge类中！

这里传入的PoseExtrapolator和前端的位姿推测器是同一个类下的不同对象，是相互独立的！

这里的PoseExtrapolator是否使用对应了lua文件的options中use_pose_extrapolator参数的设置！

这里以imu传感器的回调函数Node::HandleImuMessage为例

 * @param[in] trajectory_id 轨迹id
 * @param[in] sensor_id imu的topic名字
 * @param[in] msg imu的ros格式的数据
 
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;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  // extrapolators_使用里程计数据进行位姿预测
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

从代码可以看到，这里的sensor_bridge_ptr是由map_builder_bridge_.sensor_bridge(trajectory_id)得到的

所以在了解SensorBridge这个类之前首先得简单了解MapBuilderBridge这个类

MapBuilderBridge.h

各个传感器回调函数中比较关键的一句：

  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);

node.cc中的`map_builder_bridge_`从何而来？

上面的map_builder_bridge是在node.h中定义的

//GUARDED_BY(mutex_)表示使用时需要上锁
MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

初始化是在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) {
  // 将mutex_上锁, 防止在初始化时数据被更改
  absl::MutexLock lock(&mutex_);

因此，在node_main.cc构建node时，map_builder_bridge就存在了

  // Node类的初始化, 将ROS的topic传入SLAM, 也就是MapBuilder
  Node node(node_options, std::move(map_builder), &tf_buffer,
            FLAGS_collect_metrics);

`auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id)`的用法？

在MapBuilderBridge.h中有定义了一个成员变量的类型为std::unordered_map<int, std::unique_ptr<SensorBridge>> ，键为int类型，值为std::unique_ptr<SensorBridge>类型

  std::unordered_map<int, std::unique_ptr<SensorBridge>> sensor_bridges_;

对应的成员函数

// 获取对应轨迹id的SensorBridge的指针
SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
  return sensor_bridges_.at(trajectory_id).get();
}

因此，可以看到，sensor_bridges_.at(trajectory_id)就表示键为trajectory_id所对应的值std::unique_ptr<SensorBridge>，而值是指向SensorBridge类的智能指针，加上get()取原始指针，再将原始指针传入给sensor_bridge_ptr进行后续的传感器数据处理！

之前在开始轨迹中，在node类中的成员函数AddTrajectory中第二步，调用了MapBuilderBridge类的成员函数AddTrajectory

调用MapBuilder类中的成员函数AddTrajectoryBuilder，开始一条新的轨迹，接受返回的轨迹id
为这个新轨迹添加一个SensorBridge
保存轨迹的参数配置

// 开始一条新轨迹
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;
}

预备知识—— local frame 与 global frame

carographer中存在两个地图坐标系, 分别为global frame与local frame（slam也是这样）

local frame

是表达local slam结果的坐标系, 是固定的坐标系, 不会被回环检测与位姿图优化所更改,

其每一帧位姿间的坐标变换不会改变

global frame

是表达被回环检测与位姿图优化所更改后的坐标系, 当有新的优化结果可用时, 此坐标系与任何其他坐标系之间的转换都会跳变.

它的z轴指向上方, 即重力加速度矢量指向-z方向, 即由加速度计测得的重力分量沿+z方向.