蹄牛操作系统TINIUX
ROS中文社区
查看: 1248|回复: 0

如何自定义msg并且订阅topic?

[复制链接]
发表于 2017-2-21 17:48:27 | 显示全部楼层 |阅读模式
                               
这段时间刚接触ROS,以前C语言的基础也不牢。在看ROS上面一个程序包ar_pose,是一个相机捕获目标并给出相对位姿的程序,我就想能不能自己自定义一个msg,不用里面给出的gem_msg,直接输出T矩阵的数组呢?
部分代码贴在这里:
​ arUtilMat2QuatPos (marker_trans_, arQuat,arPos);//本身生成T矩阵,这个指令让其转化为四元数

     // **** convert to ROS frame

     double quat[4], pos[3];
  
     pos[0] = arPos[0] * AR_TO_ROS;
     pos[1] = arPos[1] * AR_TO_ROS;
     pos[2] = arPos[2] * AR_TO_ROS;

     quat[0] = -arQuat[0];
      quat[1] =-arQuat[1];
      quat[2] =-arQuat[2];
      quat[3] =arQuat[3];

      ROS_DEBUG ("QUAT: Pos x: %3.5f  y: %3.5f  z: %3.5f", pos[0],pos[1], pos[2]);
      ROS_DEBUG ("    Quat qx: %3.5f qy: %3.5f qz: %3.5f qw: %3.5f", quat[0], quat[1],quat[2], quat[3]);           

     //  **** publish themarker

         ar_pose_marker_.header.frame_id = image_msg->header.frame_id;
         ar_pose_marker_.header.stamp   = image_msg->header.stamp;
         ar_pose_marker_.id             = marker_info->id;
//ar_pose_marker_.pose =marker_trans_[3][4];
               ar_pose_marker_.pose.pose.position.x = pos[0];
         ar_pose_marker_.pose.pose.position.y = pos[1];
         ar_pose_marker_.pose.pose.position.z= pos[2];

         ar_pose_marker_.pose.pose.orientation.x = quat[0];
        ar_pose_marker_.pose.pose.orientation.y = quat[1];
        ar_pose_marker_.pose.pose.orientation.z = quat[2];
        ar_pose_marker_.pose.pose.orientation.w = quat[9];               
        
         ar_pose_marker_.confidence = marker_info->cf;

        arMarkerPub_.publish(ar_pose_marker_);
         ROS_DEBUG ("Published ar_singlemarker");
问题是我要如何更改程序使得输出的是数组而不是四元数呢?
感激不尽。


回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

Archiver|手机版|小黑屋|智能硬件开发社区 ( 京ICP备14010787号 )

GMT+8, 2019-4-19 22:25 , Processed in 0.055949 second(s), 15 queries .

Powered by Discuz! X3.3

© 2001-2013 Comsenz Inc.

快速回复 返回顶部 返回列表