ROS Kineticで、ARマーカーの検出・データ型確認

# 動機

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

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


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

# 参考にした情報
[ROSではじめるロボットプログラミング](https://www.amazon.co.jp/dp/4777519015)


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

```
$ sudo apt-get install ros-kinetic-ar-track-alvar
```
基本は、これだけ。

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

# launchファイルの作成・実行

launchファイルは、
[RobotやSystemを完成させるまで Wiki*](http://wikiwiki.jp/naonaorange/?ROS%2FPackage%2Far_track_alvar)
の情報をそのまま使わせてもらう。
まだまだ、設定パラメータの意味が分かっていないなぁ。

```python: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
```
を実行すれば、
![rosgraph_ar.png](https://qiita-image-store.s3.amazonaws.com/0/233094/7bcf256f-33bd-d370-ef58-439aada84b16.png)
となって、カメラノードやビューワーノードがあり、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は前者かな？）