如何使用cartographer_ros发布和订阅路标landmark信息？

本文共计1011个文字，预计阅读时间需要5分钟。

上一节介绍了陀螺仪IMU传感数据的订阅和发布。本节将介绍Landmark数据的发布和订阅。

Landmark在cartographer中用作定位的修正和补充，防止定位丢失。这里重点解释一下Landmark与S的关系。

Landmark是cartographer用于定位的重要工具，它代表环境中具有明确位置的特定点。通过发布Landmark数据，cartographer能够利用这些已知点来修正和补充定位信息，从而提高定位的准确性和鲁棒性。

在cartographer中，订阅Landmark数据通常涉及以下几个步骤：

1. 创建一个Landmark主题，用于发布Landmark信息。

2.将Landmark数据发布到该主题。

3.订阅该主题，以便在程序中获取Landmark信息。

具体到Landmark与S的关系，这里S可能指的是某个特定的Landmark标识符或类型。在cartographer中，每个Landmark都有一个唯一的标识符，通常是一个字符串。通过这个标识符，可以识别并区分不同的Landmark。

例如，如果S代表一个特定的Landmark ID，那么在订阅Landmark数据时，可以通过指定这个ID来获取与S相关的Landmark信息。这样，cartographer就能够使用这个信息来进行更精确的定位和导航。

总结来说，Landmark在cartographer中起着关键作用，通过发布和订阅Landmark数据，可以实现更稳定的定位和导航。

上一节介绍了陀螺仪Imu传感数据的订阅和发布。

本节会介绍路标Landmark数据的发布和订阅。Landmark在cartographer中作为定位的修正补充，避免定位丢失。

这里着重解释一下Landmark，它与Scan，Odom，Imu数据不同，并不是直接的传感数据。它是地图上的特征点，通常是易被识别的物体。
在cartographer中，通常是用反光柱或者二维码做landmark，实际上反光柱用的更多，因为反光柱同样可以使用激光雷达识别，不需要添加多的传感器。

对于用反光板构建landmark，推荐slam大佬峰哥的博文：
使用2个反光柱作为landmark
使用3个反光柱作为landmark
对于用二维码作用landmark，这里同样推荐峰哥的博文：
使用二维码作为landmark

当然，在学习构建landmark之前，先看看Landmark的结构及如何订阅和发布landmark。

目录

1：cartographer_ros_msgs/LandmarkList消息类型

2：发布LandmarkList消息

3：订阅Landmark消息

---

1：cartographer_ros_msgs/LandmarkList消息类型

在终端查看消息数据结构：

rosmsg show cartographer_ros_msgs/LandmarkList

Landmark消息类型数据结构如下：

std_msgs/Header header uint32 seq time stamp string frame_id cartographer_ros_msgs/LandmarkEntry[] landmarks string id geometry_msgs/Pose tracking_from_landmark_transform geometry_msgs/Point position float64 x float64 y float64 z geometry_msgs/Quaternion orientation float64 x float64 y float64 z float64 w float64 translation_weight float64 rotation_weight

LandmarkList中的landmarks是LandmarkEntry合集，LandmarkEntry对应的是单个路标的位置和姿势，所以LandmarkList其实是一个或多个路标的信息。

---

2：发布LandmarkList消息

#include <ros/ros.h> #include <cartographer_ros_msgs/LandmarkList.h> #include <cartographer_ros_msgs/LandmarkEntry.h> int main(int argc, char** argv){ ros::init(argc, argv, "landmark_publisher"); ros::NodeHandle n; ros::Publisher landmark_pub = n.advertise<cartographer_ros_msgs::LandmarkList>("landmark", 50); ros::Rate r(1.0); while(n.ok()){ cartographer_ros_msgs::LandmarkList landmarkList; landmarkList.header.stamp = ros::Time::now(); landmarkList.header.frame_id = "base_link"; landmarkList.landmarks.resize(10); for(int i = 0; i < 10; i++) { landmarkList.landmarks[i].id = std::to_string(i); landmarkList.landmarks[i].tracking_from_landmark_transform.position.x = 1*i; landmarkList.landmarks[i].tracking_from_landmark_transform.position.y = 2*i; landmarkList.landmarks[i].tracking_from_landmark_transform.position.z = 3*i; landmarkList.landmarks[i].tracking_from_landmark_transform.orientation.w = 1; landmarkList.landmarks[i].tracking_from_landmark_transform.orientation.x = 0; landmarkList.landmarks[i].tracking_from_landmark_transform.orientation.y = 0; landmarkList.landmarks[i].tracking_from_landmark_transform.orientation.z = 0; landmarkList.landmarks[i].translation_weight = 10; landmarkList.landmarks[i].rotation_weight = 10; } landmark_pub.publish(landmarkList); r.sleep(); } }

值得注意的是，在真实的数据中，有多个反光柱时landmarks.id应该要是独一无二的，能通过id找到确定路标的。
所以如何识别和确定id是一个问题，通常辅助其他的反光柱构建特征三角形来识别和确定id。具体的可以参照其他资料，有机会作者会对此展开补充。

---

3：订阅Landmark消息

(1) 通过rosbag订阅

rostopic echo /landmark

(2) 通过rviz查看
打开rviz

rosrun rviz rviz

同时需要在cartographer配置文件中设置use_landmarks= true，并运行cartographer节点。
因为rviz无法接收显示cartographer_ros_msgs/LandmarkList，但是可以查看cartographer接收到landmark话题消息后发布的landmrk_pose_list。
Fixed Frame修改为base_link，添加Landmark并将Topic设为/landmrk_pose_list

(3) 编写程序打印

#include "ros/ros.h" #include "cartographer_ros_msgs/LandmarkList.h" #include "cartographer_ros_msgs/LandmarkEntry.h" void LandmarkCallback(const cartographer_ros_msgs::LandmarkList::ConstPtr &msg) { ROS_INFO("Landmark Size: %d", msg->landmarks.size()); } int main(int argc, char **argv) { ros::init(argc, argv, "listener"); ros::NodeHandle node; ros::Subscriber sublandmark = node.subscribe("landmark", 1000, LandmarkCallback); ros::spin(); return 0; }

cartographer算法运行所需要的传感器数据的结束到此就告一段落了，在了解完数据的发布和订阅之后，接着来看怎样在cartographer算法中融入和配置这些数据。

---

下一节会介绍cartographer的主要配置参数。