画像処理とマニピュレータの組み合わせ

• アプリケーションの概要
• ワークスペースのセットアップ
• カメラ姿勢のキャリブレーション
• アプリケーション用のパッケージを作成
  • パッケージ作成
  • ハードウェア構成を定義
  • launchファイル作成
• システムを起動

全セクションでマニピュレータを制御するノードと画像処理でブロックの位置を見つけるノードを作成しました。
本セクションでは、これらのノードを組み合わせてアプリケーションを作成します。

アプリケーションの概要

テーブルにブロックを置くとマニピュレータはブロックを移動するというタスクを行うアプリケーションを作成します。
このような、センサーで見つけた物を移動するタスクは産業にもサービスにもよく利用されます。

ROSでは、ノード作成ではなくてノードの組み合わせによってアプリケーションを作ることがよくあります。
本アプリケーションも同様に、本セミナーで作成したマニピュレータ制御ノードを画像処理ノードを組み合わせてアプリケーションを作成します。
ノード構造は以下のようになります。

ワークスペースのセットアップ

アプリケーション用の新しいワークスペースを作りましょう。

1
2
3
$ mkdir -p ~/rsj_2017_application_ws/src
$ cd ~/rsj_2017_application_ws/src
$ catkin_init_workspace

本ワークスペースにマニピュレータ制御ノードと画像処理ノードをコピーします。

1
2
3
$ cd ~/rsj_2017_application_ws/src
$ cp -r ~/crane_plus_ws/src/rsj_2017_pick_and_placer .
$ cp -r ~/block_finder_ws/src/rsj_2017_block_finder .

自分で作成したノードの代わりに、本セミナーに用意されたノードの利用も可能です。
パッケージごとにクローンするためのコマンドは下記の通りです。

• rsj_2017_pick_and_placer　（マニピュレータ制御ノード）

  1
  2
  3
  4
  $ cd ~/rsj_2017_application_ws/src
  $ git clone https://github.com/gbiggs/rsj_2017_pick_and_placer.git
  $ cd rsj_2017_pick_and_placer
  $ git checkout full_application_version
  

• rsj_2017_block_finder　（ブロックを見つける画像処理ノード）

  1
  2
  $ cd ~/rsj_2017_application_ws/src
  $ git clone https://github.com/Suzuki1984/rsj_2017_block_finder
  

他の必要なパッケージもワークスペースに入れます.

1
2
$ cd ~/rsj_2017_application_ws/src
$ git clone https://github.com/gbiggs/crane_plus_arm.git

最後に、ワークスペースをビルドします。
（このタイミングで必須の作業では無いのですが、ワークスペースに入れたソフトウェアのビルドに問題がないかを確認するため）

1
2
3
4
5
6
7
8
9
10
11
$ cd ~/rsj_2017_application_ws
$ catkin_make
Base path: /home/rsj/rsj_2017_application_ws
Source space: /home/rsj/rsj_2017_application_ws/src
Build space: /home/rsj/rsj_2017_application_ws/build
Devel space: /home/rsj/rsj_2017_application_ws/devel
Install space: /home/rsj/rsj_2017_application_ws/install
（省略）
[100%] Linking CXX executable /home/rsj/rsj_2017_application_ws/devel/lib/
         crane_plus_camera_calibration/calibrate_camera_checkerboard
[100%] Built target calibrate_camera_checkerboard

カメラ姿勢のキャリブレーション

マニピュレータに対してのカメラ位置を計算します。

カメラから見たブロックの姿勢をマニピュレータから見た座標系に変更するために、マニピュレータの座標系の中のカメラ姿勢が必要となります。
計測機器を用いて位置（XYZ）を計測することはできますが、正確さは劣りますし、角度は測りにくいなど、おすすめできません。

本セミナーでは計測の代わりに計算で導出します。
このために、crane_plus_armの中にcrane_plus_camera_calibrationというパッケージがあります。
このパッケージは、CRANE+マニピュレータとカメラの姿勢を計算します。
下記の手順で行います。

まずはハードウェアを準備します。
マニピュレータを机で設置し、キャリブレーションボードをマニピュレータの前に置きます。
カメラを三脚の上に載せ、下記を実行してカメラはキャリブレーションボードが見えるように調整します。
キャリブレーションボードの上に何も置かないにしてください。

カメラノードを起動する前にroscoreを起動します。
ターミナルで下記を実行してください。

1
$ roscore

別のターミナルで下記を実行してカメラを起動します。

1
2
3
4
5
$ cd ~/rsj_2017_application_ws
$ source devel/setup.bash
$ rosrun usb_cam usb_cam_node __name:=camera _camera_name:="elecom_ucam" \
    _camera_frame_id:="camera_link" _video_device:="/dev/video0" _image_width:=640 \
    _image_height:=480 _pixel_format:=yuyv _io_method:=mmap

注意：お使いの環境のカメラデバイス番号が０ではない場合は、カメラの動作確認と同様に上記のコマンドの/dev/video0を/dev/video1等に変更してください。

カメラを正しい方向に向けたら、 Ctrl+c で終了します。

次にマニピュレータのハードウェアを起動します。
ターミナルで下記を実行します。

1
2
3
$ cd ~/rsj_2017_application_ws
$ source devel/setup.bash
$ roslaunch crane_plus_hardware start_arm_standalone.launch

キャリブレーション手順中に手でマニピュレータを移動することが必要です。
しかしモータがトルクを与えている状態で外力を加えてしまうとモータ内のギアを壊す可能性があります。
このために、キャリブレーションを行う前にトルクを切っておく必要があります。

1
2
3
4
5
6
7
8
9
10
11
12
13
$ cd ~/rsj_2017_application_ws
$ source devel/setup.bash
$ rosrun crane_plus_hardware set_arm_torque off
Turning torque off for motor 1
done
Turning torque off for motor 2
done
Turning torque off for motor 3
done
Turning torque off for motor 4
done
Turning torque off for motor 5
done

エラーが出る場合もあります。エラーが出なくなる、または前舳が手で動かせるようになるまでに最後のコマンドを繰り返して実行してください。

カメラも起動することが必要です。
新しいターミナルで下記を実行します。

1
2
3
4
5
$ cd ~/rsj_2017_application_ws
$ source devel/setup.bash
$ rosrun usb_cam usb_cam_node __name:=camera _camera_name:="elecom_ucam" \
    _camera_frame_id:="camera_link" _video_device:="/dev/video0" _image_width:=640 \
    _image_height:=480 _pixel_format:=yuyv _io_method:=mmap

注意：カメラのデバイス番号は０ではない場合は、上記のコマンドの/dev/video0を/dev/video1等に変更してください。

カメラはworld座標系に対してキャリブレーションします。
しかし、crane_plus_hardwareのstart_arm_standalone.launchはマニピュレータをbase_link座標系に置きます。
一時的にworldとbase_linkの関係を示すことが必要です。
新しいターミナルで下記を実行すると、worldとbase_linkの差をtfに送信します。 （ゼロにした場合、`world`と`base_link`の中央点は同地点であることを示しています）

1
$ rosrun tf static_transform_publisher 0 0 0 0 0 0 world base_link 10

そして、キャリブレーションを始めます。

1
2
3
4
5
6
7
8
9
$ cd ~/rsj_2017_application_ws
$ source devel/setup.bash
$ roslaunch crane_plus_camera_calibration calibrate.launch
（省略）
[ INFO] [1496372099.962924251]: [calibrate] Initialized.
[ INFO] [1496372100.144910497]: [calibrate] Got image info!
Got an image callback!
Is the checkerboard correct?
Move edge of gripper to point 1 in image and press Enter.

カメラキャリブレーションプロセスが始まると以下の画像が表示されます。

画面に出る画像をよく確認してください。１〜４で示されたポイントの順番が大事です。

そしてターミナルで下記の指示が表示されます。

1
2
3
4
5
6
7
8
Move edge of gripper to point 1 in image and press Enter.

Move edge of gripper to point 2 in image and press Enter.

Move edge of gripper to point 3 in image and press Enter.

Move edge of gripper to point 4 in image and press Enter.

指示通りに、マニピュレータの指先を4つのポイントに合わせて、ポイントごとに Enter を押します。

すべてのポイントに合わせたら、姿勢が計算されます。
以下のように姿勢が出力されます。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
Resulting transform (camera frame -> fixed frame):
-0.00810475    0.988067    -0.15381   0.0112857
    0.97487   -0.026437     -0.2212    -0.12158
  -0.222626   -0.151737   -0.963023     0.60675
          0           0           0           1

Resulting transform (fixed frame -> camera frame):
 -0.0081048    0.988067    -0.15381   0.0112857
    0.97487   -0.026437     -0.2212    -0.12158
  -0.222626   -0.151737   -0.963023     0.60675
4.59121e-41 4.58323e-41 4.58323e-41           1

Static transform publisher:
rosrun tf static_transform_publisher x y z qx qy qz qw frame_id child_frame_id \
period_in_ms
rosrun tf static_transform_publisher 0.253695 0.0777012 0.559156 0.703803 0.697261 \
-0.133715 -0.0246739 /base_link /camera_link 100

URDF output:
<?xml version="1.0"?>
<robot>
        <property name="calib_cam_x" value="0.253695" />
        <property name="calib_cam_y" value="0.0777012" />
        <property name="calib_cam_z" value="0.559156" />
        <property name="calib_cam_rr" value="-2.91582" />
        <property name="calib_cam_rp" value="0.154423" />
        <property name="calib_cam_ry" value="1.579" />
        <property name="calib_frame_name" value="world" />
</robot>

必ずこの情報を保存してください。この後の手順に必要となります。

全ターミナルで Ctrl+c を押して終了します。

アプリケーション用のパッケージを作成

パッケージ作成

本アプリケーションにはノード作成が必要ありませんが、アプリケーションのノード構造等を定義するlaunchファイルを作成することが必要です。
launchファイルをパッケージに保存すると扱いやすいです。
このためのパッケージを作成します。

パッケージはcatkin_create_pkgで作成します。
依存するパッケージとして、launchファイルで利用するパッケージを指定します。
ここでは、直接依存するパッケージのみ指定が必要です。
間接的な依存パッケージは他パッケージの依存関係でcatkin_makeが対応してくれます。

1
2
3
4
5
6
7
8
$ cd ~/rsj_2017_application_ws/src
$ catkin_create_pkg rsj_2017_application rsj_2017_pick_and_placer \
    rsj_2017_block_finder crane_plus_description crane_plus_hardware usb_cam
Created file rsj_2017_application/package.xml
Created file rsj_2017_application/CMakeLists.txt
Successfully created files in /home/rsj/rsj_2017_application_ws/src/
rsj_2017_application.
  Please adjust the values in package.xml.

本パッケージにはソースコードが含まれないので、CMakeLists.txtの編集は必要ではありません。

ハードウェア構成を定義

アプリケーションのハードウェアにはマニピュレータだけでなくて、カメラもあります。
全ハードウェアをURDFファイルに定義すると上記で計算したカメラの姿勢が定義しやすいです。

以下のデイレクトリを作成し、中にwork_cell.urdf.xacroというファイルを作成します。

1
2
$ cd ~/rsj_2017_application_ws/src/rsj_2017_application
$ mkdir urdf

エディタで新しいファイルを作成し、~/rsj_2017_application_ws/src/rsj_2017_application/urdf/work_cell.urdf.xacroとして保存します。
内容は下記の通りにします。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
<?xml version="1.0" ?>
<robot name="work_cell"
    xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller"
    xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface"
    xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor"
    xmlns:xacro="http://ros.org/wiki/xacro">

  <!-- Include the Crane+ generator macro -->
  <xacro:include filename="$(find crane_plus_description)/urdf/crane_plus.xacro"/>

  <!-- A link to provide the location reference of the work cell -->
  <link name="world"/>

  <!-- A link from the world to the robot's base link -->
  <link name="base_link"/>
  <joint name="world_to_base_link" type="fixed">
    <parent link="world"/>
    <child link="base_link"/>
    <origin xyz="0 0 0" rpy="0 0 0"/>
  </joint>

  <!-- Place a Crane+ manipulator at the centre of the world -->
  <crane_plus parent="base_link"
              servo_color="black" bracket_color="grey" gripper_color="black"
              joints_vlimit="1.571"
              pan_llimit="-2.617" pan_ulimit="2.617">
    <origin xyz="0 0 0"/>
  </crane_plus>

  <!-- Place a camera in the world with a link that can be calibrated -->
  <link name="camera_link"/>
  <joint name="camera" type="fixed">
    <parent link="world"/>
    <child link="camera_link"/>
    <origin xyz="0.38019 -0.00749503 0.513993" rpy="-2.70665 0.0167343 1.55906"/>
  </joint>
</robot>

計算したカメラの姿勢を上記のファイルに定義します。
35行目にある<origin xyz="0 0 0" rpy="0 0 0"/>を編集して、カメラキャリブレーションが出力した位置と角度を入力します。

例えば、カメラキャリブレーションが姿勢として以下を出力したら：

1
2
3
4
5
6
<property name="calib_cam_x" value="0.253695" />
<property name="calib_cam_y" value="0.0777012" />
<property name="calib_cam_z" value="0.559156" />
<property name="calib_cam_rr" value="-2.91582" />
<property name="calib_cam_rp" value="0.154423" />
<property name="calib_cam_ry" value="1.579" />

35行目を下記のように変更します。

1
<origin xyz="0.253695 0.0777012 0.559156" rpy="-2.91582 0.154423 1.579"/>

これで既存のハードウェア定義を再利用してアプリケーションのハードウェアが定義できました。
そして現在のカメラの位置にこの定義を合わせました。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rsj_2017_application/blob/master/urdf/work_cell.urdf.xacro

launchファイル作成

以下のディレクトリーを作成します。

1
2
$ cd ~/rsj_2017_application_ws/src/rsj_2017_application
$ mkdir launch

そして、本ディレクトリー内にstart_app.launchというファイルを作成します。
下記は最初の内容です。
エディタで新しいファイルを作成し下記の内容を入力し、~/rsj_2017_application_ws/src/rsj_2017_application/launch/start_app.launchとして保存します。

1
2
<launch>
</launch>

以降では、アプリケーションを起動するためのノードやパラメータなどを定義します。

ハードウェアを利用するアプリケーションを作成すると、基本的robot_descriptionというパラメータにハードウェア定義をロードすることが必要です。全セクションで作成したURDFファイルはここで利用します。
start_app.launchが下記の通りになるように編集します。

1
2
3
4
5
<launch>
  <param name="robot_description"
    command="$(find xacro)/xacro \
    --inorder '$(find rsj_2017_application)/urdf/work_cell.urdf.xacro'"/>
</launch>

次はマニピュレータのハードウェアへアクセス・制御するノードを起動します。

ROSのlaunchファイルは他のlaunchファイルをインクルードすることができます。
今回インクルード機能を利用することによって、マニピュレータのハードウェア用のノードを起動します。

start_app.launchに下記を追加します。

1
  <include file="$(find crane_plus_hardware)/launch/start_arm.launch"/>

MoveIt!も起動することが必要です。上記と同様にインクルードできます。

1
  <include file="$(find crane_plus_moveit_config)/launch/move_group.launch"/>

カメラは単体なノードのでインクルードできるlaunchファイルはありません。
カメラハードウェア用のノードを起動するために下記をstart_app.launchに追加します。

1
2
3
4
5
6
7
8
9
10
  <node name="camera" pkg="usb_cam" 
   type="usb_cam_node" output="screen">
    <param name="camera_name" value="elecom_ucam"/>
    <param name="camera_frame_id" value="camera_link"/>
    <param name="video_device" value="/dev/video0"/>
    <param name="image_width" value="640"/>
    <param name="image_height" value="480"/>
    <param name="pixel_format" value="yuyv"/>
    <param name="io_method" value="mmap"/>
  </node>

注意：上記の中の/dev/video0を必要に応じて自分のハードウェアに合わせて変更してください。

ハードウェアを起動したので、次にピック・アンド・プレースのノードを起動します。
下記を launch ファイルに追加します。

1
2
3
4
  <node name="pickandplace" pkg="rsj_2017_pick_and_placer" \
  type="pick_and_placer" output="screen">
    <remap from="/block" to="/block_finder/pose"/>
  </node>

上記のノードは今までと少し違います。
<remap>タグを利用しています。

マニピュレータ制御セクションで作成したpick_and_placerノードは/blockというトピックでブロックの位置を受信するとして実装しました。しかし、画像処理セクションで作成したblock_finderノードは、/block_finder/poseというトピックでブロックの位置を送信しています。

ROSでは、このようなトピック名が合わない状況は非常に多いです。
まるで普通の状態なので、ROSは簡単な対応方法を持ちます。これは「topic remapping」です。
ノードを起動するときに、ノードのpublishとsubscribeするトピック名を変更する機能です。

launchファイルでこの機能を利用するために、<node>タグ内に<remap>タグを利用します。
上記の定義は、このノードが/blockというトピック名を利用すると自動的に/block_finder/poseに変更します。
すなわち、pick_and_placerノードは/blockではなくて、/block_finder/poseトピックにsubscribeします。
これでblock_finderとpick_and_placerは、ソースを変更せずに繋がれるようにしました。

最後に、block_finderを起動します。こちらにもトピック名をtopic remappingで変更します。

1
2
3
4
5
  <node name="block_finder" pkg="rsj_2017_block_finder" \
    type="block_finder" output="screen">
    <remap from="/usb_cam_node/camera_info" to="/camera/camera_info"/>
    <remap from="/usb_cam_node/image_raw" to="/camera/image_raw"/>
  </node>

上記により、アプリケーションを起動するlaunchファイルを作成しました。
これでアプリケーションの実装が完成です。

このソースは以下のURLでダウンロード可能です。

https://github.com/gbiggs/rsj_2017_application/blob/master/launch/start_app.launch

パッケージ全体は下記のURLでダウンロード可能です。

https://github.com/gbiggs/rsj_2017_application

システムを起動

こどで、全セクションで作ったアプリケーションを実行します。
実行の前に、まずはハードウェアをつなげて電源を入れます。

アプリケーションの起動はroslaunchで行います。
新しいターミナルで下記を実行します。

1
2
3
$ cd ~/rsj_2017_application_ws
$ source devel/setup.bash
$ roslaunch rsj_2017_application start_app.launch

アプリケーションの動きが見えるように、RVizも起動します。

1
2
3
$ cd ~/rsj_2017_application_ws
$ source devel/setup.bash
$ rviz

左側のパネル内に「Global Options」の「Fixed Frame」をworldに設定します。

「RobotModel」、「TF」、「PlanningScene」、「Trajectory」および「Camera」の可視化を追加します。

「Trajectory」の可視化オプションで「Loop animation」をオンにします。

「TF」の可視化オプションで「Marker scale」をより小さい値にすると見やすくなります。

また、「TF」の可視化オプションで「Frames」を開いて可視化されるフレームの変更でほしいフレームだけの選択ができます。

「Camera」の可視化は「By Topic」で追加し、/camera/image_rawを選択します。

表示されるカメラ画像では、ロボットの可視化モデルも表示されます。
カメラキャリブレーションは成功であったら、ハードウェアロボットと一致します。

マニピュレータの前にブロックを置いたら、block_finderはブロックの位置を判断してpick_and_placerに送信します。
そしてマニピュレータはそのブロックを移動しようとします。

下記は本アプリケーションの起動中のノードとトピックです。
既存のパッケージを利用すると、URDF一つとlaunchファイル一つでこのような複雑なシステムの実現は可能です。

</video markdown="0">