cartographer探秘第四章之代码解析（七）--- Cartographer_ros

本次阅读的源码为 release-1.0 版本的代码

https://github.com/googlecartographer/cartographer_ros/tree/release-1.0

https://github.com/googlecartographer/cartographer/tree/release-1.0

也可以下载我上传的 全套工作空间的代码，包括 protobuf, cartographer, cartographer_ros, ceres,

https://download.csdn.net/download/tiancailx/11224156，现在上传资源自己不能选下载需要的积分。。。完全靠系统。

本篇文章只简要分析　cartographer_ros 下的代码，cartogrpaher 下的代码请见另一篇文章

cartographer_ros/cartographer_ros/node_main.cc

main()函数调用Run(), 在Run() 首先进行了参数的读取，包括node_options 和 trajectory_options，然后初始化了 MapBuilder 和 Node，然后有条件的分别调用 Node 的如下方法。

  if (!FLAGS_load_state_filename.empty()) {
    node.LoadState(FLAGS_load_state_filename, FLAGS_load_frozen_state);
  }

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

在main()函数中有一个  ros::start();  ，我没找到这个的定义。。。

cartographer_ros/cartographer_ros/node.cc

// Wires up ROS topics to SLAM. 将ROS主题连接到SLAM。

HandleStartTrajectory() --> AddTrajectory()

首先在Node的构造函数中 将 HandleStartTrajectory() 注册到服务中，然后通过服务进行方法的调用。

HandleStartTrajectory()  函数首先检查trajectory options，然后检查topics，都通过了之后调用AddTrajectory()。

int Node::AddTrajectory(const TrajectoryOptions& options,
                        const cartographer_ros_msgs::SensorTopics& topics) {
  const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>
      expected_sensor_ids = ComputeExpectedSensorIds(options, topics);
  const int trajectory_id =
      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);
  AddExtrapolator(trajectory_id, options);
  AddSensorSamplers(trajectory_id, options);
  LaunchSubscribers(options, topics, trajectory_id);
  is_active_trajectory_[trajectory_id] = true;
  for (const auto& sensor_id : expected_sensor_ids) {
    subscribed_topics_.insert(sensor_id.id);
  }
  return trajectory_id;
}

AddTrajectory() 方法 根据配置文档对 extrapolator 和 SensorSample 进行参数配置。然后通过 LaunchSubscribers() 方法将几种传感器的数据处理函数通过boost进行回调函数的注册，即每次相应类型的消息到来，都会进行 HandleImuMessage()等 这几个数据处理函数的调用。

void Node::HandleImuMessage(const int trajectory_id,
                            const std::string& sensor_id,
                            const sensor_msgs::Imu::ConstPtr& msg) {
  carto::common::MutexLocker lock(&mutex_);
  if (!sensor_samplers_.at(trajectory_id).imu_sampler.Pulse()) {
    return;
  }
  auto sensor_bridge_ptr = map_builder_bridge_.sensor_bridge(trajectory_id);
  auto imu_data_ptr = sensor_bridge_ptr->ToImuData(msg);
  if (imu_data_ptr != nullptr) {
    extrapolators_.at(trajectory_id).AddImuData(*imu_data_ptr);
  }
  sensor_bridge_ptr->HandleImuMessage(sensor_id, msg);
}

可以看到，如果存在IMU数据，就将IMU数据 输入 到 extrapolator 中，如果有 Odom 数据，也将 odom 数据 输入到 extrapolator 中。

IMU数据类型的处理是通过 map_builder_bridge_.sensor_bridge(trajectory_id)->HandleImuMessage(sensor_id, msg); 进行的。也就是转到了 map_builder_bridge.h ，与 sensor_bridge.h  ,  map_build 是 进行地图构建的过程，sensor_bridge 是 转换数据类型以及坐标变换。

cartographer_ros/cartographer_ros/sensor_bridge.h

Converts ROS messages into SensorData in tracking frame for the MapBuilder.

ROS消息转换为 MapBuilder 的 tracking frame 下的 SensorData

// 将 里程计数据 传入 carto::sensor::OdometryData, 返回指向 carto::sensor::OdometryData 的智能指针
std::unique_ptr<carto::sensor::OdometryData> SensorBridge::ToOdometryData(
    const nav_msgs::Odometry::ConstPtr& msg) {
  const carto::common::Time time = FromRos(msg->header.stamp);
  const auto sensor_to_tracking = tf_bridge_.LookupToTracking(
      time, CheckNoLeadingSlash(msg->child_frame_id));
  if (sensor_to_tracking == nullptr) {
    return nullptr;
  }
  return carto::common::make_unique<carto::sensor::OdometryData>(
      carto::sensor::OdometryData{
          time, ToRigid3d(msg->pose.pose) * sensor_to_tracking->inverse()});
}

通过 ToOdometryData() ToImuData() 将 ROS 下的消息类型转换为 Cartographer 的数据类型。并将 odom 的数据 通过 乘以一个 坐标变化矩阵 转换到 tracking frame 坐标系下。

laserscan, MultiEchoLaserScanMessage 的数据处理是调用 HandleLaserScan() -->  HandleRangefinder()，PointCloud2Message 的数据处理是直接调用 HandleRangefinder(), 代码如下

void SensorBridge::HandleLaserScan() {...}

void SensorBridge::HandleRangefinder(
    const std::string& sensor_id, const carto::common::Time time,
    const std::string& frame_id, const carto::sensor::TimedPointCloud& ranges) {
  const auto sensor_to_tracking =
      tf_bridge_.LookupToTracking(time, CheckNoLeadingSlash(frame_id));
  if (sensor_to_tracking != nullptr) {
    trajectory_builder_->AddSensorData(
        sensor_id, carto::sensor::TimedPointCloudData{
                       time, sensor_to_tracking->translation().cast<float>(),
                       carto::sensor::TransformTimedPointCloud(
                           ranges, sensor_to_tracking->cast<float>())});
  }
}

所有的传感器数据 包括 odom imu nav_fix landmark laser multiEchoLaser pointcloud2 的 处理都是先通过  cartographer_ros/msg_conversion.h 先将ROS 下的消息类型转换成 Carto 的消息类型，PointCloud2 的消息类型是直接应用 PCL 的　pcl::fromROSMsg() 转换的，在传入 caro 的数据类型里。

然后在将数据 传入到 cartographer::mapping::TrajectoryBuilderInterface 的 AddSensorData() 的重载函数中。

cartographer/mapping/trajectory_builder_interface.h　

// This interface is used for both 2D and 3D SLAM. Implementations wire up a
// global SLAM stack, i.e. local SLAM for initial pose estimates, scan matching
// to detect loop closure, and a sparse pose graph optimization to compute
// optimized pose estimates.

用于2D和3D SLAM。 实现了整体的SLAM过程，即用于　初始姿势估计的local SLAM，用于检测循环闭合的扫描匹配，以及用于计算优化姿势估计的稀疏姿势图优化。

cartographer_ros/cartographer_ros/map_builder_bridge.h

node.cc 调用了如下方法，也就是进入到了 sensor_bridge.h 文件。

SensorBridge* MapBuilderBridge::sensor_bridge(const int trajectory_id) {
  return sensor_bridges_.at(trajectory_id).get();
}

首先，在头文件中定义了 TrajectoryState 结构体。内置结构体　LocalSlamData　包含了匹配之后的坐标变换 local_pose，以及估计 local_pose 的时刻，以及 RangeData 数据。之后　TrajectoryState 结构体　还包含　local - map 的　坐标变换，local - tracking 的坐标变换（为什么用指针？？？）, 以及　trajectory_options.

总结，TrajectoryState　就是进行 SLAM 之后得到的　位姿坐标，以及相对于 map 和 tracking frame 的坐标变换。

  struct TrajectoryState {
    // Contains the trajectory state data received from local SLAM, after
    // it had processed accumulated 'range_data_in_local' and estimated
    // current 'local_pose' at 'time'.
    struct LocalSlamData {
      ::cartographer::common::Time time;
      ::cartographer::transform::Rigid3d local_pose;
      ::cartographer::sensor::RangeData range_data_in_local;
    };
    std::shared_ptr<const LocalSlamData> local_slam_data;
    cartographer::transform::Rigid3d local_to_map;
    std::unique_ptr<cartographer::transform::Rigid3d> published_to_tracking;
    TrajectoryOptions trajectory_options;
  };

RangeData 包含了一帧雷达数据的　起点坐标，hit障碍物的点　returns，以及 没有扫到障碍物的点　misses ，并将 misses 用一个配置的距离代替。

// cartographer/sensor/range_data.h

// Rays begin at 'origin'. 'returns' are the points where obstructions were
// detected. 'misses' are points in the direction of rays for which no return
// was detected, and were inserted at a configured distance. It is assumed that
// between the 'origin' and 'misses' is free space.
struct RangeData {
  Eigen::Vector3f origin;
  PointCloud returns;
  PointCloud misses;
};

node.cc 的 AddTrajectory()  -->  map_build_bridge.h 的 AddTrajectory() -->  map_builder_interface.h 的　AddTrajectoryBuilder() , 并且将 OnLocalSlamResult() 方法通过回调函数的形式注册进去。

并且对sensor_bridges_ 进行了第0个 trajectory_id 的初始化，并将　TrajectoryBuilder　传进去，再调用　 cartographer::mapping::TrajectoryBuilderInterface 的 AddSensorData()　方法进行传感器数据的处理。

int MapBuilderBridge::AddTrajectory(
    const std::set<cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
        expected_sensor_ids,
    const TrajectoryOptions& trajectory_options) {
  const int trajectory_id = map_builder_->AddTrajectoryBuilder(
      expected_sensor_ids, trajectory_options.trajectory_builder_options,
      ::std::bind(&MapBuilderBridge::OnLocalSlamResult, this,
                  ::std::placeholders::_1, ::std::placeholders::_2,
                  ::std::placeholders::_3, ::std::placeholders::_4,
                  ::std::placeholders::_5));
  LOG(INFO) << "Added trajectory with ID '" << trajectory_id << "'.";

  // Make sure there is no trajectory with 'trajectory_id' yet.
  CHECK_EQ(sensor_bridges_.count(trajectory_id), 0);
  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);
  CHECK(emplace_result.second == true);
  return trajectory_id;
}

OnLocalSlamResult()

void MapBuilderBridge::OnLocalSlamResult(
    const int trajectory_id, const ::cartographer::common::Time time,
    const Rigid3d local_pose,
    ::cartographer::sensor::RangeData range_data_in_local,
    const std::unique_ptr<const ::cartographer::mapping::
                              TrajectoryBuilderInterface::InsertionResult>) {
  std::shared_ptr<const TrajectoryState::LocalSlamData> local_slam_data =
      std::make_shared<TrajectoryState::LocalSlamData>(
          TrajectoryState::LocalSlamData{time, local_pose,
                                         std::move(range_data_in_local)});
  cartographer::common::MutexLocker lock(&mutex_);
  trajectory_state_data_[trajectory_id] = std::move(local_slam_data);
}

OnLocalSlamResult() 这个方法大致就是确定当前的　localSlamData ，然后传入到　TrajectoryState 集合中。

OnLocalSlamResult()  的使用如下所示：( 具体的处理函数还没找到)

// cartographer/cartographer/mapping/trajectory_builder_interface.h
  #include<functional>
  using LocalSlamResultCallback =
      std::function<void(int /* trajectory ID */, common::Time,
                         transform::Rigid3d /* local pose estimate */,
                         sensor::RangeData /* in local frame */,
                         std::unique_ptr<const InsertionResult>)>;

// cartographer/cartographer/mapping/map_builder_interface.h
  using LocalSlamResultCallback =
      TrajectoryBuilderInterface::LocalSlamResultCallback;

  // Creates a new trajectory builder and returns its index.
  virtual int AddTrajectoryBuilder(
      const std::set<SensorId>& expected_sensor_ids,
      const proto::TrajectoryBuilderOptions& trajectory_options,
      LocalSlamResultCallback local_slam_result_callback) = 0;

// cartographer/cartographer/mapping/map_builder.h
  int AddTrajectoryBuilder(
      const std::set<SensorId> &expected_sensor_ids,
      const proto::TrajectoryBuilderOptions &trajectory_options,
      LocalSlamResultCallback local_slam_result_callback) override;

// cartographer/cartographer/mapping/map_builder.cc
int MapBuilder::AddTrajectoryBuilder(
    const std::set<SensorId>& expected_sensor_ids,
    const proto::TrajectoryBuilderOptions& trajectory_options,
    LocalSlamResultCallback local_slam_result_callback) {
    // ...
    trajectory_builders_.push_back(
        common::make_unique<CollatedTrajectoryBuilder>(
            sensor_collator_.get(), trajectory_id, expected_sensor_ids,
            CreateGlobalTrajectoryBuilder3D(
                std::move(local_trajectory_builder), trajectory_id,
                static_cast<PoseGraph3D*>(pose_graph_.get()),
                local_slam_result_callback)));
}

下边３个方法读取　map_builder_->pose_graph() 进行 rviz 的可视化。

visualization_msgs::MarkerArray MapBuilderBridge::GetTrajectoryNodeList() {

visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {

visualization_msgs::MarkerArray MapBuilderBridge::GetConstraintList() {

剩下的方法都是调用　map_build.h中的方法，起到个　桥梁的作用。