cartographer_ros 代码分析1

cartographer ROS代码为两大部分：cartographer和cartographer_ros。
cartographer主要负责处理来自雷达、IMU、里程计的数据并基于这些数据进行地图的构建，是cartographer理论的底层实现。
cartographer_ros 则基于ros的通信机制获取传感器的数据并将它们转换成cartographer中定义的格式传递给cartographer处理，与此同时也将cartographer的处理结果发布用于显示或保存，是基于cartographer的上层应用

1. node_main.cc 中

main(int argc, char** argv)
    cartographer_ros::Run();
        1. LoadOptions 加载配置文件
        2. Node node(node_options, &tf_buffer);
        //启动轨迹
        3.node.StartTrajectoryWithDefaultTopics(trajectory_options);

 2. node.cc 中

 //构造函数
 Node::Node(const NodeOptions& node_options, tf2_ros::Buffer* const tf_buffer): node_options_(node_options),
            map_builder_bridge_(node_options_, tf_buffer)
{
    //submap发布器，往kSubmapListTopic 发布
    //::cartographer_ros_msgs::SubmapList 消息
    submap_list_publisher_
    
    //路径发布器，向kTrajectoryNodeListTopic这个Topic上发布了
    //一个::visualization_msgs::MarkerArray型的message
    trajectory_node_list_publisher_
    
    //请求子图submap服务
    kSubmapQueryServiceName
    
    //定时器，发布获取子图
      wall_timers_.push_back(node_handle_.createWallTimer(
      ::ros::WallDuration(node_options_.submap_publish_period_sec),
      &Node::PublishSubmapList, this));
    
}

 

//启动轨迹
StartTrajectoryWithDefaultTopics
{
    AddTrajectory(options, DefaultSensorTopics());
}  

 

int Node::AddTrajectory(const TrajectoryOptions& options,
             const cartographer_ros_msgs::SensorTopics& topics)
{
    //调用map_builder_bridge_中的AddTrajectory来处理,再次会调用到
    //map_builder_.AddTrajectoryBuilder,最终调用到cartographer底层算法的
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
    
    //插入一个位姿解析器
  AddExtrapolator(trajectory_id, options);
  AddSensorSamplers(trajectory_id, options);  
    //用来订阅必要的Topic以接收数据的LaunchSubscribers
  LaunchSubscribers(options, topics, trajectory_id);
}  

其中根据map_builder_bridge_.AddTrajectory函数中追踪到map_builder_bridge.cc

int MapBuilderBridge::AddTrajectory(
    const std::unordered_set<std::string>& expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  /*调用cartographer 底层的mapping/map_builder.cc 来处理
  */
  const int trajectory_id = map_builder_.AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options);


  sensor_bridges_[trajectory_id] =
      cartographer::common::make_unique<SensorBridge>(
          trajectory_options.num_subdivisions_per_laser_scan,
          trajectory_options.tracking_frame,
          node_options_.lookup_transform_timeout_sec, tf_buffer_,
          map_builder_.GetTrajectoryBuilder(trajectory_id));
          
  auto emplace_result =
      trajectory_options_.emplace(trajectory_id, trajectory_options);

  return trajectory_id;
}

此时map_builder_.AddTrajectoryBuilder 调用cartographer 底层的mapping/map_builder.cc 来处理,如下:
 

构造函数:
MapBuilder::MapBuilder(
    const proto::MapBuilderOptions& options,
    const LocalSlamResultCallback& local_slam_result_callback)
    : options_(options),
      thread_pool_(options.num_background_threads()),
      local_slam_result_callback_(local_slam_result_callback) {
  if (options.use_trajectory_builder_2d()) {
    pose_graph_2d_ = common::make_unique<mapping_2d::PoseGraph>(
        options_.pose_graph_options(), &thread_pool_);
    pose_graph_ = pose_graph_2d_.get();
  }

}

int MapBuilder::AddTrajectoryBuilder(
    const std::unordered_set<std::string>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options) {
  const int trajectory_id = trajectory_builders_.size();

    trajectory_builders_.push_back(
        common::make_unique<CollatedTrajectoryBuilder>(
            &sensor_collator_, trajectory_id, expected_sensor_ids,
            common::make_unique<mapping::GlobalTrajectoryBuilder<
                mapping_2d::LocalTrajectoryBuilder,
                mapping_2d::proto::LocalTrajectoryBuilderOptions,
                mapping_2d::PoseGraph>>(
                trajectory_options.trajectory_builder_2d_options(),
                trajectory_id, pose_graph_2d_.get(),
                local_slam_result_callback_)));

  if (trajectory_options.pure_localization()) {
    constexpr int kSubmapsToKeep = 3;
    pose_graph_->AddTrimmer(common::make_unique<PureLocalizationTrimmer>(
        trajectory_id, kSubmapsToKeep));
  }
  if (trajectory_options.has_initial_trajectory_pose()) {
    const auto& initial_trajectory_pose =
        trajectory_options.initial_trajectory_pose();
    pose_graph_->SetInitialTrajectoryPose(
        trajectory_id, initial_trajectory_pose.to_trajectory_id(),
        transform::ToRigid3(initial_trajectory_pose.relative_pose()),
        common::FromUniversal(initial_trajectory_pose.timestamp()));
  }
  return trajectory_id;
}

其二，根据LaunchSubscribers来处理传感器数据，如下:

void Node::LaunchSubscribers(const TrajectoryOptions& options,
                         const cartographer_ros_msgs::SensorTopics& topics,
                             const int trajectory_id)
{

    //订阅激光数据，并回调处理
    HandleLaserScanMessage
    
    //2.根据配置，订阅imu数据
    HandleImuMessage
    
    //3.根据配置，订阅里程计数据并处理
    HandleOdometryMessage
}   

 

//以处理激光数据为例子
void Node::HandleLaserScanMessage(const int trajectory_id,
                              const std::string& sensor_id,
                              const sensor_msgs::LaserScan::ConstPtr& msg)
{
  //处理激光数据
  map_builder_bridge_.sensor_bridge(trajectory_id)
      ->HandleLaserScanMessage(sensor_id, msg);
}

//调用map_builder_bridge_类成员指针sensor_bridge 的函数来处理激光数据
在sensor_bridge.cc 中
void SensorBridge::HandleLaserScanMessage(
    const std::string& sensor_id, const sensor_msgs::LaserScan::ConstPtr& msg) {
    //
  HandleLaserScan(sensor_id, FromRos(msg->header.stamp), msg->header.frame_id,
                  ToPointCloudWithIntensities(*msg));
}

void SensorBridge::HandleLaserScan
{
    HandleRangefinder(sensor_id, subdivision_time, frame_id, subdivision);
}

void SensorBridge::HandleRangefinder()
{
    trajectory_builder_->AddRangefinderData(
        sensor_id, time, sensor_to_tracking->translation().cast<float>(),
        carto::sensor::TransformTimedPointCloud(
            ranges, sensor_to_tracking->cast<float>()));
}

在此需要根据::cartographer::mapping::TrajectoryBuilderInterface* const  trajectory_builder_

指针转到底层cartographer/mapping/trajectory_builder_interface.cc 中，AddRangefinderData 函数为虚函数，因此要寻找该函数的实例化实现

在trajectory_builder.h中

  void AddRangefinderData(const std::string& sensor_id, common::Time time,
                          const Eigen::Vector3f& origin,
                          const sensor::TimedPointCloud& ranges) {
    AddSensorData(sensor_id,
                  common::make_unique<sensor::DispatchableRangefinderData>(
                      time, origin, ranges));
  }

其中AddSensorData 是虚函数

总结: 从cartographer_ros 到cartographer 底层算法，主要有如下入口

第一:MapBuilderInterface 入口

StartTrajectoryWithDefaultTopics ->AddTrajectory ->map_builder_bridge_.AddTrajectory -> map_builder_.AddTrajectoryBuilder

此时通过map_builder_进入cartographer::mapping::MapBuilderInterface接口，这个接口在/src/cartographer/cartographer/mapping/map_builder
_interface.h中定义，并由/src/cartographer/cartographer/mapping/map_builder.h和map_builder.cc实现，所以看到这里可以知道MapBuilderInterface是cartographer的入口之一

第二:激光雷达，imu，odom 等传感器数据入口

StartTrajectoryWithDefaultTopics ->AddTrajectory ->LaunchSubscribers -> HandleLaserScanMessage,HandleImuMessage,HandleOdometryMessage

以激光雷达数据处理为例子

HandleLaserScanMessage ->

map_builder_bridge_.sensor_bridge(trajectory_id)->HandleLaserScanMessage ->

 在sensor_bridge.cc 中HandleLaserScanMessage  -> SensorBridge::HandleLaserScan -> SensorBridge::HandleRangefinder

-> trajectory_builder_->AddRangefinderData

在此需要根据::cartographer::mapping::TrajectoryBuilderInterface* const  trajectory_builder_

指针转到底层cartographer/mapping/trajectory_builder_interface.cc 中，AddRangefinderData 函数为虚函数，因此要寻找该函数的实例化实现