ロボットに使用される定番センサーとしてステレオカメラやレーザーレンジファインダーなどがあります。これらを使うことで、ロボットの周囲の環境を判定し、ロボットの動作を決めることができるようになり、ロボットの自立制御の第一歩となります。
センサーのその他の選択として、マイクロソフト社Kinectやインテル社のRealSenseといった、デプスカメラなどが挙げられます。今回はロボットシミュレータにデプスカメラを追加していきます。
ToFカメラのシミュレーション
Gazeboではデプスカメラのシミュレーションが可能です。
Openni Kinectプラグインを使うと、ROSのXbox kinect用ドライバと同じメッセージを発行してくれます。このkinect用ドライバを使うと、kinectをUbuntuに接続しROSから使用することができます。今後この連載ではKintect以外のToFセンサを使う予定ですが、どのデプスカメラでも言えることですが、点の集合である点群(Point Cloud)が得られる、という点においては同一であるため、シミュレータとして十分に役立ってくれます。
ロボットにデプスカメラを取り付ける
それでは、ロボットの定義ファイルにデプスカメラのLinkと、Jointを追加しましょう。ロボットの前方に取り付けることにしましょう。
<link name="pico_flexx_link_optical_frame" />
<link name="pico_flexx_depth_frame"/>
<link name="pico_flexx_link">
<visual>
<geometry>
<box size="0.007 0.066 0.016"/>
</geometry>
<material name="green"/>
</visual>
<collision>
<geometry>
<box size="0.007 0.066 0.016"/>
</geometry>
</collision>
<inertial>
<origin xyz="0 0 0" rpy="0 0 0"/>
<mass value="1"/>
<inertia
ixx="1.0" ixy="0.0" ixz="0.0"
iyy="1.0" iyz="0.0"
izz="1.0"/>
</inertial>
</link>
<joint name="pico_flexx_base_joint" type="fixed">
<origin xyz="0.2 0 0.07" rpy="0 0 0"/>
<parent link="base_link"/>
<child link="pico_flexx_link_optical_frame"/>
</joint>
<joint name="pico_flexx_depth_joint" type="fixed">
<origin xyz="0 0 0" rpy="${-M_PI/2} 0 ${-M_PI/2}" />
<parent link="pico_flexx_link_optical_frame" />
<child link="pico_flexx_depth_frame" />
</joint>
<joint name="pico_flexx_joint" type="fixed">
<origin xyz="0 0 0" rpy="0 0 0"/>
<parent link="pico_flexx_link_optical_frame"/>
<child link="pico_flexx_link"/>
</joint>
Joint名にpico_flexx〜とつけていますが、これは今回Kinectのかわりに使用するセンサーの名前です。次回以降で使い方を解説する予定です。
rpyで回転を指定しますが、これは度ではなくラジアンでしてします。計算が面倒なため、
先頭でPIの値を定義して使っています。定義は次のように行います。
次にGazeboのプラグインの設定を追加します。
<?xml version="1.0"?> <robot name="my_robo" > <property name="M_PI" value="3.14159274"/>
次にGazeboのプラグインの設定を追加します。
<gazebo reference="pico_flexx_link">
<sensor type="depth" name="pico_flexx_ir_sensor">
<always_on>true</always_on>
<update_rate>0.0</update_rate>
<camera>
<horizontal_fov>${57.0*M_PI/180.0}</horizontal_fov>
<image>
<format>L8</format>
<width>640</width>
<height>480</height>
</image>
<clip>
<near>0.5</near>
<far>3.0</far>
</clip>
</camera>
<plugin name="pico_flexx_link_controller" filename="libgazebo_ros_openni_kinect.so">
<alwaysOn>true</alwaysOn>
<updateRate>1.0</updateRate>
<cameraName>pico_flexx_link_ir</cameraName>
<depthImageTopicName>/pico_flexx_link/depth_registered/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/pico_flexx_link/depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/pico_flexx_link/points</pointCloudTopicName>
<frameName>pico_flexx_depth_frame</frameName>
<pointCloudCutoff>0.5</pointCloudCutoff>
<distortionK1>0.00000001</distortionK1>
<distortionK2>0.00000001</distortionK2>
<distortionK3>0.00000001</distortionK3>
<distortionT1>0.00000001</distortionT1>
<distortionT2>0.00000001</distortionT2>
</plugin>
</sensor>
<material value="Gazebo/Blue" />
</gazebo>
ロボットの定義ファイルはこれで完成です。
起動してみましょう。
$ roslaunch my_robo_description gazebo.launch
青いセンサーが追加されているのがわかります。
シュミレータ上にオブジェクトを配置する
では、センサーで検知する物体をシミュレータ上に追加してみましょう。
ツールバーのCylinderを選択し、画面上をクリックすると、円柱が配置されます。
また左ペインのInsertタブより、Cinder Blockを選択して、画面上をクリックすると、コンクリートブロックが配置されます。自由に配置してみましょう。
このようにGazeboでは色々な物体を追加することが出来ます。ブロック以外にもWeb上に用意してある様々なデータをダウンロードして追加することも可能です。
センサーの値を確認する
では、この状態でセンサーの値がどのようになっているか確認してみましょう。
RVizを起動し、ロボットの状態を表示してみましょう。
rosrun rviz rviz
rosrunはノードを起動するコマンドです。
rosrun [パッケージ名] [ノード名]
RVizが起動したら、左ペインのDisplays->Global Options->Fixed Frameでbase_footprintを選択します。
左下の「Add」ボタンをクリックし、「By display type」よりRobotModelを追加します。
また、「By topic」タブより
/pico_frexx_link/points/の、PointCloud2を追加します。これで、点の集合であるPointCloudの情報が得られます。
実際のロボットではこのデータを使って障害物を判定したり、周囲の地図を作成したりして、自立制御に使用しています。
 |
| シミュレータに搭載したデプスカメラから得られたPointCloudの情報。Displaysに追加したPointCloud2の表示設定を変更し見やすくしています。 |
次回は、ROSと実際のデプスカメラを接続し使用できるようにセットアップします。
ROSで始めるロボティクス（6） ー ROSでデプスカメラPMD CamBoard pico flexxを使う
ROSで始めるロボティクス（7） ー ロボットのための二次元mapを作る
ROSで始めるロボティクス（8） ー ロボットのナビゲーションを行う
ROSで始めるロボティクス（7） ー ロボットのための二次元mapを作る
ROSで始めるロボティクス（8） ー ロボットのナビゲーションを行う
記載されている会社名、および商品名等は、各社の商標または登録商標です。
障害物の位置座標を数値として出力する方法があったら教えて頂けませんか？
返信削除