imu_to_pose6D
IMU・AHRS の姿勢のクォータニオンを、機種によらず pose6D にします。
処理内容
- クォータニオンを正規化して回転行列にし、ロール・ピッチ・ヨーにします（R = Rz(yaw) Ry(pitch) Rx(roll)）。
- reference_frame = NED のときは、ワールド ENU・機体 FLU に直します。ロールは同じ符号、ピッチは逆の符号で、yaw = 90° − 方位です。
- 出力のタイムスタンプは入力と同じです。
- ここではセンサーフュージョンをしません。姿勢を出すセンサか、`imu_orientation_estimator` を使ってください。
データ形式
- quaternion は 4 要素です。ほかの要素数ではエラーで止まります。
- pose6D は [0, 0, 0, roll, pitch, yaw]（m、rad）です。IMU では位置を測れないので、並進は常に 0 です。
注意事項
- 有限でないサンプルと、ノルムが 1e-6 より小さいサンプルは飛ばします。警告は 1 回だけ出します。
- 出力は IMU 自身の姿勢です。IMU の取り付けは、姿勢を使う側で Sensor Calibration File Parser の行列を当ててください。
- ヨーの基準は機種によります。6 軸のセンサはヨーがドリフトします。
参考文献
- [1] T. Foote and M. Purvis, REP 103: Standard Units of Measure and Coordinate Conventions, ROS Enhancement Proposals. https://www.ros.org/reps/rep-0103.html — 使っている機能：ワールド ENU と機体 FLU の軸、単位。
- [2] REP 145: Conventions for IMU Sensor Drivers, ROS Enhancement Proposals. https://www.ros.org/reps/rep-0145.html — 使っている機能：IMU のドライバが出す姿勢の約束。
- [3] sensor_msgs/Imu message definition, ROS common_msgs. https://docs.ros.org/en/api/sensor_msgs/html/msg/Imu.html — 使っている機能：クォータニオンの並び xyzw。
プロパティ
| 名前 | 既定値 | 説明 |
|---|---|---|
quaternion_order | wxyz | 要素の並び：wxyz（スカラーが先頭）、または xyzw（スカラーが末尾。ROS の sensor_msgs/Imu と同じ）。選択肢: wxyz, xyzw。 |
reference_frame | ENU | 入力の座標：ENU（ワールド ENU、機体 FLU。ROS と同じ）、または NED（ワールド NED、機体 FRD）。選択肢: ENU, NED。 |
出力
| 名前 | 説明 |
|---|---|
pose6D | IMU の姿勢：[0, 0, 0, roll, pitch, yaw]（m、rad）。 型: Float64 |
入力
| 名前 | 説明 |
|---|---|
quaternion | 姿勢のクォータニオン（4 要素）。並びは quaternion_order で決まります。 型: Float64 |