AR Tag 机器人定位 - 范例

4960阅读 0评论2019-08-20 iibull
分类:其他平台

rosrun ar_track_alvar createMarker
生成 0 和 5 的 AR tags 对应 PNG 图像, 并打印贴在墙上。

执行 roslaunch realsense2_camera rs_rgbd.launch

编辑 D435对应的 launch。

点击(此处)折叠或打开

  1. $ cat ar_track_D435_indiv.launch
  2. <launch>
  3.     <node pkg="tf" type="static_transform_publisher" name="world_to_cam" args="1 0 0.5 0 1.57 0 world camera_link 10" />  //这个坐标系比较关键, args的位置和角度都会自动计算到最终结果的.

  4.     <arg name="marker_size" default="4.4" /> //这个是标签的边长, 厘米单位.
  5.     <arg name="max_new_marker_error" default="0.08" />
  6.     <arg name="max_track_error" default="0.2" />

  7.     <arg name="cam_image_topic" default="/camera/depth_registered/points" />
  8.     <arg name="cam_info_topic" default="/camera/depth/camera_info" />
  9.     <arg name="output_frame" default="/camera_color_optical_frame" />

  10.     <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkers" respawn="false" output="screen">
  11.         <param name="marker_size" type="double" value="$(arg marker_size)" />
  12.         <param name="max_new_marker_error" type="double" value="$(arg max_new_marker_error)" />
  13.         <param name="max_track_error" type="double" value="$(arg max_track_error)" />
  14.         <param name="output_frame" type="string" value="$(arg output_frame)" />

  15.         <remap from="camera_image" to="$(arg cam_image_topic)" />
  16.         <remap from="camera_info" to="$(arg cam_info_topic)" />
  17.     </node>

  18. </launch>

D435的相机内部参数已经写入其固件中,因此不需要在ROS中标定并使用其参数文件

roslaunch ar_track_alvar ar_track_D435_indiv.launch

查看 

点击(此处)折叠或打开

  1. $ rostopic info /ar_pose_marker
  2. Type: ar_track_alvar_msgs/AlvarMarkers

  3. Publishers:
  4.  * /ar_track_alvar (http://127.0.0.1:42163/)

  5. $ rostopic echo /ar_pose_marker
  6. ---
  7. header:
  8.   seq: 2927
  9.   stamp:
  10.     secs: 1563960640
  11.     nsecs: 231283205
  12.   frame_id: ''
  13. markers:
  14.   -
  15.     header:
  16.       seq: 0
  17.       stamp:
  18.         secs: 1563960640
  19.         nsecs: 231283205
  20.       frame_id: "/camera_color_optical_frame"
  21.     id: 5
  22.     confidence: 0
  23.     pose:
  24.       header:
  25.         seq: 0
  26.         stamp:
  27.           secs: 0
  28.           nsecs: 0
  29.         frame_id: ''
  30.       pose:
  31.         position:
  32.           x: 0.0184008729013
  33.           y: -0.0709123074642
  34.           z: 0.462459038027
  35.         orientation:
  36.           x: 0.955082543292
  37.           y: -0.253301801299
  38.           z: 0.096619174648
  39.           w: 0.119667322389
  40. ---
id 为标签的 ID
Quaternions.online 在线转化四元数为欧拉角orientation

基于摄像头的数学坐标系. (上正Y, 右正X, 前正Z)
因为ar tag 是在摄像头的视野内的, 所有x,y,z是有限制的, 落在数学坐标系XZ的第一和第四象限
//Z距离一定是正值.  (Z->Yaw         )      (X->Roll) (Y->Pitch)

一般取 pitch 的值. tag 垂直Z 时, 为 0度. 如果从下向上的Y正向看, 如果顺时针转即 X正向转向正Z, 则为 +角度, 否则为 负角度. 即 Pitch 为 和 X 轴的夹角.

上一篇:四元数 - 欧拉角转化
下一篇:linux awk 内置函数实例