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

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

https://twitter.com/ko1_baseba11/status/1625873202349244419?s=20

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

https://github.com/koichirokato/gazebo_sensor_ros2

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

| 項目 | バージョン |
|:-: | :-:|
| 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](https://github.com/pal-robotics/realsense_gazebo_plugin/tree/foxy-devel))のREADMEを参考にしています。

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

```xml
<?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](https://github.com/pal-robotics/realsense_gazebo_plugin/tree/foxy-devel))をそのまま使用しています。

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

```xml
<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といい感じにつなげれば使えるかと思います。本記事が参考になれば幸いです。

# 参考

https://github.com/pal-robotics/realsense_gazebo_plugin/tree/foxy-devel

https://github.com/ROBOTIS-JAPAN-GIT/turtlebot3_jp_custom/tree/humble-jp-devel