【cartographer_ros】六： 釋出和訂閱路標landmark資訊

CloudFlame發表於2022-07-11
ROS

上一節介紹了陀螺儀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的主要配置引數。

相關文章