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

# はじめに

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



- 使用したRealSense

RealSense D455

https://www.switch-science.com/products/6430



# 内蔵IMUの仕様について

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

- BMI055

https://www.mouser.jp/datasheet/2/783/BST_BMI055_DS000-1509583.pdf


- BMI085

https://www.mouser.jp/datasheet/2/783/BST_BMI085_DS001-1509577.pdf


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

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



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

![rviz_screenshot_2023_12_15-15_05_12.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/c37c38e5-9f69-07b9-83ef-dc33e845dcb3.png)



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

```bash
ros2 launch realsense2_camera rs_launch.py  enable_accel:=true
```

```bash
ros2 run rqt_plot rqt_plot 
```

- 静止状態

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

![1.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/6a355c5c-f6b3-5955-d3f2-83f3c9eb3296.png)

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

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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/e320602c-2242-2949-9f03-733355e45030.png)


# 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を参照。

https://github.com/IntelRealSense/realsense-ros


```bash
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を使って姿勢情報を計算する。

```bash
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について


https://wiki.ros.org/imu_filter_madgwick

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

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


## imu_filter_madgwickで姿勢を計算する

imu_filter_madgwickのインストール

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

realsense2_cameraの実行

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

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


```bash
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で回転を見る

```bash
rviz2
```


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

![image.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/8e81d501-d5e0-16df-0fb1-300a6f0c4b18.png)


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


![rviz_screenshot_2023_12_16-21_33_01.png](https://qiita-image-store.s3.ap-northeast-1.amazonaws.com/0/399606/3fafaaa2-e477-2c7b-63d8-7ce07afd733d.png)




# 参考

https://github.com/introlab/rtabmap_ros/blob/humble-devel/rtabmap_examples/launch/realsense_d435i_color.launch.py


https://tattatatakemori.hatenablog.com/entry/2018/06/24/140422

https://qiita.com/fumiya_sato/items/df28dff5ccc653183235


https://github.com/CCNYRoboticsLab/imu_tools/tree/humble