feat(pose_instability_detector): add pose_instability_detector module

[wakabame]

Description

本PRでは自己位置推定はずれ検知を行う pose_instability_detector を導入します

pose_instability_detectorは以下の機能を提供します；

1. 自己位置推定と速度推定から、以下の２つのオドメトリを算出して比較します
   • 最新の自己位置
   • 以前の（デフォルトでは0.5秒前）自己位置に、速度推定どおりに進んだと仮定したときの現時点の推定自己位置
2. パラメータにより算出されるしきい値を元にして、上記の比較結果についてOKとWARNを判定し、診断トピックに出力する

Related links

Parent Issue: https://tier4.atlassian.net/browse/AEAP-1118

• Link
  • 仕様資料
  • 管理チケット: https://tier4.atlassian.net/browse/AEAP-1707

How was this PR tested?

pose_instability_detector.launch.xml を用いた pose_instability_detector の単体起動を行いました

ros2 launch pose_instability_detector pose_instability_detector.launch.xml \
  input_odometry:="/localization/kinematic_state" \ 
  input_twist:="/localization/twist_estimator/twist_with_covariance"

機能性能確認

性能評価資料 にて示されている性能値と上記LSim結果としての推定ズレ量がおよそ一致することを確認しました。
性能評価資料にて利用したROSBAGを再生し、自己位置推定のずれ量が最大値となった瞬間のずれ量を比較しています。

※以下検証用のグラフは横軸の時間の絶対値が毎計測ごとにずれてしまう。入力の波形をもとに比較時刻を合わせます

入力データ（比較参照用）

性能評価資料では150 sec 近辺で自己位置推定ずれが最大となっていたが、今回試験では入力波形から 210 sec 近辺 が該当することがわかります

位置推定ずれ比較データ

リグレッション確認

• autoware.universe に対しては新機能の追加であるため、行わない

妥当性確認

• 推定ズレ量（縦方向・横方向）が診断トピックに対して出力されていることを確認しました
  • 検証欄を参照してください

検証

• 診断トピックへの出力が外部仕様通りになっていることを確認しました

  • output_x_y_yaw_only=trueの場合

    ---
    header:
      stamp:
        sec: 1716360264
        nanosec: 420329912
      frame_id: ''
    status:
    - level: "\0"
      name: 'localization: pose_instability_detector'
      message: OK
      hardware_id: pose_instability_detector
      values:
      - key: diff_position_x:threshold
        value: '0.151670'
      - key: diff_position_x:value
        value: '-0.001912'
      - key: diff_position_x:status
        value: OK
      - key: diff_position_y:threshold
        value: '0.118271'
      - key: diff_position_y:value
        value: '0.000052'
      - key: diff_position_y:status
        value: OK
      - key: diff_angle_z:threshold
        value: '0.021513'
      - key: diff_angle_z:value
        value: '0.000004'
      - key: diff_angle_z:status
        value: OK
    ---
    

  • output_x_y_yaw_only=falseの場合

    ---
    header:
      stamp:
        sec: 1716360262
        nanosec: 818272333
      frame_id: ''
    status:
    - level: "\0"
      name: 'localization: pose_instability_detector'
      message: OK
      hardware_id: pose_instability_detector
      values:
      - key: diff_position_x:threshold
        value: '0.151670'
      - key: diff_position_x:value
        value: '0.001953'
      - key: diff_position_x:status
        value: OK
      - key: diff_position_y:threshold
        value: '0.118271'
      - key: diff_position_y:value
        value: '0.000026'
      - key: diff_position_y:status
        value: OK
      - key: diff_position_z:threshold
        value: '0.118271'
      - key: diff_position_z:value
        value: '0.002381'
      - key: diff_position_z:status
        value: OK
      - key: diff_angle_x:threshold
        value: '0.021513'
      - key: diff_angle_x:value
        value: '-0.000074'
      - key: diff_angle_x:status
        value: OK
      - key: diff_angle_y:threshold
        value: '0.021513'
      - key: diff_angle_y:value
        value: '0.000032'
      - key: diff_angle_y:status
        value: OK
      - key: diff_angle_z:threshold
        value: '0.021513'
      - key: diff_angle_z:value
        value: '0.000002'
      - key: diff_angle_z:status
        value: OK
    ---
    

• ログ出力が仕様どおりであること

  • ログ出力が仕様通りに行われていることを、Lsim 起動直後の INFO, WARN ログから確認しました

    $ ros2 launch pose_instability_detector pose_instability_detector.launch.xml input_odometry:="/localization/kinematic_state" input_twist:="/localization/twist_estimator/twist_with_covariance"
    [INFO] [launch]: All log files can be found below /home/yukikimura/.ros/log/2024-09-24-01-10-23-577157-yukikimura-Z6I9G70SRACC-46636
    [INFO] [launch]: Default logging verbosity is set to INFO
    [INFO] [pose_instability_detector_node-1]: process started with pid [46639]
    [pose_instability_detector_node-1] [INFO] [1727107823.704945450] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [WARN] [1727107823.705013274] [pose_instability_detector]: pose was nullopt
    [pose_instability_detector_node-1] [INFO] [1727107823.726127271] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.744762455] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.774736960] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.788346424] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.805840730] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.827734059] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.844516067] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.864740594] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.885638121] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.904777062] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.927568044] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.943975186] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.975074333] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107823.991593926] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.008766280] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.027575377] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.044699570] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.064798945] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.084591556] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.112399964] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.128221387] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.144129090] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.174658484] [pose_instability_detector]: target_time must be within the pose_buffer_!
    [pose_instability_detector_node-1] [INFO] [1727107824.189117118] [pose_instability_detector]: target_time must be within the pose_buffer_!
    

Notes for reviewers

• 検証項目詳細は https://tier4.atlassian.net/wiki/spaces/EP1/pages/3328901197 に記載しています

• この機能はOSSにある pose_instability_detector 実装をベースに下記の差分が含まれます

  • OSS最新にあるautoware::universe_utilsライブラリのinverseTransformPose関数取り込み（参照）
    • ※最新のライブラリを利用できないため
  • x, y, yawのずれのみに限定して出力するモードを追加（参照）
  • パラメータ heading_velocity_maximum のデフォルト値はOSS版の 16.667 [m/s] から 2.778 [m/s] = 10 [km/h] に値を変更している（参照）

Interface changes

None.

Effects on system behavior

None.

[TaikiYamada4]

@yn-mrse @wakabame

ご指摘された点は大丈夫そうに見えます！ 👍
input/pose と json schema まわりの修正漏れはお手数おかけしました。

[sonarcloud]

Quality Gate passed

Issues
2 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarCloud

[wakabame]

@yn-mrse
こちら、commit suggestion 後のソースに対して再度ビルドを行い、topic の出力を確認しました。診断トピックの出力まで確認できたので、マージします。