AR Tag 机器人定位 - 范例

rosrun ar_track_alvar createMarker

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

执行 roslaunch realsense2_camera rs_rgbd.launch

编辑 D435对应的 launch。

$ cat ar_track_D435_indiv.launch
<launch>
    <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的位置和角度都会自动计算到最终结果的.

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

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

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

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

</launch>

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

roslaunch ar_track_alvar ar_track_D435_indiv.launch

查看 

$ rostopic info /ar_pose_marker
Type: ar_track_alvar_msgs/AlvarMarkers

Publishers:
 * /ar_track_alvar (http://127.0.0.1:42163/)

$ rostopic echo /ar_pose_marker
---
header:
  seq: 2927
  stamp:
    secs: 1563960640
    nsecs: 231283205
  frame_id: ''
markers:
  -
    header:
      seq: 0
      stamp:
        secs: 1563960640
        nsecs: 231283205
      frame_id: "/camera_color_optical_frame"
    id: 5
    confidence: 0
    pose:
      header:
        seq: 0
        stamp:
          secs: 0
          nsecs: 0
        frame_id: ''
      pose:
        position:
          x: 0.0184008729013
          y: -0.0709123074642
          z: 0.462459038027
        orientation:
          x: 0.955082543292
          y: -0.253301801299
          z: 0.096619174648
          w: 0.119667322389
---

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 轴的夹角.