• 欢迎访问开心洋葱网站,在线教程,推荐使用最新版火狐浏览器和Chrome浏览器访问本网站,欢迎加入开心洋葱 QQ群
  • 为方便开心洋葱网用户,开心洋葱官网已经开启复制功能!
  • 欢迎访问开心洋葱网站,手机也能访问哦~欢迎加入开心洋葱多维思维学习平台 QQ群
  • 如果您觉得本站非常有看点,那么赶紧使用Ctrl+D 收藏开心洋葱吧~~~~~~~~~~~~~!
  • 由于近期流量激增,小站的ECS没能经的起亲们的访问,本站依然没有盈利,如果各位看如果觉着文字不错,还请看官给小站打个赏~~~~~~~~~~~~~!

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

人工智能 光头明明 2600次浏览 0个评论
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  
浅析ros下订阅消息,并发布apriltags算法位姿   看到此消息类型的定义,这里更像是一个大的结构体,里面定义了各种消息类型,从最下面看到实际的消息定义  
浅析ros下订阅消息,并发布apriltags算法位姿   通过这里可以理顺last_msg_->pose进行类似于对结构体的指针操作找到pose,所以写成这种形式last_msg_->pose.pose 点开geometry_msgs/Pose 可以看到内部包含了位置(xyz)和方向(xyzw) 进一步点开geometry_msgs/Point 和geometry_msgs/Quaternion 可以看到最终的消息类型 这里说一下Quaternion 有三种表示方式:旋转矩阵,欧拉角,四元数(上面定义的四元数) 到这里我们也许看到消息机制中引用的恰恰是我们熟悉的位姿格式  
浅析ros下订阅消息,并发布apriltags算法位姿   下面进入到我们的主题,取出apriltags2_ros下的类似于上面的位姿 运行   roslaunch usb_cam usb_cam-test.launch (打开usb摄像头,找到tag) roslaunch apriltags2_ros continuous_detection.launch (结合opencv,用apriltags2_ros可以实现空间tags相对于相机位姿的计算) 输入rostopic list  
浅析ros下订阅消息,并发布apriltags算法位姿   通过代码发现这里的/tag_detections 是apriltag2_ros发布的主题,这里就是我们需要订阅的主题。 然后这里依然是需要找到消息文件,编写订阅节点   这里很懵逼的发现包含两个文件如下: AprilTagDetection.msg  
浅析ros下订阅消息,并发布apriltags算法位姿   AprilTagDetectionArray.msg  
浅析ros下订阅消息,并发布apriltags算法位姿   这里困扰了我好久,怎么编写这里消息的订阅格式,刚开始整了两天多,断断续续的,越来越浮躁,最后索性先放了一下,找了Mastering ROS for Robotics Programming (1)书看了前面几章,然后跟着例程走了一遍,过了三天又满血复合的投入到编写节点订阅位姿的过程中。   然后对比两个msg文件发现,定义的位姿的格式不太一样 例程是geometry_msgs/PoseWithCovariance pose 这里是geometry_msgs/PoseWithCovarianceStamped pose 索性修改了一下这里的订阅格式修改成geometry_msgs/PoseWithCovariance pose 果不其然出错了,但看到下面这个错误,我恍然大悟原来detections[ ]是一个数组,需要给其指定数组位  
浅析ros下订阅消息,并发布apriltags算法位姿   然后从AprilTagDetectionArray.msg文件和源码文件都发现了这是个数组的定义,只能说自己看的不够仔细,思维定视。 然后自己修改写出了下面订阅位姿的节点  

#include "ros/ros.h"
#include "apriltags2_ros/AprilTagDetectionArray.h"
#include <iostream>
class Localizer
{
public:
  Localizer(ros::NodeHandle& nh)
  {
      ar_sub_ = nh.subscribe<apriltags2_ros::AprilTagDetectionArray>("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

 
浅析ros下订阅消息,并发布apriltags算法位姿   看出了此话题的类型是apriltags2_ros/AprilTagDetectionArray 所以头文件写成 #include “apriltags2_ros/AprilTagDetectionArray.h” 输入  

rostopic echo /tag_detections

 
浅析ros下订阅消息,并发布apriltags算法位姿   这里我们看到了我们想要的position和orientation,也提示我们为什么这麽写输出流 ROS_INFO_STREAM(last_msg_->detections[0].pose.pose.pose); 通过last_msg_->detections[0]订阅到话题下的detections消息,然后通过三级的pose指针订阅到了我们想要的位姿   运行节点  

rosrun apriltags2_node vision_node1

  订阅到了实时的位姿输出  
浅析ros下订阅消息,并发布apriltags算法位姿  

rqt_graph

  节点图如下:  
浅析ros下订阅消息,并发布apriltags算法位姿


开心洋葱 , 版权所有丨如未注明 , 均为原创丨未经授权请勿修改 , 转载请注明浅析ros下订阅消息,并发布apriltags算法位姿
喜欢 (0)

您必须 登录 才能发表评论!

加载中……