cartorapher_ros：理解cartographer_ros之源码解读

阅读量：529 次

发布时间：2019-03-08

本文共 11654 字，大约阅读时间需要 38 分钟。

其实对于cartographer建图来说，cartographer_ros只是为了用户方便调用接口，建图算法的核心在cartographer中，不过cargographer_ros对于用户使用来说，还是很便捷的。

先来看看cartographer建图的节点图。

除了bag数据包和机器人发布的坐标变换，真正有用的就两个节点，cartographer_node和cartographer_occupance_grid_node，前者为调用底层算法节点，后者为地图输出节点。下面分别做介绍。

一. 接口关系

为了便于理解，先来看看他们通信的桥梁接口。

cartographer_ros

map_builder_bridge.h中定义了：std::unique_ptr<cartographer::mapping::MapBuilderInterface> map_builder_;

map_bulider_bridege.h中还定义了：SensorBridge* sensor_bridge(int trajectory_id);，它在sensor_bridge.h中定义，而在它里面又定义了 ::cartographer::mapping::TrajectoryBuilderInterface* const trajectory_builder_;

cartographer

cartographer::mapping::MapBuilderInterface：它是一个抽象类，它的具体实现在map_builder.h中。

cartographer::mapping::TrajectoryBuilderInterface也是抽象类，它的实例化在mapping/internal/collated_trajector_builder.h。

看看TrajectoryBuilderInterface的实例化，在map_builder.h中,定义了

std::vector
   
    
     > trajectory_builders_;

然后又在map_builder.cc中实现了实例化：

//CollatedTrajectoryBuilder类型的对象才是所谓的轨迹跟踪器，轨迹跟踪器可能不止一条    trajectory_builders_.push_back(absl::make_unique
   
    (        trajectory_options, sensor_collator_.get(), trajectory_id,        expected_sensor_ids,        CreateGlobalTrajectoryBuilder2D(//这个应该是用在后面的全局slam中            std::move(local_trajectory_builder), trajectory_id,            static_cast
    
     (pose_graph_.get()),            local_slam_result_callback, pose_graph_odometry_motion_filter)));

所以从上面可以看到，cartographer_ros的接口为map_builder_bridge.h，而cartographer的对接文件为map_builder_interface.h。最终数据都会在map_builder.h中

汇总它们的接口数据传输流程图：

二. cartographer_node

1. node_main.cc

该函数为cartographer_node的主函数，主要调用功能：

加载(LoadOptions)lua文件，并存入node_options和trajectory_options

监听tf：存入tf_buffer

Node构造函数(传入node_options，map_builder，tf_buffer)，订阅与发布

加载pbstream地图文件

开启默认轨迹(node.StartTrajectoryWithDefaultTopics)：传入trajectory_options

ros::spin()结束后：

完成轨迹：node.FinishAllTrajectories();

最终的地图优化：node.RunFinalOptimization();

地图系统状态： if (!FLAGS_save_state_filename.empty()) {

node.SerializeState(FLAGS_save_state_filename,
true /* include_unfinished_submaps */);
}

下面介绍构造函数和默认轨迹函数的流程。

2. node.cc

在node.h中定义了关键桥梁： map_builder_bridge_，也就是上面介绍的cartographer_ros的桥梁接口，数据会通过这个桥梁进行传输。

MapBuilderBridge map_builder_bridge_ GUARDED_BY(mutex_);

（1）构造函数(Node::Node)

在构造函数中，定义了一些发布器和服务器。注意在构造函数中没有进行订阅函数的定义。

发布器

变量	话题名称	说明
kSubmapListTopic	/submap_list	构建好的子图列表。
kTrajectoryNodeListTopic	/trajectory_node_list	跟踪轨迹路径点列表。
kLandmarkPosesListTopic	/landmark_poses_list	路标点位姿列表。
kConstraintListTopic	/constraint_list	优化约束。
kScanMatchedPointCloudTopic	/scan_matched_points2	匹配的2D点云数据。

这些发布器会通过定时器进行定时发布，分别定时发送上面五个发布器，定时器的时间在lua文件中定义。

wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.submap_publish_period_sec), &Node::PublishSubmapList, this));wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(node_options_.pose_publish_period_sec), &Node::PublishTrajectoryStates, 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));wall_timers_.push_back(node_handle_.createWallTimer(::ros::WallDuration(kConstraintPublishPeriodSec), &Node::PublishConstraintList, this));

服务器

总共有7个服务器，包括子图、开始轨迹、结束轨迹等的回调。需要注意的是建图过程中的地图显示是通过这个服务器回调实现的，之前我一直误以为通过话题订阅实现。

//地图显示服务器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));

（2）开启轨迹(Node::StartTrajectoryWithDefaultTopics)

除此之外，还可以调用服务"start_trajectory"来开启。两者的不同是，默认轨迹，使用系统默认的订阅主题，后者在调用服务的时候还需要提供轨迹配置和订阅主题。两者最终都是通过AddTrajectory来处理。这里以默认开启轨迹进行讲解。

void Node::StartTrajectoryWithDefaultTopics(const TrajectoryOptions& options) {
   //传递前端参数  absl::MutexLock lock(&mutex_);  CHECK(ValidateTrajectoryOptions(options));  AddTrajectory(options);}

这里直接只有一个函数AddTrajectory(options)函数，因此，我们看看这个函数的实现。

int Node::AddTrajectory(const TrajectoryOptions& options) {
     const std::set
   
          expected_sensor_ids = ComputeExpectedSensorIds(options);//处理一些重复的话题信息，比如scan_1,scan_2       //接着通过接口map_builder_bridge_向Cartographer添加一条新的轨迹并获取轨迹的索引。然后以该索引为键值，  //通过函数AddExtrapolator和AddSensorSamplers添加用于位姿插值(PoseExtrapolator)和传感器采样(TrajectorySensorSamplers)的对象。  const int trajectory_id =//trajectory_id是0，第一条轨迹，而且是const类型，不会变      map_builder_bridge_.AddTrajectory(expected_sensor_ids, options);//通过cartographer实现Add  AddExtrapolator(trajectory_id, options);//位姿  AddSensorSamplers(trajectory_id, options);//传感器采样,会修改sensor_samplers_的值，然后影响订阅  LaunchSubscribers(options, trajectory_id);//订阅传感器  wall_timers_.push_back(node_handle_.createWallTimer(      ::ros::WallDuration(kTopicMismatchCheckDelaySec),      &Node::MaybeWarnAboutTopicMismatch, this, /*oneshot=*/true));       //最后记录当前正在更新的轨迹，以及订阅的主题，并返回新建的轨迹索引。   //在node对象中，通过成员函数LaunchSubscribers根据配置订阅需要的主题。  for (const auto& sensor_id : expected_sensor_ids) {
       subscribed_topics_.insert(sensor_id.id);  }  return trajectory_id;}

（3）数据订阅(Node::LaunchSubscribers)

其中最重要的是订阅传感器数据函数LaunchSubscribers(options, trajectory_id);。我们来看看数据订阅的流程。

实际上在这个函数中，可以看到很多判断语句，因为需要判断lua中限定了订阅传感器的类型。在这里我们以雷达和imu数据进行举例。

Node::HandleLaserScanMessage

void Node::HandleLaserScanMessage(const int trajectory_id,                                  const std::string& sensor_id,                                  const sensor_msgs::LaserScan::ConstPtr& msg) {
     absl::MutexLock lock(&mutex_);  if (!sensor_samplers_.at(trajectory_id).rangefinder_sampler.Pulse()) {
       return;  }  map_builder_bridge_.sensor_bridge(trajectory_id)      ->HandleLaserScanMessage(sensor_id, msg);}

我们可以看到，订阅的雷达数据直接传入到map_builder_bridge_中去了，它属于和底层的通信接口。

这里我们还需要看到的是，它实际上调用的是sensor_bridge的接口。符合接口数据传输关系图。看看它的流程图：

传递的数据格式为：cartographer/sensor/point_cloud.h中的TimedPointCloud，如下图：

void AddSensorData(      const std::string& sensor_id,      const sensor::TimedPointCloudData& timed_point_cloud_data) override {
       AddData(sensor::MakeDispatchable(sensor_id, timed_point_cloud_data));  }

Node::HandleImuMessage

//下面是处理IMU的消息回调函数，除了要通过map_builder_bridge_传递给建图器之外//还需要将IMU数据传递给位姿估计器。 //这个位姿估计器的数据类型是定义在cartographer的PoseExtrapolator，//同样是在函数AddTrajectory中通过调用AddExtrapolator完成初始化操作。void Node::HandleImuMessage(const int trajectory_id,                            const std::string& sensor_id,                            const sensor_msgs::Imu::ConstPtr& msg) {
     absl::MutexLock 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);//直接传到cartographer底层}

在这里我们可以看到imu数据传给了两个模块：位姿态估计器和cartographer底层。传给底层的流程和雷达数据一样，通过map_builder_bridge_.sensor_bridge传输数据。传给位姿估计器的extrapolators_，实际上直接和cartographer对接，在node.h中定义如下：

std::map
   
     extrapolators_;

三. cartographer_occupancy_grid_node

下面我们看看另外一个节点。

occupancy_grid_node_main.cc

我们可以看到这个函数中既有主函数，又有Node函数的一些定义。而主函数中直接转入到了Node的构造函数。构造函数只传递了分辨率和发布频率。

::cartographer_ros::Node node(FLAGS_resolution, FLAGS_publish_period_sec);

在构造函数中，定义了一个发布器、一个订阅器和一个服务器。

订阅器类型：submap_list
发布器类型： nav_msgs::OccupancyGrid类型。

Node::Node(const double resolution, const double publish_period_sec)    : resolution_(resolution),      client_(node_handle_.serviceClient<::cartographer_ros_msgs::SubmapQuery>(          kSubmapQueryServiceName)),//服务器      submap_list_subscriber_(node_handle_.subscribe(          kSubmapListTopic, kLatestOnlyPublisherQueueSize,          boost::function
   
    (              [this](const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
                   HandleSubmapList(msg);              }))),//订阅器      occupancy_grid_publisher_(          node_handle_.advertise<::nav_msgs::OccupancyGrid>(              FLAGS_occupancy_grid_topic, kLatestOnlyPublisherQueueSize,              true /* latched */)),//发布器      occupancy_grid_publisher_timer_(//定时器          node_handle_.createWallTimer(::ros::WallDuration(publish_period_sec),                                       &Node::DrawAndPublish, this)) {
   }

然后我们直接看订阅函数的回调函数。在这个回调函数中，我们直接对msg进行处理，然后传递给submap_slices_，它是需要发布的数据。(submap_slices_经过加锁)

std::map
   
     submap_slices_ GUARDED_BY(mutex_);

void Node::HandleSubmapList(    const cartographer_ros_msgs::SubmapList::ConstPtr& msg) {
     absl::MutexLock locker(&mutex_);  // We do not do any work if nobody listens.  if (occupancy_grid_publisher_.getNumSubscribers() == 0) {
       return;  }  // Keep track of submap IDs that don't appear in the message anymore.  std::set
   
     submap_ids_to_delete;  for (const auto& pair : submap_slices_) {
       submap_ids_to_delete.insert(pair.first);  }  for (const auto& submap_msg : msg->submap) {
       const SubmapId id{
   submap_msg.trajectory_id, submap_msg.submap_index};    submap_ids_to_delete.erase(id);    if ((submap_msg.is_frozen && !FLAGS_include_frozen_submaps) ||        (!submap_msg.is_frozen && !FLAGS_include_unfrozen_submaps)) {
         continue;    }    SubmapSlice& submap_slice = submap_slices_[id];    submap_slice.pose = ToRigid3d(submap_msg.pose);    submap_slice.metadata_version = submap_msg.submap_version;    if (submap_slice.surface != nullptr &&        submap_slice.version == submap_msg.submap_version) {
         continue;    }    auto fetched_textures =        ::cartographer_ros::FetchSubmapTextures(id, &client_);    if (fetched_textures == nullptr) {
         continue;    }    CHECK(!fetched_textures->textures.empty());    submap_slice.version = fetched_textures->version;    // We use the first texture only. By convention this is the highest    // resolution texture and that is the one we want to use to construct the    // map for ROS.    const auto fetched_texture = fetched_textures->textures.begin();    submap_slice.width = fetched_texture->width;    submap_slice.height = fetched_texture->height;    submap_slice.slice_pose = fetched_texture->slice_pose;    submap_slice.resolution = fetched_texture->resolution;    submap_slice.cairo_data.clear();    submap_slice.surface = ::cartographer::io::DrawTexture(        fetched_texture->pixels.intensity, fetched_texture->pixels.alpha,        fetched_texture->width, fetched_texture->height,        &submap_slice.cairo_data);  }  // Delete all submaps that didn't appear in the message.  for (const auto& id : submap_ids_to_delete) {
       submap_slices_.erase(id);  }  last_timestamp_ = msg->header.stamp;  last_frame_id_ = msg->header.frame_id;}

四. 其他说明

4.1 保存地图脚本

#!/bin/bashrosservice call /finish_trajectory 0rosservice call /write_state "filename: '$(pwd)/map.pbstream'"killall rosmaster

4.2 结束建图

rosservice call /finish_trajectory 0

4.3 保存地图

rosservice call /write_state "filename: '$(pwd)/map.pbstream'"

转载地址：http://kzbiz.baihongyu.com/

你可能感兴趣的文章