複数kinectを用いたROSシミュレーション上での3Dデータ取得方法と座標変換

# はじめに
今回はGazeboシミュレーション上にある3Dセンサ(kinect)の情報をRVizを使用して可視化する．
また，[GAZEBOのROS深度カメラ統合](https://classic.gazebosim.org/tutorials/?tut=ros_depth_camera)を元に，RVizで可視化をすると下の図のようにGazeboとRVizで座標があっていないことがわかる．
この座標が合うように変換し，さらに複数台のセンサを統合させるパッケージを作成．
パッケージは[作業手順](#作業手順)から使えるようになる。すぐに使いたい方は[作業手順](#作業手順)から行うことがおすすめ．
下記にパッケージの内容を示しており，理解するとパッケージを自分で作成することもできる．

![座標変換前.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3828823/5ad4102c-87fd-529a-b06a-a6262ccb9748.jpeg)

## 環境
この記事は以下の環境で動いている

| 項目 | 値 |
|:-:|:-:|
| Ubuntu | 20.04 |
| ROS | Noetic |
| CUDA | 11.4 |

## 座標変換
GazeboとRVizで表示される内容が異なる原因は、RVizのワールド座標系とKinectのセンサ座標系があっていないことが考えられる．
* ワールド座標系
ワールド座標系はコンピュータ上で3次元空間全体を定義する座標系．
* センサ座標系
センサ座標系はその名前の通り，センサ上に設定された座標系．そのため、センサの動きとともに座標系が変化していく．

RVizでKinectが上を向く原因は，Kinectのセンサ座標系の奥行きがZ軸になっており，下記のRVizのワールド座標系のZ軸に沿っているため．これを修正するために座標変換ライブラリのTFを用いて座標を合わせる．

![画像 1.jpg](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3828823/7e9d64c5-3bfc-3a2c-8849-b98ec7a6e39d.jpeg)

## 座標変換ファイル
```gazebo_to_tf.py
#!/usr/bin/env python3
import rospy
import tf
import math
from gazebo_msgs.msg import ModelStates

def callback(data):
    br = tf.TransformBroadcaster()
    try:
        for model_name in ['kinect1', 'kinect2', 'kinect3']:
            index = data.name.index(model_name)
            pose = data.pose[index]

            # Gazeboからのクォータニオンを取得
            original_orientation = (
                pose.orientation.x,
                pose.orientation.y,
                pose.orientation.z,
                pose.orientation.w
            )

            # 追加の座標変換を適用（x軸周りに-90度回転、z軸周りに-90度回転）
            additional_rotation = tf.transformations.quaternion_from_euler(-math.pi / 2, 0, -math.pi / 2)

            # クォータニオンの乗算を行い、最終的な回転を求める
            final_orientation = tf.transformations.quaternion_multiply(original_orientation, additional_rotation)

            # ブロードキャスト
            br.sendTransform((pose.position.x, pose.position.y, pose.position.z), #Kinectモデルの位置
                             final_orientation, #最終的なクォータニオン
                             rospy.Time.now(), #現在のros時間
                             f"{model_name}_camera_link", 
                             "world") #親フレームと子フレーム
    except ValueError: #Kinectモデルがdata.nameにない場合何もしないでスルー
        pass

if __name__ == '__main__':
    rospy.init_node('gazebo_to_tf') #gazebo_to_tfという名前のrosノードを初期化
    rospy.Subscriber('/gazebo/model_states', ModelStates, callback) #gazebo/model_statesトピックからメッセージを受け取るとcallback関数を呼び出す
    rospy.spin() #ROSノードが終了するまでループを維持
```
* `quaternion_from_euler`で座標変換を行っている
* `quaternion_multiply`で`original`と`additinal`のクォータニオンの乗算を行っている
* `br.sendTransform`でそれぞれのKinectの最終的な設定などを行っている
* 3つのKinectを利用しているので関数を利用してそれぞれを座標変換している

## launchファイル
```start_camera.launch
<launch>
  <!-- Gazeboの起動 -->
  <include file="$(find gazebo_ros)/launch/empty_world.launch" />

  <!-- Kinect1の起動 -->
  <node name="gazebo_camera_plugin_1" pkg="gazebo_ros" type="spawn_model" args="-file $(find my_camera_package)/models/kinect/model1.sdf -sdf -model kinect1 -x 0 -y 0 -z 0" />

  <!-- Kinect2の起動 -->
  <node name="gazebo_camera_plugin_2" pkg="gazebo_ros" type="spawn_model" args="-file $(find my_camera_package)/models/kinect/model2.sdf -sdf -model kinect2 -x 2 -y 1 -z 0 -R 0 -P 0 -Y 3.92699" />

  <!-- Kinect3の起動 -->
  <node name="gazebo_camera_plugin_3" pkg="gazebo_ros" type="spawn_model" args="-file $(find my_camera_package)/models/kinect/model3.sdf -sdf -model kinect3 -x 2 -y -1 -z 0 -R 0 -P 0 -Y 2.35619" />

  <!-- Gazeboのモデル状態をTFに変換するノード -->
  <node name="gazebo_to_tf" pkg="my_camera_package" type="gazebo_to_tf.py" output="screen" />
</launch>

```
GazeboやそれぞれのKinectを起動

## Kinectモデル
Kinectモデルは[gazebo_models](https://github.com/osrf/gazebo_models)のgithubを適当な場所にクローンして，その中のkinectディレクトリを使う．

このモデルデータではKinectモデルをGazeboに表示させることができるが，シミュレーション上に表示させても3Dデータを出力する機能は付いていないので，gazeboのplugin機能を使って3DデータをRVizに出力させる．
`kinect/model.sdf`のカメラタグとセンサタグの間にpluginを記述．

```追加したplugin.sdk
<plugin name="camera_plugin" filename="libgazebo_ros_openni_kinect.so">
          <robotNamespace>/kinect1</robotNamespace>
          <baseline>0.2</baseline>
          <alwaysOn>true</alwaysOn>
          <updateRate>0.0</updateRate>
          <cameraName>camera_ir</cameraName>
          <imageTopicName>/kinect1/camera/color/image_raw</imageTopicName>
          <cameraInfoTopicName>/kinect1/camera/color/camera_info</cameraInfoTopicName>
          <depthImageTopicName>/kinect1/camera/depth/image_raw</depthImageTopicName>
          <depthImageCameraInfoTopicName>/kinect1/camera/depth/camera_info</depthImageCameraInfoTopicName>
          <pointCloudTopicName>/kinect1/camera/depth/points</pointCloudTopicName>
          <frameName>kinect1_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>
```

kinectには
* `model.sdk`
* `model-1_2.sdk`
* `model-1_3.sdk`
* `model-1_4.sdk`

があるが、わかりやすいように
* `model1.sdk`
* `model2.sdk`
* `model3.sdk`
* `model4.sdk`

と変更して，今回は3つkinectを使うため，`model3.sdf`まで同じようにpluginを追加．
それぞれのsdfの中の`<model name>`を`kinect1,kinect2,kinect3`と変更．

## 作業手順
* GitHubリポジトリのクローン
```bash
$ cd ~/catkin_ws/src
$ git clone https://github.com/UkyoTaniguchi/kinect_coordinate_transformation.git
```

* 依存関係のインストールとビルド
```bash
$ cd ~/catkin_ws
$ rosdep install --from-paths src --ignore-src -r -y
$ catkin build
```

* 環境設定
```bash
$ source devel/setup.bash
```
* パッケージの実行と可視化
```bash
$ roslaunch my_camera_package start_camera.launch
$ rviz
```

GazeboとRVizが起動する．

1. Gazeboにオブジェクトを設置
今回は球体のオブジェクトを設置.
![gazebo1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3828823/55c8212d-5f91-18cb-ecc3-f93bd258d633.png)

1. RVizのFixed Frameでworldを選択
kinectの親フレームのworldを選択．
![RViz1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3828823/7fc6daa9-0fdd-0396-ea6a-c31b48c300e6.png)

1. Add→Bytopicからkinect1/camera/depth/points/PointCloud2を選択
kinect2とkinect3も同様にPointCloud2を追加
![RViz2.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3828823/dba80a49-6c5d-b2b6-37a1-0ae98c7ebf72.png)

## 結果
下記のように座標変換を行い、複数のKinectを追加することができた．![RViz3.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/3828823/436aa62e-86cb-dd8e-538d-9700c46c07cb.png)

パッケージのGitHub：[kinect_coordinate_transformation](https://github.com/UkyoTaniguchi/kinect_coordinate_transformation)

## 参考
[ROS Depth Camera Integration](https://classic.gazebosim.org/tutorials/?tut=ros_depth_camera)
[ROSでシミュレーション上の3Dデータを取得する](https://zenn.dev/naonaorange/articles/20200418_ros_sim)