Cartographer学习记录：cartographer源码3D SLAM部分（二）

原创

已于 2022-03-01 21:51:55 修改 · 545 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#学习 #3d #自动驾驶

于 2022-02-28 20:05:05 首次发布

本文详细分析了Cartographer ROS节点的`node.h`和`node.cc`文件，展示了节点如何作为中间媒介处理ROS话题、服务和传感器数据，进行SLAM轨迹更新、子图绘制、跟踪位姿发布等功能。节点构造时，通过lua配置创建MapBuilder，并处理ROS的订阅、服务请求。节点还包括处理不同传感器数据的回调函数，如里程计、GPS、地标和IMU数据。此外，节点还负责发布子图、轨迹节点、地标位姿和约束信息。文章还探讨了话题订阅、服务处理和定时器绑定的实现。通过对代码的解析，揭示了Cartographer ROS节点在构建3D和2D地图中的核心作用。

在阅读完cartographer_ros的主函数文件（参考上一篇）后，我们了解到，在主函数中其实最终重要的是进行node类对象的构造，读取配置node_options和创立map_builder都是为了传入node类的构造函数进行构造。所以本节内容将从node.h和node.cc文件进行突破。

一、node.h文件

个人看具体类代码的习惯是先统览类包含哪些变量和成员函数，然后再看具体实现方法。一个成熟的工程项目往往会将不同功能函数的声明与实现进行区分，前者通常为.h，后者通常为.cc文件。

// cartographer_ros命名空间
namespace cartographer_ros {

// node类的主要功用是作为中间介质将ROS的各类topic信息传入底层cartographer算法进行建图定位
class Node {
 public:
  // 构造函数，也就是前面node_main.cc文件里面调用的，参数分别为lua配置、指向MapBuilderInterface类的智能指针、tf监听参数、决定运行时度量集合是否开启的参数
  Node(const NodeOptions& node_options,
       std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder,
       tf2_ros::Buffer* tf_buffer, bool collect_metrics);
  ~Node();

  // 补充——c++11: =delete: 禁止编译器自动生成默认函数; =default: 要求编译器生成一个默认函数

  // 禁止编译器自动生成默认拷贝构造函数(复制构造函数)
  Node(const Node&) = delete;
  // 禁止编译器自动生成默认赋值函数
  Node& operator=(const Node&) = delete;

  // 结束所有处于活动状态的轨迹
  void FinishAllTrajectories();
  // 结束指定编号的轨迹，若是成功则返回true，否则返回false
  bool FinishTrajectory(int trajectory_id);

  // 运行最后的全局优化，在调用此函数时确保所有路径都处于非活动状态
  void RunFinalOptimization();

  // 以默认的话题信息开始进行轨迹的更新
  void StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options);

  // 根据各自的配置参数信息从多个输入数据包中分离出各自的ROS topic数据，以容器的形式返回
  std::vector<
      std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>>
  ComputeDefaultSensorIdsForMultipleBags(
      const std::vector<TrajectoryOptions>& bags_options) const;

  // Adds a trajectory for offline processing, i.e. not listening to topics.
  // 离线状态（采用数据包的形式）下的轨迹更新
  int AddOfflineTrajectory(
      const std::set<
          cartographer::mapping::TrajectoryBuilderInterface::SensorId>&
          expected_sensor_ids,
      const TrajectoryOptions& options);


  // 下面各类函数就是监听到各自的传感器topic数据，分别采用的不同函数进行处理，处理后加入到轨迹中

  // 处理里程计信息
  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 cartographer_ros_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);

  // 从pbstream文件中加载SLAM状态
  void LoadState(const std::string& state_filename, bool load_frozen_state);

  // ROS的句柄
  ::ros::NodeHandle* node_handle();

 private:
  // 自定义Subscriber结构体
  struct Subscriber {
    // ROS的订阅者
    ::ros::Subscriber subscriber;

    // ROS的topic名
    std::string topic;
  };
  
  // 下面的函数是上层cartographer_ros启用的各类服务处理函数，调用相应服务会进入对应函数进行处理
  
  // 子图查阅服务
  bool HandleSubmapQuery(
      cartographer_ros_msgs::SubmapQuery::Request& request,
      cartographer_ros_msgs::SubmapQuery::Response& response);
  // 轨迹查询服务
  bool HandleTrajectoryQuery(
      ::cartographer_ros_msgs::TrajectoryQuery::Request& request,
      ::cartographer_ros_msgs::TrajectoryQuery::Response& response);
  // 开始更新轨迹服务（此类服务是开始建图的信号，由终端输入rosservice call后跟对应参数开启）
  bool HandleStartTrajectory(
      cartographer_ros_msgs::StartTrajectory::Request& request,
      cartographer_ros_msgs::StartTrajectory::Response& response);
  // 结束更新轨迹
  bool HandleFinishTrajectory(
      cartographer_ros_msgs::FinishTrajectory::Request& request,
      cartographer_ros_msgs::FinishTrajectory::Response& response);
  // 保存对应状态信息
  bool HandleWriteState(cartographer_ros_msgs::WriteState::Request& request,
                        cartographer_ros_msgs::WriteState::Response& response);
  // 获取轨迹状态信息
  bool HandleGetTrajectoryStates(
      ::cartographer_ros_msgs::GetTrajectoryStates::Request& request,
      ::cartographer_ros_msgs::GetTrajectoryStates::Response& response);
  // 获取度量信息（暂时还未弄明白这个度量是啥意思，后续更新吧。。。）
  bool HandleReadMetrics(
      cartographer_ros_msgs::ReadMetrics::Request& request,
      cartographer_ros_msgs::ReadMetrics::Response& response);

  // 根据配置参数信息从单输入数据包中分离出ROS topic信息，以集合的形式返回
  std::set<::cartographer::mapping::TrajectoryBuilderInterface::SensorId>
  ComputeExpectedSensorIds(const TrajectoryOptions& options) const;
  // 增加路径
  int AddTrajectory(const TrajectoryOptions& options);
  // 各类话题的订阅
  void LaunchSubscribers(const TrajectoryOptions& options, int trajectory_id);
  // 发布子图
  void PublishSubmapList(const ::ros::WallTimerEvent& timer_event);
  // 位姿估计相关函数
  void AddExtrapolator(int trajectory_id, const TrajectoryOptions& options);
  // 传感器采样
  void AddSensorSamplers(int trajectory_id, const TrajectoryOptions& options);
  // 局部建图信息
  void PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event);
  // 发布路径点信息
  void PublishTrajectoryNodeList(const ::ros::WallTimerEvent& timer_event);
  // 发布地标位姿信息
  void PublishLandmarkPosesList(const ::ros::WallTimerEvent& timer_event);
  // 发布节点间约束信息
  void PublishConstraintList(const ::ros::WallTimerEvent& timer_event);
  // 判断配置是否符合规范
  bool ValidateTrajectoryOptions(const TrajectoryOptions& options);
  // 判断topic命名是否符合规范
  bool ValidateTopicNames(const TrajectoryOptions& options);
  cartographer_ros_msgs::StatusResponse FinishTrajectoryUnderLock(
      int trajectory_id) EXCLUSIVE_LOCKS_REQUIRED(mutex_);
  void MaybeWarnAboutTopicMismatch(const ::ros::WallTimerEvent&);

  // 发布点云地图
  void PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event);

  // 查询id对应轨迹状态函数
  cartographer_ros_msgs::StatusResponse TrajectoryStateToStatus(
      int trajectory_id,
      const std::set<
          cartographer::mapping::PoseGraphInterface::TrajectoryState>&
          valid_states);
  // 节点配置
  const NodeOptions node_options_;

  tf2_ros::TransformBroadcaster tf_broadcaster_;

  absl::Mutex mutex_;
  std::unique_ptr<cartographer_ros::metrics::FamilyFactory> metrics_registry_;

  // c++11: GUARDED_BY是在Clang Thread Safety Analysis（线程安全分析）中定义的属性
  // GUARDED_BY是数据成员的属性, 该属性声明数据成员受给定功能保护.
  // 对数据的读操作需要共享访问, 而写操作则需要互斥访问.
  MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

  ::ros::NodeHandle node_handle_;
  // 各类topic信息发布者
  ::ros::Publisher submap_list_publisher_;
  ::ros::Publisher trajectory_node_list_publisher_;
  ::ros::Publisher landmark_poses_list_publisher_;
  ::ros::Publisher constraint_list_publisher_;
  ::ros::Publisher tracked_pose_publisher_;
  ::ros::Publisher scan_matched_point_cloud_publisher_;
  // 储存各类服务
  std::vector<::ros::ServiceServer> service_servers_;
  
  
  // 控制各个传感器数据的采样频率
  struct TrajectorySensorSamplers {
    TrajectorySensorSamplers(const double rangefinder_sampling_ratio,
                             const double odometry_sampling_ratio,
                             const double fixed_frame_pose_sampling_ratio,
                             const double imu_sampling_ratio,
                             const double landmark_sampling_ratio)
        : rangefinder_sampler(rangefinder_sampling_ratio),
          odometry_sampler(odometry_sampling_ratio),
          fixed_frame_pose_sampler(fixed_frame_pose_sampling_ratio),
          imu_sampler(imu_sampling_ratio),
          landmark_sampler(landmark_sampling_ratio) {}

    ::cartographer::common::FixedRatioSampler rangefinder_sampler;
    ::cartographer::common::FixedRatioSampler odometry_sampler;
    ::cartographer::common::FixedRatioSampler fixed_frame_pose_sampler;
    ::cartographer::common::FixedRatioSampler imu_sampler;
    ::cartographer::common::FixedRatioSampler landmark_sampler;
  };

  // 根据对应id查找相应信息的各类容器
  std::map<int, ::cartographer::mapping::PoseExtrapolator> extrapolators_;
  std::map<int, ::ros::Time> last_published_tf_stamps_;
  std::unordered_map<int, TrajectorySensorSamplers> sensor_samplers_;
  std::unordered_map<int, std::vector<Subscriber>> subscribers_;
  std::unordered_set<std::string> subscribed_topics_;
  std::unordered_set<int> trajectories_scheduled_for_finish_;

  std::vector<::ros::WallTimer> wall_timers_;
  ::ros::Timer publish_local_trajectory_data_timer_;

  ::ros::Publisher point_cloud_map_publisher_;
  absl::Mutex point_cloud_map_mutex_;
  bool load_state_ = false;
  size_t last_trajectory_nodes_size_ = 0;
  sensor_msgs::PointCloud2 ros_point_cloud_map_;
};

}  // namespace cartographer_ros

上面是node.h的全部内容，部分代码的含义已有相应的注释，针对node_main.cc里面调用的对应构造函数可能有人会问，为什么是指向MapBuilderInterface类的智能指针，而不是前面MapBuilder类呢？这里提前说明下，在底层的cartographer中，MapBuilderInterface属于MapBuilder的父类，MapBuilder继承自MapBuilderInterface，如下所示为map_builder.h文件内容：

// Wires up the complete SLAM stack with TrajectoryBuilders (for local submaps)
// and a PoseGraph for loop closure.
class MapBuilder : public MapBuilderInterface

二、node.cc文件

在看完node.h文件后，我们可以先对node类的作用有个笼统性的认识，主要功用包括：（1）提供各类服务；（2）接收传感器数据并进行处理；（3）进行SLAM轨迹更新、子图绘制、跟踪位姿、节点间约束关系并将其作为话题发布。下面是注释版node.cc文件，有需要可自行复制。然后我将挑一些重要的函数进行单独讲解，有不对的地方还望多多指正。

因为原来的node.cc文件顺序不是太符合个人习惯，所以下面我根据个人意见将其中函数按照功能简单的进行归类注释。

1.非类成员函数

namespace {
// 利用ROS句柄为指定id的路径订阅相应topic名称的数据，然后进行处理并返回相应的Subscriber
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    void (Node::*handler)(int, const std::string&,
                          const typename MessageType::ConstPtr&),
    const int trajectory_id, const std::string& topic,
    ::ros::NodeHandle* const node_handle, Node* const node) {
  return node_handle->subscribe<MessageType>(
      topic, kInfiniteSubscriberQueueSize,
      boost::function<void(const typename MessageType::ConstPtr&)>(
          [node, handler, trajectory_id,
           topic](const typename MessageType::ConstPtr& msg) {
            (node->*handler)(trajectory_id, topic, msg);
          }));
}
 
// 路径状态显示函数
std::string TrajectoryStateToString(const TrajectoryState trajectory_state) {
  switch (trajectory_state) {
    case TrajectoryState::ACTIVE:
      return "ACTIVE";
    case TrajectoryState::FINISHED:
      return "FINISHED";
    case TrajectoryState::FROZEN:
      return "FROZEN";
    case TrajectoryState::DELETED:
      return "DELETED";
  }
  return "";
}
 
}  // namespace

2.构造函数与析构函数

// 关键
// node类构造函数，主要作用有：声明ROS的相关topic的发布器, 服务的发布器, 以及将时间驱动的函数与定时器进行绑定
Node::Node(
    const NodeOptions& node_options,
    std::unique_ptr<cartographer::mapping::MapBui

最低0.47元/天解锁文章