feat: add autoware_localization_evaluation_scripts (#215)

* Added autoware_localization_evaluation_scripts

Signed-off-by: Shintaro Sakoda <[email protected]>

* Added a linebreak

Signed-off-by: Shintaro Sakoda <[email protected]>

* style(pre-commit): autofix

* Added a storage option

Signed-off-by: Shintaro Sakoda <[email protected]>

* Added options in README.md

Signed-off-by: Shintaro Sakoda <[email protected]>

* Added ylabel("gyro_bias [rad/s]")

Signed-off-by: Shintaro Sakoda <[email protected]>

---------

Signed-off-by: Shintaro Sakoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>

localization/autoware_localization_evaluation_scripts/CMakeLists.txt

@@ -0,0 +1,12 @@
+cmake_minimum_required(VERSION 3.5)
+project(autoware_localization_evaluation_scripts)
+
+find_package(autoware_cmake REQUIRED)
+autoware_package()
+
+install(PROGRAMS
+    scripts/plot_diagnostics.py
+    DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_auto_package()

localization/autoware_localization_evaluation_scripts/README.md

@@ -0,0 +1,61 @@
+# autoware_localization_evaluation_scripts
+
+This package contains some scripts for evaluating the localization of Autoware.
+
+Ths scripts are used for the result rosbags of localization, particularly the result_bag from `driving_log_replayer_v2`.
+
+<https://tier4.github.io/driving_log_replayer_v2/quick_start/setup/>
+
+As a test, execute `driving_log_replayer_v2` with the following command:
+
+```bash
+ros2 launch driving_log_replayer_v2 driving_log_replayer_v2.launch.py \
+    scenario_path:=$HOME/driving_log_replayer_v2/localization.yaml
+```
+
+Then, use the scripts for processing the result_bag:
+
+```bash
+$HOME/driving_log_replayer_v2/out/latest/result_bag
+```
+
+## plot_diagnostics.py
+
+```bash
+ros2 run autoware_localization_evaluation_scripts plot_diagnostics.py \
+   <rosbag_path> \
+   --save_dir=/your/path (default:rosbag_path/../) \
+   --storage=mcap (default:sqlite3)
+```
+
+[Example]
+
+```bash
+$ ros2 run autoware_localization_evaluation_scripts plot_diagnostics.py \
+    $HOME/driving_log_replayer_v2/out/latest/result_bag
+[INFO] [1740561002.740748735] [rosbag2_storage]: Opened database '$HOME/driving_log_replayer_v2/out/latest/result_bag/result_bag_0.db3' for READ_ONLY.
+save_dir=PosixPath('$HOME/driving_log_replayer_v2/out/latest/diagnostics_result')
+```
+
+This command outputs each diagnostic (tsv) and plot result (png).
+
+```bash
+$ tree $HOME/driving_log_replayer_v2/out/latest/diagnostics_result
+$HOME/driving_log_replayer_v2/out/latest/diagnostics_result
+├── gyro_bias_validator__gyro_bias_validator.png
+├── gyro_bias_validator__gyro_bias_validator.tsv
+├── localization__ekf_localizer.png
+├── localization__ekf_localizer.tsv
+├── localization__pose_instability_detector.png
+├── localization__pose_instability_detector.tsv
+├── localization_error_monitor__ellipse_error_status.png
+├── localization_error_monitor__ellipse_error_status.tsv
+├── ndt_scan_matcher__scan_matching_status.png
+└── ndt_scan_matcher__scan_matching_status.tsv
+
+0 directories, 10 files
+```
+
+[Example : ndt_scan_matcher__scan_matching_status.png]
+
+![ndt_scan_matcher__scan_matching_status.png](./media/ndt_scan_matcher__scan_matching_status.png)

localization/autoware_localization_evaluation_scripts/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>autoware_localization_evaluation_scripts</name>
+  <version>0.1.0</version>
+  <description>A package for evaluation localization</description>
+  <maintainer email="[email protected]">Yamato Ando</maintainer>
+  <maintainer email="[email protected]">Taiki Yamada</maintainer>
+  <maintainer email="[email protected]">Shintaro Sakoda</maintainer>
+  <maintainer email="[email protected]">Anh Nguyen</maintainer>
+  <maintainer email="[email protected]">Masahiro Sakamoto</maintainer>
+  <maintainer email="[email protected]">Hayato Mizushima</maintainer>
+  <license>Apache License 2.0</license>
+
+  <author email="[email protected]">Shintaro Sakoda</author>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+  <buildtool_depend>autoware_cmake</buildtool_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>