動機

カメラの接続・認識や複数台のカメラの取扱いができたので、次はARマーカーを使って、その位置・姿勢の情報を取得したい。逆行列をつかえば、マーカーに対しての、カメラの位置・姿勢を取得できるはず。

ROSには、ar_track_arlvarという、ARのパッケージと連携出きるのでそれを使う。

環境

Ubuntu 16.04 LTS
ROS Kinetic Kame
今回使用したのUSBカメラ（Logicool C270　UVC対応）1台

参考にした情報

ROSではじめるロボットプログラミング

ARパッケージのインストール

$ sudo apt-get install ros-kinetic-ar-track-alvar

基本は、これだけ。

ARマーカーの準備

上記、リンクに、マーカーのpdfなどがあるので、それらを印刷

launchファイルの作成・実行

launchファイルは、
RobotやSystemを完成させるまで Wiki*
の情報をそのまま使わせてもらう。
まだまだ、設定パラメータの意味が分かっていないなぁ。

ar_track.launch

<launch>
  <node pkg="tf" type="static_transform_publisher" name="map_to_camera" output="screen" args="0 0 0 0.785398163 0 0 map camera 10" />

  <node name="uvc_camera" pkg="uvc_camera" type="uvc_camera_node" output="screen" >
    <param name="device" value="/dev/video0" />
    <param name="width" value="640" />
    <param name="height" value="480" />
    <param name="fps" value="30"/>
    <param name="frame_id" value="camera" />
    <!-- <param name="camera_info_url" type="string" value="file://$(find PACKAGE_NAME)/cal.yml" />-->
    <param name="camera_info_url" type="string" value="file:///home/{user_name}/.../camera.yaml"/>
  </node>

  <node name="image_view" pkg="image_view" type="image_view" output="screen" >
    <remap from="image" to="image_raw" />
  </node>

  <!-- <arg name="marker_size" default="5.5" />-->
  <arg name="marker_size" default="7.8" />
  <arg name="max_new_marker_error" default="0.08" />
  <arg name="max_track_error" default="0.2" />
  <arg name="cam_image_topic" default="/image_raw" />
  <arg name="cam_info_topic" default="/camera_info" />	
  <arg name="output_frame" default="/camera" />

  <node name="ar_track_alvar" pkg="ar_track_alvar" type="individualMarkersNoKinect" respawn="false" output="screen" 
    args="$(arg marker_size) $(arg max_new_marker_error) $(arg max_track_error) $(arg cam_image_topic) $(arg cam_info_topic) $(arg output_frame)" />

重要なのは、下のほうか。
{user_name}/.../あたりは、それぞれの環境で。
コメントアウトしたように、パッケージ名を元に、相対パスを指定することもできる。
（汎用的になのは、相対パスだよなぁ。）

確認

上記launchファイルを動かしただけだと、カメラ画像がでるだけなので、
可視化をしてみる。

$ rosrun rviz rviz

を実行して、空の表示がでるだけなら、左下方の[Add]ボタンを押して、[by display type]タブの[TF]を選択して、[OK]。すると、カメラを原点にしたときの、マーカーの座標軸の様子が出る。
また、現在のカメラで映っている画像を確認するには、同様に[Add]ボタンから[Camera]を追加する。何も映らないようならば、左上部のCameraツリーの[Image Topic]に、/image_rawをプルダウンから選んで、決定をすればよい。

次には、ノードとトピックの関係を調べてみる。

$ rqt_graph

を実行すれば、

となって、カメラノードやビューワーノードがあり、ar_track_alvarノードが解析部？

つぎに、やりとりされているデータの確認。

$ rostopic list

とすれば、やりとりされているトピックなどが見える。

$ rostopic list
/ar_pose_marker
/ar_track_alvar/enable_detection
/ar_track_alvar/parameter_descriptions
/ar_track_alvar/parameter_updates
/camera_info
/image_raw
/image_raw/compressed
/image_raw/compressed/parameter_descriptions
/image_raw/compressed/parameter_updates
/image_raw/compressedDepth
/image_raw/compressedDepth/parameter_descriptions
/image_raw/compressedDepth/parameter_updates
/image_raw/theora
/image_raw/theora/parameter_descriptions
/image_raw/theora/parameter_updates
/image_view/output
/image_view/parameter_descriptions
/image_view/parameter_updates
/rosout
/rosout_agg
/tf
/tf_static
/visualization_marker

今後、必要なのは、/ar_pose_markerや/tfあたり。

rostopic info /tf

とすれば、

$ rostopic info /tf
Type: tf2_msgs/TFMessage

Publishers: 
 * /map_to_camera (http://deepstation:46842/)
 * /ar_track_alvar (http://deepstation:44478/)

Subscribers: 
 * /ar_track_alvar (http://deepstation:44478/)

また、

$ rostopic info /ar_pose_marker

とすると、

Type: ar_track_alvar_msgs/AlvarMarkers

Publishers: 
 * /ar_track_alvar (http://deepstation:44478/)

Subscribers: None

と出るので、メッセージの型と、誰から誰に送られているかが分かる。
データを他でも横取りするには、この情報を元に、別のSubscriberを用意すれば良さそうだ。

ちなみに、やりとりされているメッセージの内部構造を調べておく。

まずは、/tfの内容。

$ rosmsg info tf2_msgs/TFMessage

とすれば、

geometry_msgs/TransformStamped[] transforms
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  string child_frame_id
  geometry_msgs/Transform transform
    geometry_msgs/Vector3 translation
      float64 x
      float64 y
      float64 z
    geometry_msgs/Quaternion rotation
      float64 x
      float64 y
      float64 z
      float64 w

transformsの配列があり、それぞれに、headerとchild_frame_id,transformが入っている様子。位置や姿勢を取り出すには、transformsの一つを取り出して(xxxとする)、
xxx.transform.translationで並進移動量ベクトルを、xxx.transform.rotationで座標軸の回転ベクトル（ここではクォータニオン）が取得できそう。

また、/ar_pose_markerのほうも見てみると、

$ rosmsg info ar_track_alvar_msgs/AlvarMarkers

とすれば、ar_pose_markerでやりとりされているメッセージの型が以下のように見える。

std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
ar_track_alvar_msgs/AlvarMarker[] markers
  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  uint32 id
  uint32 confidence
  geometry_msgs/PoseStamped pose
    std_msgs/Header header
      uint32 seq
      time stamp
      string frame_id
    geometry_msgs/Pose pose
      geometry_msgs/Point position
        float64 x
        float64 y
        float64 z
      geometry_msgs/Quaternion orientation
        float64 x
        float64 y
        float64 z
        float64 w

すると、一つのマーカーの位置・姿勢ベクトルは、
(やりとされているメッセージをmarkersという変数で受けたとすれば)、

ar_markers = markers.markers
for m in ar_markers:
    ar_pose = pose.pose
    pos = ar_pose.position
    rot = ar_pose.orientation

のようにすれば、取得できそう。

まだ、個別の数値をチェックしていないのだけれども、メッセージの型を見る限り、/ar_pose_markerと/tfの両方に似たようなデータがあるけど、どっちをつかえばいいのだろう。カメラから見たマーカー座標系の位置・姿勢と、マーカーから見たカメラ座標の位置・姿勢ということなのかな？（RVizで可視化したときの振る舞いを考えると、/tfは前者かな？）