浅析ros下订阅消息，并发布apriltags算法位姿

fake_ar_publisher::ARMarkerConstPtr last_msg_;
void visionCallback(const fake_ar_publisher::ARMarkerConstPtr& msg)
    {
      last_msg_ = msg;
      ROS_INFO_STREAM(last_msg_->pose.pose);
    }

这里的消息文件是：ARMarker.msg在fake_ar_publisher包里，内容如下：
Header header
uint32 id
geometry_msgs/PoseWithCovariance pose
uint32 confidence

可以看出这里先是先是通过指针找到ARMarker下的pose消息，而这里的消息类型是geometry_msgs/PoseWithCovariance，然后从官网上搜索geometry_msgs/PoseWithCovariance

看到此消息类型的定义，这里更像是一个大的结构体，里面定义了各种消息类型，从最下面看到实际的消息定义

通过这里可以理顺last_msg_->pose进行类似于对结构体的指针操作找到pose，所以写成这种形式last_msg_->pose.pose
点开geometry_msgs/Pose 可以看到内部包含了位置（xyz）和方向（xyzw）
进一步点开geometry_msgs/Point 和geometry_msgs/Quaternion 可以看到最终的消息类型
这里说一下Quaternion 有三种表示方式：旋转矩阵，欧拉角，四元数（上面定义的四元数）
到这里我们也许看到消息机制中引用的恰恰是我们熟悉的位姿格式

下面进入到我们的主题，取出apriltags2_ros下的类似于上面的位姿
运行
roslaunch usb_cam usb_cam-test.launch （打开usb摄像头，找到tag）
roslaunch apriltags2_ros continuous_detection.launch （结合opencv，用apriltags2_ros可以实现空间tags相对于相机位姿的计算）
输入rostopic list

通过代码发现这里的/tag_detections 是apriltag2_ros发布的主题，这里就是我们需要订阅的主题。
然后这里依然是需要找到消息文件，编写订阅节点
这里很懵逼的发现包含两个文件如下：
AprilTagDetection.msg

AprilTagDetectionArray.msg

这里困扰了我好久，怎么编写这里消息的订阅格式，刚开始整了两天多，断断续续的，越来越浮躁，最后索性先放了一下，找了Mastering ROS for Robotics Programming (1)书看了前面几章，然后跟着例程走了一遍，过了三天又满血复合的投入到编写节点订阅位姿的过程中。
然后对比两个msg文件发现，定义的位姿的格式不太一样
例程是geometry_msgs/PoseWithCovariance pose
这里是geometry_msgs/PoseWithCovarianceStamped pose
索性修改了一下这里的订阅格式修改成geometry_msgs/PoseWithCovariance pose
果不其然出错了，但看到下面这个错误，我恍然大悟原来detections[ ]是一个数组，需要给其指定数组位

然后从AprilTagDetectionArray.msg文件和源码文件都发现了这是个数组的定义，只能说自己看的不够仔细，思维定视。
然后自己修改写出了下面订阅位姿的节点

#include "ros/ros.h"
#include "apriltags2_ros/AprilTagDetectionArray.h"
#include 
class Localizer
{
public:
  Localizer(ros::NodeHandle& nh)
  {
      ar_sub_ = nh.subscribe("tag_detections", 1,
      &Localizer::number_callback, this);
  }
  void number_callback(const apriltags2_ros::AprilTagDetectionArray::ConstPtr& msg)
  {
      last_msg_ = msg;
      ROS_INFO_STREAM(last_msg_->detections[0].pose.pose.pose);
      //ROS_INFO("the number %d ",);
      //ROS_INFO("%s\n", s.data.c_str());
  }

  ros::Subscriber ar_sub_;
  apriltags2_ros::AprilTagDetectionArrayConstPtr last_msg_;
};


int main(int argc, char **argv)
{
    ros::init(argc, argv,"apriltag_detector_subscriber");
    ros::NodeHandle node_obj;
    Localizer localizer(node_obj);
    //ros::Subscriber number_subscriber = node_obj.subscribe("tag_detections",1,number_callback);
    ROS_INFO("1Vision node starting");
    ros::spin();
    //return 0;
}

这里输入

rostopic info /tag_detections

看出了此话题的类型是apriltags2_ros/AprilTagDetectionArray
所以头文件写成 #include “apriltags2_ros/AprilTagDetectionArray.h”
输入

rostopic echo /tag_detections

这里我们看到了我们想要的position和orientation，也提示我们为什么这麽写输出流
ROS_INFO_STREAM(last_msg_->detections[0].pose.pose.pose);
通过last_msg_->detections[0]订阅到话题下的detections消息，然后通过三级的pose指针订阅到了我们想要的位姿

运行节点

rosrun apriltags2_node vision_node1

订阅到了实时的位姿输出

rqt_graph

节点图如下：