ROSでシミュレーション上の3Dデータを取得する
Rvizを使用して実機の3Dセンサの情報を可視化する方法は色んな所にありますが、今回はシミュレーション上にある3Dセンサの情報を可視化する方法をやってみます。
この方法を使用すると、シミュレーション上の理想的な3D空間でどのような3Dデータが取得できるのかわかるので、ロボットのVisualizationのデバッグがしやすいと思います。
最終的には、下の図のようににGazeboのシミュレーション空間内にKinectを配置し、Kinectが出力する3Dデータを可視化ソフトのRvizで表示します。
シミュレーション空間内のモデル配置が変われば、Rviz内の3Dデータも変化します。
前提
Ubuntu 16.04 64bit
ROS Kinetic Kame
事前準備
必要なソフトをインストールします。
sudo apt install ros-kinetic-gazebo-ros-pkgs ros-kinetic-gazebo-ros-control ros-kinetic-openni2-launch
また、下記レポジトリのデータをダウンロードします。
3Dデータを出力するKinectモデルを作成
モデルデータの移動
ダウンロードしたkinect.zipを展開し、フォルダを~/.gazebo/modelsフォルダに移動させます。
~/.gazebo/models内に3Dデータがあることで、gazeboを起動したあとに手動でモデルを追加することができます。
モデルデータの編集
gazebo_modelsフォルダにあるデータは単なる3Dモデルです。
このモデルをシミュレーション上に表示させたからと言って、3Dデータを出力する機能はついていません。
そこで、gazeboのplugin機能を使って、3Dデータを出力させるようにします。
ここで変更するのはkinectフォルダ内にある4つのsdfファイルです。
- model.sdf
- model-1_2.sdf
- model-1_3.sdf
- model-1_4.sdf
変更の内容はcameraタグの終わりとsensorタグの終わりの間に下の内容を挿入してください。
<plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
<baseline>0.2</baseline>
<alwaysOn>true</alwaysOn>
<!-- Keep this zero, update_rate in the parent <sensor> tag
will control the frame rate. -->
<updateRate>0.0</updateRate>
<cameraName>camera_ir</cameraName>
<imageTopicName>/camera/color/image_raw</imageTopicName>
<cameraInfoTopicName>/camera/color/camera_info</cameraInfoTopicName>
<depthImageTopicName>/camera/depth/image_raw</depthImageTopicName>
<depthImageCameraInfoTopicName>/camera/depth/camera_info</depthImageCameraInfoTopicName>
<pointCloudTopicName>/camera/depth/points</pointCloudTopicName>
<frameName>camera_link</frameName>
<pointCloudCutoff>0.5</pointCloudCutoff>
<pointCloudCutoffMax>3.0</pointCloudCutoffMax>
<distortionK1>0</distortionK1>
<distortionK2>0</distortionK2>
<distortionK3>0</distortionK3>
<distortionT1>0</distortionT1>
<distortionT2>0</distortionT2>
<CxPrime>0</CxPrime>
<Cx>0</Cx>
<Cy>0</Cy>
<focalLength>0</focalLength>
<hackBaseline>0</hackBaseline>
</plugin>
これで準備はOKです。
GazeboでKinectモデルを表示する
空のgazeboプロジェクトを開きます。
roslaunch gazebo_ros empty_world.launch
Insertタブを選択し、Kinectモデルをworldにドラッグ・アンド・ドロップします。
次に、3Dデータが分かりやすくなるように、適当なモデルをworldに追加します。
Rvizで3Dデータを可視化する
Rvizを起動します。
rosrun rviz rviz
赤枠内の設定を行うと、gazeboでシミュレーションした状態の3Dデータを表示できます。
こんな感じで、gazebo内のモデル位置を変更すると、Rviz内の3Dデータも変化します。
3Dデータ出力内容について
gazebo, rvizを起動した状態では下のようなトピックが出力されています。
rostopic list
/camera/color/camera_info
/camera/color/image_raw
/camera/color/image_raw/compressed
/camera/color/image_raw/compressed/parameter_descriptions
/camera/color/image_raw/compressed/parameter_updates
/camera/color/image_raw/compressedDepth
/camera/color/image_raw/compressedDepth/parameter_descriptions
/camera/color/image_raw/compressedDepth/parameter_updates
/camera/color/image_raw/theora
/camera/color/image_raw/theora/parameter_descriptions
/camera/color/image_raw/theora/parameter_updates
/camera/depth/camera_info
/camera/depth/image_raw
/camera/depth/points
/camera_ir/parameter_descriptions
/camera_ir/parameter_updates
/clicked_point
/clock
/gazebo/link_states
/gazebo/model_states
/gazebo/parameter_descriptions
/gazebo/parameter_updates
/gazebo/set_link_state
/gazebo/set_model_state
/gazebo_gui/parameter_descriptions
/gazebo_gui/parameter_updates
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/tf
/tf_static
ここで、3Dデータは/camera/depth/pointsになります。
シミュレーションであるgazeboが出力し、rivizが受信して可視化しているということですね。
ですので、この出力を使えば、自分のプログラムに3Dデータを利用できます。
rostopic info /camera/depth/points
Type: sensor_msgs/PointCloud2
Publishers:
* /gazebo
Subscribers:
* /rviz_1587205942154024664
最後に
Kinectのモデルデータを変更すれば、3Dセンサの仕様を変更できるため、自分の好きなデバイスに変更できます。
また、今回使った円柱のモデルもCADのモデルを使えたり、プログラムで動かせたりもできます。
今回は手軽に確認するためにできるだけプログラミングせずに使用してみました。
この記事は基本的に下記のサイトの内容をやっていて、記載がなかったところを補完しながら記事を作っています。
Discussion