cartographer（1） 程序入口

程序的入口：cartographer_ros/cartographer_ros/cartographer_ros/node_main.cc 文件中

在主函数中创建节点：cartographer_node

并开始：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);//监听tf-tree
  NodeOptions node_options;//节点配置选项
  TrajectoryOptions trajectory_options;//轨迹的配置选项

  std::string&& directory = GetSlamConfigPath();//配置路径
  std::string basename = std::string("cotek_fork_localization.lua");//配置文件名

  std::tie(node_options, trajectory_options) = LoadOptions(directory, basename);

  auto map_builder = absl::make_unique<cartographer::mapping::MapBuilder>(
      node_options.map_builder_options);//地图构建器

  Node node(node_options, std::move(map_builder), &tf_buffer, false);
   if (!FLAGS_load_state_filename.empty()) {// 如果加载了pbstream文件, 就执行这个函数
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
   }
   if ("true" == FLAGS_start_trajectory_with_default_topics) {
     node.StartTrajectoryWithDefaultTopics(trajectory_options);
   } 使用默认topic 开始轨迹

  ::ros::spin();

  node.FinishAllTrajectories();// 结束所有处于活动状态的轨迹
  node.RunFinalOptimization(); // 当所有的轨迹结束时, 再执行一次全局优化

    if (!FLAGS_save_state_filename.empty()) {
      node.SerializeState(FLAGS_save_state_filename,
                          true /* include_unfinished_submaps */);
    }
}

 node 相关

  // 结束所有轨迹
  void FinishAllTrajectories();
  
  // 通过ID结束轨迹
  bool FinishTrajectory(int trajectory_id);

  //跑最后的优化
  void RunFinalOptimization();

  // 通过topic开始轨迹
  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);

 

  // Odometry数据
  void HandleOdometryMessage(int trajectory_id, const std::string& sensor_id,
                             const nav_msgs::Odometry::ConstPtr& msg);
//gps数据
  void HandleNavSatFixMessage(int trajectory_id, const std::string& sensor_id,
                              const sensor_msgs::NavSatFix::ConstPtr& msg);
//地标
  void HandleLandmarkMessage(int trajectory_id, const std::string& sensor_id,
                             const cotek_msgs::LandmarkList::ConstPtr& msg);
//IMU
  void HandleImuMessage(int trajectory_id, const std::string& sensor_id,
                        const sensor_msgs::Imu::ConstPtr& msg);
//普通的激光雷达
  void HandleLaserScanMessage(int trajectory_id, const std::string& sensor_id,
                              const sensor_msgs::LaserScan::ConstPtr& msg);
//多回波激光雷达
  void HandleMultiEchoLaserScanMessage(
      int trajectory_id, const std::string& sensor_id,
      const sensor_msgs::MultiEchoLaserScan::ConstPtr& msg);
//点云数据
  void HandlePointCloud2Message(int trajectory_id, const std::string& sensor_id,
                                const sensor_msgs::PointCloud2::ConstPtr& msg);
  // 节点状态序列化
  void SerializeState(const std::string& filename,
                      const bool include_unfinished_submaps);

 

 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) {
  absl::MutexLock lock(&mutex_); // 将mutex_上锁, 防止在初始化时数据被更改
  if (collect_metrics) {
    metrics_registry_ = absl::make_unique<metrics::FamilyFactory>();
    carto::metrics::RegisterAllMetrics(metrics_registry_.get());
  }

  submap_list_publisher_ =
      node_handle_.advertise<::cartographer_ros_msgs::SubmapList>(
          kSubmapListTopic, kLatestOnlyPublisherQueueSize);// 发布SubmapList

  slam_deviation_publisher_ =
      node_handle_.advertise<::cotek_msgs::slam_pose_deviation>(
          cotek_topic::kSlamPoseDeviationTopic, kLatestOnlyPublisherQueueSize);

  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);// 发布约束
  service_servers_.push_back(node_handle_.advertiseService(
      kSubmapQueryServiceName, &Node::HandleSubmapQuery, this));//通过submap index获取对应的轨迹
  service_servers_.push_back(node_handle_.advertiseService(
      kStartTrajectoryServiceName, &Node::HandleStartTrajectory, this)); //获取对应id的轨迹
  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));
  service_servers_.push_back(node_handle_.advertiseService(
      kDeleteTrajectoryServiceName, &Node::HandleDeleteTrajectory, this));//通过服务来删除轨迹

  service_servers_.push_back(node_handle_.advertiseService(
      kReLoadMapbuilderServiceName, &Node::HandleReLoadMapbuilder, this));

  service_servers_.push_back(node_handle_.advertiseService(
      kSaveCartoMapServiceName, &Node::HandleSaveCartoMap, this));

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

  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),
      &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),
        &Node::PublishLocalTrajectoryData, this);//发布局部轨迹
  }
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
      &Node::PublishTrajectoryNodeList, this));//发布轨迹节点定时器
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.trajectory_publish_period_sec),
      &Node::PublishLandmarkPosesList, this));//发布landmark定时器
  wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(kConstraintPublishPeriodSec),
      &Node::PublishConstraintList, this));//发布约束的定时器
}