上一节介绍了陀螺仪Imu传感数据的订阅和发布。
本节会介绍路标Landmark数据的发布和订阅。Landmark在cartographer中作为定位的修正补充，避免定位丢失。
这里着重解释一下Landmark，它与Scan，Odom，Imu数据不同，并不是直接的传感数据。它是地图上的特征点，通常是易被识别的物体。
在cartographer中，通常是用反光柱或者二维码做landmark，实际上反光柱用的更多，因为反光柱同样可以使用激光雷达识别，不需要添加多的传感器。
当然，在学习构建landmark之前，先看看Landmark的结构及如何订阅和发布landmark。
在终端查看消息数据结构：
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其实是一个或多个路标的信息。
#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();
  }
}
</cartographer_ros_msgs::landmarklist></cartographer_ros_msgs></cartographer_ros_msgs></ros>
值得注意的是，在真实的数据中，有多个反光柱时landmarks.id应该要是独一无二的，能通过id找到确定路标的。
所以如何识别和确定id是一个问题，通常辅助其他的反光柱构建特征三角形来识别和确定id。具体的可以参照其他资料，有机会作者会对此展开补充。
(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的主要配置参数。
Original: https://www.cnblogs.com/CloudFlame/p/16443729.html
Author: CloudFlame
Title: 【cartographer_ros】六：发布和订阅路标landmark信息
原创文章受到原创版权保护。转载请注明出处：https://www.johngo689.com/712968/
转载文章受原作者版权保护。转载请注明原作者出处！