cartographer_ros node （一）：构造函数

本次的主要目的是讲解cartographer_ros中的node.cc函数中的Node类的构造函数。前面我们已经说过Node类是cartographer和cartographer_ros之间进行信息传递的主要节点，也是真正的SLAM算法与ROS封装之间的桥梁。具体的代码如下：

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

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

其中主要做了以下几件事：

• 声明消息的发布者
• 声明服务的发布者
• 声明扫描匹配后的点云的发布者
• 将定时器与对应的时间驱动函数进行绑定
  这里面的所有消息和服务名称都定义在node_constants.h这个文件中

constexpr char kLaserScanTopic[] = "scan";                                  // 扫描消息
constexpr char kMultiEchoLaserScanTopic[] = "echoes";                       // 多回声波雷达消息
constexpr char kPointCloud2Topic[] = "points2";                             // 点云消息
constexpr char kImuTopic[] = "imu";                                         // imu消息
constexpr char kOdometryTopic[] = "odom";                                   // 里程计消息
constexpr char kNavSatFixTopic[] = "fix";                                   // gps消息
constexpr char kLandmarkTopic[] = "landmark";                               // 路标消息
constexpr char kFinishTrajectoryServiceName[] = "finish_trajectory";        // 结束轨迹的服务
constexpr char kOccupancyGridTopic[] = "map";                               // 占据栅格地图消息
constexpr char kScanMatchedPointCloudTopic[] = "scan_matched_points2";      // 扫描匹配后的点云消息
constexpr char kSubmapListTopic[] = "submap_list";                          // 子图
constexpr char kTrackedPoseTopic[] = "tracked_pose";                        // 跟踪的位姿
constexpr char kSubmapQueryServiceName[] = "submap_query";                  // 子图查询的服务
constexpr char kTrajectoryQueryServiceName[] = "trajectory_query";          // 轨迹查询的服务
constexpr char kStartTrajectoryServiceName[] = "start_trajectory";          // 开始轨迹的服务
constexpr char kWriteStateServiceName[] = "write_state";                    // 写入状态的服务
constexpr char kGetTrajectoryStatesServiceName[] = "get_trajectory_states"; // 获取轨迹状态的服务
constexpr char kReadMetricsServiceName[] = "read_metrics";                  // 读取度量的服务？
constexpr char kTrajectoryNodeListTopic[] = "trajectory_node_list";         // 轨迹节点消息
constexpr char kLandmarkPosesListTopic[] = "landmark_poses_list";           // 路标位姿消息
constexpr char kConstraintListTopic[] = "constraint_list";                  // 约束消息

这里需要注意的是，cartographer_ros内部的消息名称在这里已经确定了，我们要想使用自己的消息名称则需要利用ros提供的remap功能