描述

Run()函数中的函数AddTrajectory()源码继续解析

int Node::AddTrajectory(const TrajectoryOptions& options) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddExtrapolator(trajectory_id, options);
  AddSensorSamplers(trajectory_id, options);
  LaunchSubscribers(options, trajectory_id);
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

核心语句包括以下几个步骤，展开来分析，今天这篇分析3、4、5、6

1.ComputeExpectedSensorIds 确定传感器topic【已分析】
2.AddTrajectory 增加轨迹 【已分析】
3.AddExtrapolator
4.AddSensorSamplers
5.LaunchSubscribers
6.createWallTimer
解析
1. AddExtrapolator

void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms
  CHECK(extrapolators_.count(trajectory_id) == 0);
  const double gravity_time_constant =
      node_options_.map_builder_options.use_trajectory_builder_3d()
          ? options.trajectory_builder_options.trajectory_builder_3d_options()
                .imu_gravity_time_constant()
          : options.trajectory_builder_options.trajectory_builder_2d_options()
                .imu_gravity_time_constant();
  extrapolators_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

1.

constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms

没什么可说的，定义了一个double型变量，从名字上来看，叫做“ 推断估计时间（秒）”

extrapolators_声明在了node.h中

std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;

PoseExtrapolator声明在pose_extrapolator.h中

CHECK(extrapolators_.count(trajectory_id) == 0);

显然，这句话的意思就是在找map中，是否有trajectory_id的PoseExtrapolator

3.

const double gravity_time_constant =
      node_options_.map_builder_options.use_trajectory_builder_3d()
          ? options.trajectory_builder_options.trajectory_builder_3d_options()
                .imu_gravity_time_constant()
          : options.trajectory_builder_options.trajectory_builder_2d_options()
                .imu_gravity_time_constant();

没有时间去扒代码了，这句话的意思很明显。
一个double型的重力时间常量，如果现在是3D建图，就用3D的参数，否则使用2D的参数

4.

extrapolators_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));

piecewise_construct的作用是，结合std::forward_as_tuple使用，避免把整个std::forward_as_tuple(trajectory_id)当成key
而::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec), gravity_time_constant正是::cartographer::mapping::PoseExtrapolator类的构造函数参数

std::forward_as_tuple的作用，就是让它的参数成为一个tuple
显然这句话就是向extrapolators_推送一个新的键值对

2. AddSensorSamplers

void Node::AddSensorSamplers(const int trajectory_id,
                             const TrajectoryOptions& options) {
  CHECK(sensor_samplers_.count(trajectory_id) == 0);
  sensor_samplers_.emplace(
      std::piecewise_construct, std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          options.rangefinder_sampling_ratio, options.odometry_sampling_ratio,
          options.fixed_frame_pose_sampling_ratio, options.imu_sampling_ratio,
          options.landmarks_sampling_ratio));
}

sensor_samplers_依旧定义在node.h中，和上面一节extrapolators_一样，是个和trajectory_id相关的map类型

std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;

一眼看过去和上一小节很像，这段代码的功能，就是向sensor_samplers_推送一个键值对

3. LaunchSubscribers

本篇的分析重点

源码在node.cc中

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::LaserScan>(
             &Node::HandleLaserScanMessage, trajectory_id, topic, &node_handle_,
             this),
         topic});
  }
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::MultiEchoLaserScan>(
             &Node::HandleMultiEchoLaserScanMessage, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::PointCloud2>(
             &Node::HandlePointCloud2Message, trajectory_id, topic,
             &node_handle_, this),
         topic});
  }

  // For 2D SLAM, subscribe to the IMU if we expect it. For 3D SLAM, the IMU is
  // required.
  if (node_options_.map_builder_options.use_trajectory_builder_3d() ||
      (node_options_.map_builder_options.use_trajectory_builder_2d() &&
       options.trajectory_builder_options.trajectory_builder_2d_options()
           .use_imu_data())) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::Imu>(&Node::HandleImuMessage,
                                                trajectory_id, kImuTopic,
                                                &node_handle_, this),
         kImuTopic});
  }

  if (options.use_odometry) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<nav_msgs::Odometry>(&Node::HandleOdometryMessage,
                                                  trajectory_id, kOdometryTopic,
                                                  &node_handle_, this),
         kOdometryTopic});
  }
  if (options.use_nav_sat) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<sensor_msgs::NavSatFix>(
             &Node::HandleNavSatFixMessage, trajectory_id, kNavSatFixTopic,
             &node_handle_, this),
         kNavSatFixTopic});
  }
  if (options.use_landmarks) {
    subscribers_[trajectory_id].push_back(
        {SubscribeWithHandler<cartographer_ros_msgs::LandmarkList>(
             &Node::HandleLandmarkMessage, trajectory_id, kLandmarkTopic,
             &node_handle_, this),
         kLandmarkTopic});
  }
}

总体来说，这段代码分为两大部分吧

1.

第一部分

就是前三个for循环，分别对应：kLaserScanTopic、kMultiEchoLaserScanTopic、kPointCloud2Topic三个topic类型

for循环的变量，就是这三个topic类型通过ComputeRepeatedTopicNames()函数得到的“话题_id”字符串。

subscribers_声明在node.h中，依旧是一个map类型

std::unordered_map<int, std::vector<Subscriber>> subscribers_;

SubscribeWithHandler就定义在node.cc中，源码在下

template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    const int trajectory_id, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node) {
  return node_handle->subscribe<MessageType>(
      topic, kInfiniteSubscriberQueueSize,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

SubscribeWithHandler的函数功能：使用’node_handle’订阅’trajectory_id’的’topic’，并在’node’上调用’handler’来处理消息。返回订阅者subscriber。

2.

第二部分

和第一部分类似，只不过第一部分应该是针对点云的，slam中点云数据是必须的

在这一部分，通过LaunchSubscribers传入的const TrajectoryOptions& options，也就是通过参数文件设置的一些选项设定，来决定是否订阅对应消息。

options.trajectory_builder_options.trajectory_builder_2d_options().use_imu_data()、options.use_odometry、options.use_nav_sat、options.use_landmarks这几个参数，来决定是否向subscribers_推送对应订阅信息。

4. createWallTimer

wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kTopicMismatchCheckDelaySec),
      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));

// We have to keep the timer handles of ::ros::WallTimers around, otherwise
// they do not fire.
std::vector<::ros::WallTimer> wall_timers_;

我们必须保留::ros::WallTimers的定时器句柄，否则它们不会触发。

向这个wall_timers_的vector中推送::ros::WallTimer

这实际上会涉及到以后多传感器数据的时间戳对齐问题，目前暂不进一步分析，因为以后肯定还会再翻回来继续理解。