[C++知识库]Cartographer_learn_1

写在前面

刚刚从隔壁的视觉slam跑过来学习激光slam，听说cartographer是激光slam的经典，所以决定通过学习cartographer来学习激光slam，刚刚开始学习，边学习边写博客巩固学习成果，如所述有误还请指出。

程序的入口

整个算法的入口在:
/src/cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 文件中

## 主函数
// An highlighted block
int main(int argc, char** argv) {
......
  // ros节点的初始化
  ::ros::init(argc, argv, "cartographer_node");
  ::ros::start();
  cartographer_ros::ScopedRosLogSink ros_log_sink;
  cartographer_ros::Run();
......
}

主函数最主要的工作就是启动了一个函数cartographer_ros::Run()

cartographer_ros::Run()

cartographer_ros::Run()和入口函数在同一个文件中

void Run() {
constexpr double kTfBufferCacheTimeInSeconds = 10.;
  tf2_ros::Buffer tf_buffer{::ros::Duration(kTfBufferCacheTimeInSeconds)};
  tf2_ros::TransformListener tf(tf_buffer);

  NodeOptions node_options;
  TrajectoryOptions trajectory_options;

  // 根据配置文件中的内容, 为node_options, trajectory_options 赋值
  std::tie(node_options, trajectory_options) =
      LoadOptions(FLAGS_configuration_directory, FLAGS_configuration_basename);

  // MapBuilder类执行SLAM算法类
  auto map_builder =
      cartographer::mapping::CreateMapBuilder(node_options.map_builder_options);

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

  ::ros::spin();

  // 结束所有处于活动状态的轨迹
  node.FinishAllTrajectories();

  // 当所有的轨迹结束时, 再执行一次全局优化
  node.RunFinalOptimization();
  if (!FLAGS_save_state_filename.empty()) {
    node.SerializeState(FLAGS_save_state_filename,
                        true /* include_unfinished_submaps */);
  }
}

可以看到这个函数先是开启了ros的tf功能包，根据配置文件加载一些配置选项，声明一个真正执行slam算法的类——map_builder（这个类是后面重点，但现在不是），声明一个Node类（现在的重点），这个类开启了一堆ros的服务，上传话题等等。
这里插一个c++的知识就是move函数，就是将一个对象转换成右值引用，这样能避免一些不必要的复制，析构，具体可以阅读《C++ Primer Plus》的第18章。同时也建议看一下Lamda表达式

Node类

这个类在：/src/cartographer_ros/cartographer_ros/cartographer_ros/node.h 和node.cc文件中，先来看它的构造函数，一大段代码：

## 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_);

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

  // Step: 1 声明需要发布的topic这些topic的发布通过定时器发布，见Step: 4
  submap_list_publisher_ =
      node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
          kSubmapListTopic, kLatestOnlyPublisherQueueSize);// 发布SubmapList
  trajectory_node_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kTrajectoryNodeListTopic, kLatestOnlyPublisherQueueSize);  // 发布轨迹
  landmark_poses_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kLandmarkPosesListTopic, kLatestOnlyPublisherQueueSize);  // 发布landmark_pose
  constraint_list_publisher_ =
      node_handle_.advertise<::visualization_msgs::MarkerArray>(
          kConstraintListTopic, kLatestOnlyPublisherQueueSize);  // 发布约束
  if (node_options_.publish_tracked_pose) {
    tracked_pose_publisher_ =
        node_handle_.advertise<::geometry_msgs::PoseStamped>(
            kTrackedPoseTopic, kLatestOnlyPublisherQueueSize); } // 发布tracked_pose, 默认不发布

  // Step: 2 声明发布对应名字的ROS服务, 并将服务的发布器放入到vector容器中
  service_servers_.push_back(node_handle_.advertiseService(
      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));   //通过submap index获取对应的轨迹
  service_servers_.push_back(node_handle_.advertiseService(
      kTrajectoryQueryServiceName, &Node::HandleTrajectoryQuery, this));   //获取对应id的轨迹
  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));
  }
}

可以看到它的构造函数就是发布一些topic，提供一些ros中的服务。后面我们并非去看这话题怎么发布，这些服务怎么提供，而是要去先着重了解这些话题的发布，服务的提供具体由谁（哪些类）提供。这里又要废话一下，视觉slam学的是orbslam2，没学过ros，这里想通过cartographer的学习把激光slam和ros都学了，一举三得（痴人说梦）。那么我们下一篇见。