😀

ROSでシミュレーション上の3Dデータを取得する

2022/12/24に公開約4,400字

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

また、下記レポジトリのデータをダウンロードします。
https://bitbucket.org/osrf/gazebo_models/src/default/

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のモデルを使えたり、プログラムで動かせたりもできます。
今回は手軽に確認するためにできるだけプログラミングせずに使用してみました。

この記事は基本的に下記のサイトの内容をやっていて、記載がなかったところを補完しながら記事を作っています。
http://gazebosim.org/tutorials/?tut=ros_depth_camera

GitHubで編集を提案

Discussion

ログインするとコメントできます