cartographer源码阅读2

cartographer源码阅读2

---

前言

---

提示：以下是本篇文章正文内容

一、node_main.cc

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

这里可以学习一下C++11的新特性：
std::move
// 只是转移, 没有内存的搬迁或者内存拷贝所以可以提高利用效率,改善性能…
// 右值引用是用来支持转移语义的.转移语义可以将资源 ( 堆, 系统对象等 ) 从一个对象转移到另一个对象,
// 这样能够减少不必要的临时对象的创建、拷贝以及销毁, 能够大幅度提高 C++ 应用程序的性能.
// 临时对象的维护 ( 创建和销毁 ) 对性能有严重影响.

1. StartTrajectoryWithDefaultTopics（）；

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
  absl::MutexLock lock(&mutex_);
  // 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息
  CHECK(ValidateTrajectoryOptions(options));
  // 添加一条轨迹
  AddTrajectory(options);
}

1.1 ValidateTrajectoryOptions();

// 检查TrajectoryOptions是否存在2d或者3d轨迹的配置信息

bool Node::ValidateTrajectoryOptions(const TrajectoryOptions& options) {
  if (node_options_.map_builder_options.use_trajectory_builder_2d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_2d_options();
  }
  if (node_options_.map_builder_options.use_trajectory_builder_3d()) {
    return options.trajectory_builder_options
        .has_trajectory_builder_3d_options();
  }
  return false;
}

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

1.2.1 ComputeExpectedSensorIds();
type 是传感器类型，id 是就是ros 的　话题，多个相同传感器时在　话题　名字后加　_1。

std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
Node::ComputeExpectedSensorIds(const TrajectoryOptions& options) const {
  /*
    enum class SensorType {
      RANGE = 0,
      IMU,
      ODOMETRY,
      FIXED_FRAME_POSE,
      LANDMARK,
      LOCAL_SLAM_RESULT
    };

    struct SensorId {
      SensorType type;  // 传感器的种类
      std::string id;   // topic的名字
    };
  */
 
  using SensorId = cartographer::mapping::TrajectoryBuilderInterface::SensorId;
  using SensorType = SensorId::SensorType;
  std::set<SensorId> expected_topics;
  // Subscribe to all laser scan, multi echo laser scan, and point cloud topics.

  // 如果只有一个传感器, 那订阅的topic就是topic
  // 如果是多个传感器, 那订阅的topic就是topic_1,topic_2, 依次类推
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kLaserScanTopic, options.num_laser_scans)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  for (const std::string& topic : ComputeRepeatedTopicNames(
           kMultiEchoLaserScanTopic, options.num_multi_echo_laser_scans)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }
  for (const std::string& topic :
       ComputeRepeatedTopicNames(kPointCloud2Topic, options.num_point_clouds)) {
    expected_topics.insert(SensorId{SensorType::RANGE, topic});
  }

ComputeRepeatedTopicNames（）；
如果只有一个传感器, 那订阅的topic就是topic,
如果是多个传感器, 那订阅的topic就是topic_1,topic_2

std::vector<std::string> ComputeRepeatedTopicNames(const std::string& topic,
                                                   const int num_topics) {
  CHECK_GE(num_topics, 0);
  if (num_topics == 1) {
    return {topic};
  }
  std::vector<std::string> topics;
  topics.reserve(num_topics);
  for (int i = 0; i < num_topics; ++i) {
    topics.emplace_back(topic + "_" + std::to_string(i + 1));
  }
  // num_topics要是0就返回空的vector
  return topics;
}

1.2.2 AddExtrapolator();
新增一个位姿估计器
trajectory_id 轨迹id
options 参数配置

void Node::AddExtrapolator(const int trajectory_id,
                           const TrajectoryOptions& options) {
  constexpr double kExtrapolationEstimationTimeSec = 0.001;  // 1 ms

  // 新生成的轨迹的id 不应该在extrapolators_中
  CHECK(extrapolators_.count(trajectory_id) == 0);

  // imu_gravity_time_constant在2d, 3d中都是10
  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();

  // c++11: map::emplace() 用于通过在容器中插入新元素来扩展map容器
  // 元素是直接构建的（既不复制也不移动）.仅当键不存在时才进行插入
  // c++11: std::forward_as_tuple tuple的完美转发
  // 该 tuple 在以右值为参数时拥有右值引用数据成员, 否则拥有左值引用数据成员
  // c++11: std::piecewise_construct 分次生成tuple的标志常量
  // 在map::emplace()中使用forward_as_tuple时必须要加piecewise_construct,不加就报错
  // 

  // 以1ms, 以及重力常数10, 作为参数构造PoseExtrapolator
  extrapolators_.emplace(
      std::piecewise_construct, 
      std::forward_as_tuple(trajectory_id),
      std::forward_as_tuple(
          ::cartographer::common::FromSeconds(kExtrapolationEstimationTimeSec),
          gravity_time_constant));
}

forward_as_tuple

1.2.3 LaunchSubscribers();订阅话题与注册回调函数
可以订阅单个雷达，多个雷达以及点云，以单个雷达为例

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                             const int trajectory_id) {
  // laser_scan 的订阅与注册回调函数, 多个laser_scan 的topic 共用同一个回调函数
  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});
  }

 

SubscribeWithHandler（）；在node_handle中订阅topic,并与传入的回调函数进行注册

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,  // kInfiniteSubscriberQueueSize = 0
      // 使用boost::function构造回调函数,被subscribe注册
      boost::function<void(const typename MessageType::ConstPtr&)>(
          // c++11: lambda表达式
          [node, handler, trajectory_id, topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}

主要作用就是订阅一个以topic( 不同的传感器中的topic这个变量是否for循环体中的这一句代码赋值的const std::string& topic : ComputeRepeatedTopicNames( topics.laser_scan_topic, options.num_laser_scans) )为名字的Topic，然后返回了一个node_handle->subscribe。在LaunchSubscribers函数里把这个返回值压入了subscribers_[trajectory_id]列表中。

lamda表达式