(02)Cartographer源码无死角解析-(76) ROS数据发布→3D点云地图、子图位姿、Landmark

讲解关于slam一系列文章汇总链接:史上最全slam从零开始，针对于本栏目讲解(02)Cartographer源码无死角解析-链接如下:
(02)Cartographer源码无死角解析- (00)目录_最新无死角讲解：https://blog.csdn.net/weixin_43013761/article/details/127350885
 
$文末正下方中心提供了本人联系方式，点击本人照片即可显示WX\to 官方认证$
 

一、前言

通过前面的博客，到目前为止，关于 Cartographer 数据处理，前端，后端都讲解完成了，也就是说，目前已经能够获得比较准确的地图以及位姿了。那么这些位姿外界 cartographer_ros 是如何获取到的，且进行可视化发布到 rviz 的呢？ 接下来，我们会从 src/cartographer 跳出来，回到最初的 cartographer_ros 部分继续分析源码。首先来看久违的文件 src/cartographer_ros/cartographer_ros/cartographer_ros/node.cc 文件，可以看到如下代码：

//每0.3s发布一次submap list,
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
	absl::MutexLock lock(&mutex_);
	submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
}

//每5e-3s发布一次tf与tracked_pose
void Node::PublishLocalTrajectoryData(const ::ros::TimerEvent& timer_event) {
	......
	map_builder_bridge_.GetLocalTrajectoryData()
	......
}

// 每30e-3s发布一次轨迹路径点数据
void Node::PublishTrajectoryNodeList(
	......
    map_builder_bridge_.GetTrajectoryNodeList());
    ......
}


// 每30e-3s发布一次landmark pose 数据
void Node::PublishLandmarkPosesList(
	......
    map_builder_bridge_.GetLandmarkPosesList());
    ......
}

// 每0.5s发布一次约束数据
void Node::PublishConstraintList(
	......
	map_builder_bridge_.GetConstraintList();
	.......
}

void Node::PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event) {
	......
	map_builder_bridge_.GetTrajectoryNodes()
	......

可以明显的看到，其都涉及到 map_builder_bridge_，是本质上对应于上一篇博客中提到
src/cartographer_ros/cartographer_ros/cartographer_ros/map_builder_bridge.cc 中的如下代码。

MapBuilderBridge::GetSubmapList()
	map_builder_->pose_graph()->GetAllSubmapPoses();
	
MapBuilderBridge::GetLocalTrajectoryData()
	map_builder_->pose_graph()->GetLocalToGlobalTransform(trajectory_id),
	
MapBuilderBridge::HandleTrajectoryQuery
	map_builder_->pose_graph()->GetTrajectoryNodePoses()

MapBuilderBridge::GetTrajectoryNodeList()
	map_builder_->pose_graph()->GetTrajectoryNodePoses()
	map_builder_->pose_graph()->constraints()
	
MapBuilderBridge::GetLandmarkPosesList()
	map_builder_->pose_graph()->GetLandmarkPoses()
	
MapBuilderBridge::GetConstraintList()
	pose_graph()->GetTrajectoryNodePoses()
	map_builder_->pose_graph()->GetAllSubmapPoses()
	map_builder_->pose_graph()->constraints()

二、MarkerArray消息

再后续过程中 ROS 中的 visualization_msgs/MarkerArray 这个消息类型经常被使用到，可以使用使用指令 rosmsg info visualization_msgs/MarkerArray，可以查看该消息数据，本人注释如下：

visualization_msgs/Marker[] markers  # 定义名为 markers 的可视化标记数组

uint8 ARROW=0  # 常量定义，箭头标记类型的值为 0
uint8 CUBE=1  # 常量定义，立方体标记类型的值为 1
uint8 SPHERE=2  # 常量定义，球体标记类型的值为 2
uint8 CYLINDER=3  # 常量定义，圆柱体标记类型的值为 3
uint8 LINE_STRIP=4  # 常量定义，线条标记类型的值为 4，用于连接线条的连续点
uint8 LINE_LIST=5  # 常量定义，线段标记类型的值为 5，用于连接离散点的线段
uint8 CUBE_LIST=6  # 常量定义，立方体列表标记类型的值为 6
uint8 SPHERE_LIST=7  # 常量定义，球体列表标记类型的值为 7
uint8 POINTS=8  # 常量定义，点云标记类型的值为 8
uint8 TEXT_VIEW_FACING=9  # 常量定义，文本标记类型的值为 9，面向视图
uint8 MESH_RESOURCE=10  # 常量定义，网格资源标记类型的值为 10
uint8 TRIANGLE_LIST=11  # 常量定义，三角形列表标记类型的值为 11
uint8 ADD=0  # 常量定义，添加标记操作类型的值为 0
uint8 MODIFY=0  # 常量定义，修改标记操作类型的值为 0
uint8 DELETE=2  # 常量定义，删除标记操作类型的值为 2
uint8 DELETEALL=3  # 常量定义，删除全部标记操作类型的值为 3

std_msgs/Header header  # 标记的消息头部分
  uint32 seq  # 消息序列号
  time stamp  # 时间戳
  string frame_id  # 坐标系名称

string ns  # 命名空间，用于标记分类
int32 id  # 标记的唯一标识符
int32 type  # 标记的类型，参考上面的常量定义
int32 action  # 标记的操作类型，参考上面的常量定义

geometry_msgs/Pose pose  # 标记的位姿信息
  geometry_msgs/Point position  # 位置信息
    float64 x  # x坐标
    float64 y  # y坐标
    float64 z  # z坐标
  geometry_msgs/Quaternion orientation  # 方向信息
    float64 x  # x分量
    float64 y  # y分量
    float64 z  # z分量
    float64 w  # w分量

geometry_msgs/Vector3 scale  # 缩放系数
  float64 x  # x轴缩放
  float64 y  # y轴缩放
  float64 z  # y轴缩放
  std_msgs/ColorRGBA color  # 颜色信息
  float32 r  # 红色通道值
  float32 g  # 绿色通道值
  float32 b  # 蓝色通道值
  float32 a  # 透明度通道值

duration lifetime  # 生命周期

bool frame_locked  # 是否锁定坐标系

geometry_msgs/Point[] points  # 点的集合
  float64 x  # x坐标
  float64 y  # y坐标
  float64 z  # z坐标

std_msgs/ColorRGBA[] colors  # 颜色的集合
  float32 r  # 红色通道值
  float32 g  # 绿色通道值
  float32 b  # 蓝色通道值
  float32 a  # 透明度通道值

string text  # 文本内容

string mesh_resource  # 网格资源路径

bool mesh_use_embedded_materials  # 是否使用网格嵌入的材质

不要想得太复杂了，本质上就是一堆点的坐标描述而已 ，points 表示坐标位位置，colors 是对显示时坐标颜色的描述，还有一些显示形式或者连接方式的定义。其 geometry_msgs/Pose pose 可以设置一个基准位姿，核心要注意一下的就是 visualization_msgs/Marker[] markers，表示 markers 可以存储多个 visualization_msgs/Marke 类型的数据，visualization_msgs/Marke 与 visualization_msgs/MarkerArray 存在一定相似性，这里久不重复注释了。

对于 MarkerArray 中的每个 Marker 对象，它们会继承 MarkerArray 的基本属性，例如命名空间 (ns)、类型 (type)、操作类型 (action) 等。这些基本属性在整个 MarkerArray 中是共享的，因此不需要在每个 Marker 对象中重复定义。

然而，每个 Marker 对象也可以通过自己的属性来覆盖或修改 MarkerArray 的基本属性。例如，你可以在 Marker 对象中指定不同的位置 (position)、尺寸 (scale)、颜色 (color) 等，从而使每个 Marker 对象具有不同的外观和属性。

通过在 MarkerArray 中包含多个 Marker 对象，你可以在同一个消息中传递一组相关的标记，并根据需要对每个标记进行个性化的设置。这种设计允许在一个消息中同时管理和传递多个标记，从而方便地进行可视化操作。

因此，重复包含 Marker 的信息是为了在 MarkerArray 中支持多个标记并允许个性化设置，而不必每次都重新定义基本属性。

三、PublishSubmapList()

下面就是对这些函数一步一步进行分析了。先挑几个简单的函数进行讲解，复杂的放在后面分析，先来看 PublishSubmapList() 函数：

/**
 * @brief 每0.3s发布一次submap list,
 * 这里的submap只有节点的id与当前submap的节点数, 并没有地图数据
 *
 * @param[in] unused_timer_event
 */
void Node::PublishSubmapList(const ::ros::WallTimerEvent& unused_timer_event) {
  absl::MutexLock lock(&mutex_);
  submap_list_publisher_.publish(map_builder_bridge_.GetSubmapList());
}

submap_list_publisher_ 在构造函数中被赋值：

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

::cartographer_ros_msgs::SubmapList 表示要发布消息的类型，constexpr char kSubmapListTopic[] = “submap_list” 表示消息发布 topic 名称，kLatestOnlyPublisherQueueSize 表示队列只保留最新的几个数据，默认为1。

::cartographer_ros_msgs::SubmapList 消息定义于 src/cartographer_ros/cartographer_ros_msgs/msg/SubmapList.msg 文件，如下：

std_msgs/Header header
cartographer_ros_msgs/SubmapEntry[] submap

也就是说 SubmapList 最核心的就是 SubmapEntry[] 这个列表，列表的元素当然就是 SubmapEntry 类型：

int32 trajectory_id //子图所属轨迹id
int32 submap_index //子图序列号
int32 submap_version //子图版本
geometry_msgs/Pose pose //子图位姿
bool is_frozen //是否冻结

从这里可以知道 ::cartographer_ros_msgs::SubmapLis 只有子图的一些基本信息，并没有包含图下信息。 MapBuilderBridge::GetSubmapList() 函数的作用就是通过 map_builder_->pose_graph()->GetAllSubmapPoses() 获得子图基于local系位姿，然后构建 geometry_msgs::Pose 实例赋值给 submap_entry.pose，最后封装成ROS需要的 cartographer_ros_msgs::SubmapList 类型数据。

这里现在本人也有一些奇怪，为什么是获得基于local系位姿，而不是基于glocal系位姿，可能是因为这里仅仅只包含了子图位姿，而不包含子图数据吧。

四、PublishLandmarkPosesList()

// 每30e-3s发布一次landmark pose 数据
void Node::PublishLandmarkPosesList(
    const ::ros::WallTimerEvent& unused_timer_event) {
  if (landmark_poses_list_publisher_.getNumSubscribers() > 0) {
    absl::MutexLock lock(&mutex_);
    landmark_poses_list_publisher_.publish(
        map_builder_bridge_.GetLandmarkPosesList());
  }
}

先判断一下是否存在订阅者，有订阅者才进行发布，通过调用 MapBuilderBridge::GetLandmarkPosesList() 函数获得 MarkerArray 类型的数据进行发布，实现也不算太复杂:

// landmark 的rviz可视化设置
visualization_msgs::MarkerArray MapBuilderBridge::GetLandmarkPosesList() {
  visualization_msgs::MarkerArray landmark_poses_list;
  // 获取landmark poses
  const std::map<std::string, Rigid3d> landmark_poses =
      map_builder_->pose_graph()->GetLandmarkPoses();
  for (const auto& id_to_pose : landmark_poses) {
    landmark_poses_list.markers.push_back(CreateLandmarkMarker(
        GetLandmarkIndex(id_to_pose.first, &landmark_to_index_),
        id_to_pose.second, node_options_.map_frame));
  }
  return landmark_poses_list;
}

其首先通过 map_builder_->pose_graph()->GetLandmarkPoses() 从后端拿到优化过后的 Landmark 数据，需要注意的是，这里优化过的数据是相对于 global 坐标系，如下：

// 获取所有的landmark的位姿
std::map<std::string, transform::Rigid3d> PoseGraph2D::GetLandmarkPoses()
    const {
  std::map<std::string, transform::Rigid3d> landmark_poses;
  absl::MutexLock locker(&mutex_);
  for (const auto& landmark : data_.landmark_nodes) {
    // Landmark without value has not been optimized yet.
    if (!landmark.second.global_landmark_pose.has_value()) continue;
    landmark_poses[landmark.first] =
        landmark.second.global_landmark_pose.value();
  }
  return landmark_poses;
}

然后调用 CreateLandmarkMarker() 函数为每个 Landmark 数据构建 visualization_msgs::Marker，把这些数据都存储于消息 visualization_msgs/MarkerArray 的实例 landmark_poses_list 中。

// landmark的Marker的声明与初始化
visualization_msgs::Marker CreateLandmarkMarker(int landmark_index,
                                                const Rigid3d& landmark_pose,
                                                const std::string& frame_id) {
  visualization_msgs::Marker marker;  //创建一个零时变量
  marker.ns = "Landmarks"; //命名空间
  marker.id = landmark_index; //索引
  marker.type = visualization_msgs::Marker::SPHERE;//以圆形进行显示
  marker.header.stamp = ::ros::Time::now();//执行该代码的时间戳
  marker.header.frame_id = frame_id; //默认为 "map"
  marker.scale.x = kLandmarkMarkerScale; //控制可视化形状大小
  marker.scale.y = kLandmarkMarkerScale;
  marker.scale.z = kLandmarkMarkerScale;
  marker.color = ToMessage(cartographer::io::GetColor(landmark_index)); //根据索引赋予颜色
  marker.pose = ToGeometryMsgPose(landmark_pose); //位姿转换赋值
  return marker;
}

因为 landmark 是相对于global 系的位姿，所以 frame_id 默认为 “map”，赋值给 marker.header.frame_id。

五、Node::PublishConstraintList()

// 每0.5s发布一次约束数据
void Node::PublishConstraintList(
    const ::ros::WallTimerEvent& unused_timer_event) {
  if (constraint_list_publisher_.getNumSubscribers() > 0) {
    absl::MutexLock lock(&mutex_);
    constraint_list_publisher_.publish(map_builder_bridge_.GetConstraintList());
  }
}

该函数通过 map_builder_bridge_.GetConstraintList() 把后端的约束转换成ROS格式然后进行发布，对于该部分的代码后续再进行细节分析。

六、Node::PublishTrajectoryNodeList()

// 每30e-3s发布一次轨迹路径点数据
void Node::PublishTrajectoryNodeList(
    const ::ros::WallTimerEvent& unused_timer_event) {
  // 只有存在订阅者的时候才发布轨迹
  if (trajectory_node_list_publisher_.getNumSubscribers() > 0) {
    absl::MutexLock lock(&mutex_);
    trajectory_node_list_publisher_.publish(
        map_builder_bridge_.GetTrajectoryNodeList());
  }
}

map_builder_bridge_.GetTrajectoryNodeList() 函数比较复杂，后续再进行进行详细讲解，其主要还是把后端优化过的数据节点转换成ROS的 visualization_msgs::MarkerArray 消息进行发布。

七、PublishPointCloudMap()

该函数的作用是发布3D点云地图，注意，不是2D，不过代码不算很复杂，所以就在这里讲解了。纯定位模式是不会发送的。目的是构建一个 sensor_msgs/PointCloud2 类型的数据，不过值得注意的是 Node::ros_point_cloud_map_ 就是该类型的成员变量。使用 rosmsg info sensor_msgs/PointCloud2 查看该消息的具体信息，这里久不粘贴复制了。

$（1）：$ 首先判断是否为建图模式，且存在订阅者，否者 return 不发布点云数据。然后设置 constexpr int trajectory_id = 0，表示只发布轨迹id为0的点云地图，获得该轨迹所有节点 trajectory_nodes，根据前面对 PoseGraph3D::RunOptimization() 函数的分析，可以知道节点数据 TrajectoryNode 包含有lcoal位姿与global位姿。如下：

// 包含节点的在global map下的坐标, 在local map 下的坐标与时间
struct TrajectoryNodePose {
  struct ConstantPoseData {
    common::Time time;
    transform::Rigid3d local_pose;
  };
  // The node pose in the global SLAM frame.
  transform::Rigid3d global_pose;

  absl::optional<ConstantPoseData> constant_pose_data;
};

接着判断一下轨迹节点数目是否发生变化，如果没有发生变换也不进行发布，且用 last_trajectory_nodes_size_ 记录当前轨迹节点数。

$（2）：$上锁并创建两个两个 pcl::PointCloud<pcl::PointXYZ>::Ptr 类型的变量，然然获得轨迹的起始节点与结束节点的迭代器，使用for循环进行遍历，获得遍历节点的高分辨率点云数据以及global位姿，这里回想一下 trajectory_node.constant_data 的类型定义：

  struct Data {
    common::Time time;

    // Transform to approximately gravity align the tracking frame as
    // determined by local SLAM.
    Eigen::Quaterniond gravity_alignment;

    // Used for loop closure in 2D: voxel filtered returns in the
    // 'gravity_alignment' frame.
    sensor::PointCloud filtered_gravity_aligned_point_cloud;

    // Used for loop closure in 3D.
    sensor::PointCloud high_resolution_point_cloud;
    sensor::PointCloud low_resolution_point_cloud;
    Eigen::VectorXf rotational_scan_matcher_histogram;

    // The node pose in the local SLAM frame.
    transform::Rigid3d local_pose;
  };
  

$（3）：$ 遍历每一个点云进行坐标变换，首先点云数据是相对于机器人(tracking)坐标系，在借用节点的global位姿，则可以点云数据变换到global系下。变换之后的点云都存储于 *point_cloud_map 之中。然后进行进行数据格式的转换了。

$源码注释：$

void Node::PublishPointCloudMap(const ::ros::WallTimerEvent& timer_event) {
  // 纯定位时不发布点云地图
  if (load_state_ || point_cloud_map_publisher_.getNumSubscribers() == 0) {
    return;
  }

  // 只发布轨迹id 0 的点云地图
  constexpr int trajectory_id = 0;

  // 获取优化后的节点位姿与节点的点云数据
  std::shared_ptr<MapById<NodeId, TrajectoryNode>> trajectory_nodes =
      map_builder_bridge_.GetTrajectoryNodes();

  // 如果个数没变就不进行地图发布
  size_t trajectory_nodes_size = trajectory_nodes->SizeOfTrajectoryOrZero(trajectory_id);
  if (last_trajectory_nodes_size_ == trajectory_nodes_size)
    return;
  last_trajectory_nodes_size_ = trajectory_nodes_size;

  absl::MutexLock lock(&point_cloud_map_mutex_);
  pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud_map(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr node_point_cloud(new pcl::PointCloud<pcl::PointXYZ>());
  
  // 遍历轨迹0的所有优化后的节点
  auto node_it = trajectory_nodes->BeginOfTrajectory(trajectory_id);
  auto end_it = trajectory_nodes->EndOfTrajectory(trajectory_id);
  for (; node_it != end_it; ++node_it) {
    auto& trajectory_node = trajectory_nodes->at(node_it->id);
    auto& high_resolution_point_cloud = trajectory_node.constant_data->high_resolution_point_cloud;
    auto& global_pose = trajectory_node.global_pose;

    if (trajectory_node.constant_data != nullptr) {
      node_point_cloud->clear();
      node_point_cloud->resize(high_resolution_point_cloud.size());
      // 遍历点云的每一个点, 进行坐标变换
      for (const RangefinderPoint& point :
           high_resolution_point_cloud.points()) {
        RangefinderPoint range_finder_point = global_pose.cast<float>() * point;
        node_point_cloud->push_back(pcl::PointXYZ(
            range_finder_point.position.x(), range_finder_point.position.y(),
            range_finder_point.position.z()));
      }
      // 将每个节点的点云组合在一起
      *point_cloud_map += *node_point_cloud;
    }
  } // end for

  ros_point_cloud_map_.data.clear();
  pcl::toROSMsg(*point_cloud_map, ros_point_cloud_map_);
  ros_point_cloud_map_.header.stamp = ros::Time::now();
  ros_point_cloud_map_.header.frame_id = node_options_.map_frame;
  LOG(INFO) << "publish point cloud map";
  point_cloud_map_publisher_.publish(ros_point_cloud_map_);
}

八、结语

还剩下一个函数 PublishLocalTrajectoryData() 其与 PublishPointCloudMap() 具备一定相似性，后者发布的3D点云数据。PublishLocalTrajectoryData() 还是比较复杂的，在下篇博客为大家进行分析。从这些已经分析过的函数来看，可以知道，其基本都是把数据变换到global系然后进行发布的。