ROS 2でRealSenseの内蔵IMUを使用する

はじめに

RealSenseの内蔵6軸IMUをROS2で使用する手順についてまとめる。

• 使用したRealSense

RealSense D455

内蔵IMUの仕様について

IMUがついている機種はD435i, D455, D456, D457の4機種
D435iとD455にはBoschのBMI055搭載、D456とD457はBMI085搭載。
以下に各IMUのデータシートのリンクを貼ったので詳しい仕様は以下を参照のこと。

• BMI055

• BMI085

内蔵IMUのデータの座標について

RealSenseから取得する加速度、角速度のデータはカメラ座標系と同じになっていることに注意。

以下rvizでcamera_linkとIMUデータのframe_id：camera_imu_optical_frameを表示した時の画像（下に緑が向いているのがcamera_imu_optical_frame）。
tf上でIMUのframeが回転しているのでrtabmap-ros などに入力するときは座標の回転を気にせずそのままIMUのトピックを入力して良い（Subscribeする側でtfで座標変換処理をしてくれる）

実際にrqt_plotで加速度のデータを見てみる。

ros2 launch realsense2_camera rs_launch.py  enable_accel:=true

ros2 run rqt_plot rqt_plot 

• 静止状態

Y（緑）のデータが- 重力加速度

• 手で真横（camera_linkのY軸方向）に動かしたとき

他の軸も動いてしまっているが、X（赤）が変化している。

ROS2でRealSenseの内蔵IMUを使用する

enable_gyro, enable_accel, unite_imu_methodの3つのパラメータを設定することでIMUトピックが使えるようになる。
enable_gyro, enable_accelを両方Trueにし、unite_imu_methodを1か2にすると/camera/imuという名前のsensor_msgs/msg/Imu型のトピックが出力される。
パラメータの詳細はrealsense-rosのREADMEを参照。

ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1

以下はros2 topic echo /camera/imuした時のトピックの中身、姿勢情報となるorientationのデータがなくこのままでは使いにくいので、次にimu_filter_madgwickを使って姿勢情報を計算する。

header:
  stamp:
    sec: 1702628265
    nanosec: 338649344
  frame_id: camera_imu_optical_frame
orientation:
  x: 0.0
  y: 0.0
  z: 0.0
  w: 0.0
orientation_covariance:
- -1.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
- 0.0
angular_velocity:
  x: -0.001745329238474369
  y: -0.003490658476948738
  z: 0.0
angular_velocity_covariance:
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01
linear_acceleration:
  x: 0.2059396505355835
  y: -8.865211486816406
  z: 0.9512450098991394
linear_acceleration_covariance:
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01
- 0.0
- 0.0
- 0.0
- 0.01

imu_filter_madgwickについて

加速度、ジャイロ、地磁気センサのデータからMadgwickフィルターで姿勢（角度）を推定して出力するROSパッケージ。

Madgwickフィルターの詳細については参考の記事を参照。

imu_filter_madgwickで姿勢を計算する

imu_filter_madgwickのインストール

sudo apt install ros-humble-imu-filter-madgwick

realsense2_cameraの実行

ros2 launch realsense2_camera rs_launch.py enable_gyro:=true enable_accel:=true unite_imu_method:=1

imu_filter_madgwickの実行、トピック名は元のimuトピックと区別するために/imu/data_rawにする。

ros2 run imu_filter_madgwick imu_filter_madgwick_node --ros-args -p use_mag:=False -p world_frame:=enu -p publish_tf:=False -r imu/data_raw:=/camera/imu

rviz2で回転を見る

rviz2

Fixed Frameを/camera_linkにしてADDからrviz_imu_pluginのimuを選択しトピック名を/imu/data_rawにする。

RealSense本体を回転させてrviz2の軸を回転していれば成功。

参考