はじめに

ROS2のGazebo上でRealSenseのシミュレーションできる環境を見つけた&GitHunにまとめたので紹介します。
以下のようなシミュレーションを実行できます。

また、以下のリポジトリで公開しています。

環境

以下の環境で動作を確認しています。

項目	バージョン
Windows	11
WSL	2
Ubuntu	22.04
ROS2	Humble

使い方

依存関係のインストール

まず、RealSenseのROS2用のライブラリをインストールします。
ros2_wsは各自のROS2用のディレクトリに読み替えてください。

sudo apt-get install ros-$ROS_DISTRO-realsense2-camera
cd ~/ros2_ws/src
git clone https://github.com/IntelRealSense/realsense-ros.git -b ros2-development
git clone https://github.com/pal-robotics/realsense_gazebo_plugin.git -b foxy-devel
cd ~/ros2_ws
colcon build --symlink-install

次に上記で紹介したリポジトリをダウンロードしてビルドします。

cd ~/ros2_ws/src
git clone https://github.com/koichirokato/gazebo_sensor_ros2
cd ~/ros2_ws
colcon build --symlink-install
source install/setup.bash

環境変数の追加

RealSenseのモデルファイルを読み込ませるために下記を実行します。
(実行しなくても動きますが、Gazebo上にモデルが出てこないです。)

export GAZEBO_MODEL_PATH=~/ros2_ws/install/realsense2_description/share/realsense2_description/meshes/:$GAZEBO_MODEL_PATH

ROSノードの起動

以下でGazeboが立ち上がります。

ros2 launch gazebo_sensor_ros2 realsense_gazebo.launch.py

また、Rvizの設定ファイルも用意したので、以下のコマンドでPointCloudが確認できる画面が立ち上がります。

rviz2 -d ~/ros2_ws/src/gazebo_sensor_ros2/rviz/config.rviz

簡単な解説

使い方自体はこのリポジトリ(realsense_gazebo_plugin)のREADMEを参考にしています。

URDFの定義

今回は、ただのRealSenseのモデルを定義しているため、空のbase_linkを定義して、RealSense本体のlinkのrealsense_linkとのtfを定義しています。

<?xml version="1.0" ?>
<robot name="realsense"
  xmlns:xacro="http://ros.org/wiki/xacro">

  <link name="base_link"/>

  <link name="camera_bottom_screw_frame"></link>

  <link name="camera_link">
     <visual>
      <origin xyz="0 0 0" rpy="-1.57 0 -1.57"/>
      <geometry>
        <mesh filename="file://$(find realsense2_description)/meshes/d435.dae" />
      </geometry>
    </visual>
    <collision>
      <geometry>
        <mesh filename="file://$(find realsense2_description)/meshes/d435.dae" />
      </geometry>
    </collision>
  </link>

  <link name="camera_depth_frame"></link>

  <link name="camera_depth_optical_frame"></link>

  <link name="camera_color_frame"></link>

  <link name="camera_color_optical_frame"></link>

  <link name="camera_left_ir_frame"></link>

  <link name="camera_left_ir_optical_frame"></link>

  <link name="camera_right_ir_frame"></link>

  <link name="camera_right_ir_optical_frame"></link>

  <link name="realsense_link"></link>

  <joint name="camera_joint" type="fixed">
    <parent link="realsense_link" />
    <child link="camera_bottom_screw_frame" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <joint name="camera_link_joint" type="fixed">
    <parent link="camera_bottom_screw_frame" />
    <child link="camera_link" />
    <origin xyz="0 0.0175 0.0125 " rpy="0 0 0" />
  </joint>

  <joint name="camera_depth_joint" type="fixed">
    <parent link="camera_link" />
    <child link="camera_depth_frame" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

  <!-- 一部省略 -->

  <joint name="realsense_joint" type="fixed">
    <parent link="base_link"></parent>
    <child link="realsense_link"></child>
    <pose xyz="0 0 0 " rpy="-1.57 0 -1.57" />
  </joint>

</robot>

model.sdfの定義

基本的に上記リポジトリ(realsense_gazebo_plugin)をそのまま使用しています。

RealSenseのPluginについては以下のように定義しており、トピック名やセンサの範囲も変えることができます。

<plugin name="camera" filename="librealsense_gazebo_plugin.so">
  <prefix>camera</prefix>
  <depthUpdateRate>10.0</depthUpdateRate>
  <colorUpdateRate>10.0</colorUpdateRate>
  <infraredUpdateRate>1.0</infraredUpdateRate>
  <depthTopicName>aligned_depth_to_color/image_raw</depthTopicName>
  <depthCameraInfoTopicName>depth/camera_info</depthCameraInfoTopicName>
  <colorTopicName>color/image_raw</colorTopicName>
  <colorCameraInfoTopicName>color/camera_info</colorCameraInfoTopicName>
  <infrared1TopicName>infra1/image_raw</infrared1TopicName>
  <infrared1CameraInfoTopicName>infra1/camera_info</infrared1CameraInfoTopicName>
  <infrared2TopicName>infra2/image_raw</infrared2TopicName>
  <infrared2CameraInfoTopicName>infra2/camera_info</infrared2CameraInfoTopicName>
  <colorOpticalframeName>camera_color_optical_frame</colorOpticalframeName>
  <depthOpticalframeName>camera_depth_optical_frame</depthOpticalframeName>
  <infrared1OpticalframeName>camera_left_ir_optical_frame</infrared1OpticalframeName>
  <infrared2OpticalframeName>camera_right_ir_optical_frame</infrared2OpticalframeName>
  <rangeMinDepth>0.2</rangeMinDepth>
  <rangeMaxDepth>10.0</rangeMaxDepth>
  <pointCloud>true</pointCloud>
  <pointCloudTopicName>depth/color/points</pointCloudTopicName>
  <pointCloudCutoff>0.25</pointCloudCutoff>
  <pointCloudCutoffMax>9.0</pointCloudCutoffMax>
</plugin>

おわりに

自作のロボットモデルでRealSenseを使用したい場合は、自分で新たなlinkとそれらの関係を定義して、realsense_linkといい感じにつなげれば使えるかと思います。本記事が参考になれば幸いです。

【ROS2/Gazebo】GazeboでRealSense D435のシミュレーション