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

はじめに

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

環境

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

項目	値
Ubuntu	20.04
ROS	Noetic
CUDA	11.4

座標変換

GazeboとRVizで表示される内容が異なる原因は、RVizのワールド座標系とKinectのセンサ座標系があっていないことが考えられる．

• ワールド座標系
  ワールド座標系はコンピュータ上で3次元空間全体を定義する座標系．
• センサ座標系
  センサ座標系はその名前の通り，センサ上に設定された座標系．そのため、センサの動きとともに座標系が変化していく．

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

座標変換ファイル

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の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リポジトリのクローン

$ cd ~/catkin_ws/src
$ git clone https://github.com/UkyoTaniguchi/kinect_coordinate_transformation.git

• 依存関係のインストールとビルド

$ cd ~/catkin_ws
$ rosdep install --from-paths src --ignore-src -r -y
$ catkin build

• 環境設定

$ source devel/setup.bash

• パッケージの実行と可視化

$ roslaunch kinect_coordinate_transformation start_camera.launch
$ rviz

GazeboとRVizが起動する．

1. Gazeboにオブジェクトを設置
   今回は球体のオブジェクトを設置.
   

2. RVizのFixed Frameでworldを選択
   kinectの親フレームのworldを選択．
   

3. Add→Bytopicからkinect1/camera/depth/points/PointCloud2を選択
   kinect2とkinect3も同様にPointCloud2を追加
   

結果

下記のように座標変換を行い、複数のKinectを追加することができた．

パッケージのGitHub：kinect_coordinate_transformation

参考

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