From 231496c900c56eddf8d799f9b5c72cf9cebc8e85 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:04:56 +0900 Subject: [PATCH 01/18] Fixed storage_id Signed-off-by: Shintaro Sakoda --- .../scripts/plot_diagnostics.py | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py b/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py index 82eac299..e8bc9a86 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py @@ -16,7 +16,6 @@ def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser() parser.add_argument("rosbag_path", type=Path) parser.add_argument("--save_dir", type=Path, default=None) - parser.add_argument("--storage", type=str, default="sqlite3", choices=["sqlite3", "mcap"]) return parser.parse_args() @@ -28,11 +27,19 @@ def diag_name_to_filename(diag_name: str) -> str: return diag_name.replace(":", "_").replace(" ", "_") -def parse_diagnostics_msgs(rosbag_path: str, target_list: list, storage: str) -> dict: +def parse_diagnostics_msgs(rosbag_dir: str, target_list: list) -> dict: serialization_format = "cdr" + storage_id = None + if len(list(Path(rosbag_dir).rglob("*.db3"))) > 0: + storage_id = "sqlite3" + elif len(list(Path(rosbag_dir).rglob("*.mcap"))) > 0: + storage_id = "mcap" + assert ( + storage_id is not None + ), f"Error: {rosbag_dir} is not a valid rosbag directory." storage_options = rosbag2_py.StorageOptions( - uri=str(rosbag_path), - storage_id=storage, + uri=str(rosbag_dir), + storage_id=storage_id, ) converter_options = rosbag2_py.ConverterOptions( input_serialization_format=serialization_format, @@ -71,7 +78,6 @@ def parse_diagnostics_msgs(rosbag_path: str, target_list: list, storage: str) -> args = parse_args() rosbag_path = args.rosbag_path save_dir = args.save_dir - storage = args.storage if save_dir is None: if rosbag_path.is_dir(): # if specified directory containing db3 files @@ -87,7 +93,7 @@ def parse_diagnostics_msgs(rosbag_path: str, target_list: list, storage: str) -> "gyro_bias_validator: gyro_bias_validator", ] - data_dict = parse_diagnostics_msgs(rosbag_path, target_list, storage) + data_dict = parse_diagnostics_msgs(rosbag_path, target_list) save_dir.mkdir(exist_ok=True) print(f"{save_dir=}") From bef418f0aa3a9d0ccd456b26ceca1a9fe212637a Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:05:12 +0900 Subject: [PATCH 02/18] Added analyze_rosbags_parallel.sh Signed-off-by: Shintaro Sakoda --- .../CMakeLists.txt | 1 + .../README.md | 7 +++++ .../scripts/analyze_rosbags_parallel.sh | 30 +++++++++++++++++++ 3 files changed, 38 insertions(+) create mode 100755 localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh diff --git a/localization/autoware_localization_evaluation_scripts/CMakeLists.txt b/localization/autoware_localization_evaluation_scripts/CMakeLists.txt index 69f9597c..befcd0d2 100644 --- a/localization/autoware_localization_evaluation_scripts/CMakeLists.txt +++ b/localization/autoware_localization_evaluation_scripts/CMakeLists.txt @@ -5,6 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() install(PROGRAMS + scripts/analyze_rosbags_parallel.sh scripts/compare_trajectories.py scripts/extract_pose_from_rosbag.py scripts/plot_diagnostics.py diff --git a/localization/autoware_localization_evaluation_scripts/README.md b/localization/autoware_localization_evaluation_scripts/README.md index 95d43db9..bbfcc2c0 100644 --- a/localization/autoware_localization_evaluation_scripts/README.md +++ b/localization/autoware_localization_evaluation_scripts/README.md @@ -117,3 +117,10 @@ $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state_ 0 directories, 4 files ``` + +## analyze_rosbags_parallel.sh + +```bash +ros2 run autoware_localization_evaluation_scripts analyze_rosbags_parallel.sh \ + +``` diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh new file mode 100755 index 00000000..78c76459 --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh @@ -0,0 +1,30 @@ +#!/bin/bash +set -ux + +result_dir=$1 +parallelism=${2:-1} + +process_directory() { + local dir=$1 + + target_rosbag="$dir/result_bag" + compare_result_dir="$dir/compare_trajectories" + mkdir -p "$compare_result_dir" + + ros2 run autoware_localization_evaluation_scripts plot_diagnostics.py "$target_rosbag" + + ros2 run autoware_localization_evaluation_scripts extract_pose_from_rosbag.py \ + "$target_rosbag" \ + --save_dir "$compare_result_dir" \ + --target_topics "/localization/kinematic_state" \ + "/localization/reference_kinematic_state" + + ros2 run autoware_localization_evaluation_scripts compare_trajectories.py \ + "$compare_result_dir/localization__kinematic_state.tsv" \ + "$compare_result_dir/localization__reference_kinematic_state.tsv" +} + +export -f process_directory + +find "$result_dir" -mindepth 1 -maxdepth 1 -type d | sort | \ + xargs -I {} -P "$parallelism" bash -c 'process_directory "{}"' From b55051e56bae03c9a8a0210c07cd9d85b21e5edf Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Feb 2025 09:06:41 +0000 Subject: [PATCH 03/18] style(pre-commit): autofix --- .../scripts/analyze_rosbags_parallel.sh | 30 +++++++++---------- .../scripts/plot_diagnostics.py | 4 +-- 2 files changed, 16 insertions(+), 18 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh index 78c76459..63e33091 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh @@ -5,26 +5,26 @@ result_dir=$1 parallelism=${2:-1} process_directory() { - local dir=$1 + local dir=$1 - target_rosbag="$dir/result_bag" - compare_result_dir="$dir/compare_trajectories" - mkdir -p "$compare_result_dir" + target_rosbag="$dir/result_bag" + compare_result_dir="$dir/compare_trajectories" + mkdir -p "$compare_result_dir" - ros2 run autoware_localization_evaluation_scripts plot_diagnostics.py "$target_rosbag" + ros2 run autoware_localization_evaluation_scripts plot_diagnostics.py "$target_rosbag" - ros2 run autoware_localization_evaluation_scripts extract_pose_from_rosbag.py \ - "$target_rosbag" \ - --save_dir "$compare_result_dir" \ - --target_topics "/localization/kinematic_state" \ - "/localization/reference_kinematic_state" + ros2 run autoware_localization_evaluation_scripts extract_pose_from_rosbag.py \ + "$target_rosbag" \ + --save_dir "$compare_result_dir" \ + --target_topics "/localization/kinematic_state" \ + "/localization/reference_kinematic_state" - ros2 run autoware_localization_evaluation_scripts compare_trajectories.py \ - "$compare_result_dir/localization__kinematic_state.tsv" \ - "$compare_result_dir/localization__reference_kinematic_state.tsv" + ros2 run autoware_localization_evaluation_scripts compare_trajectories.py \ + "$compare_result_dir/localization__kinematic_state.tsv" \ + "$compare_result_dir/localization__reference_kinematic_state.tsv" } export -f process_directory -find "$result_dir" -mindepth 1 -maxdepth 1 -type d | sort | \ - xargs -I {} -P "$parallelism" bash -c 'process_directory "{}"' +find "$result_dir" -mindepth 1 -maxdepth 1 -type d | sort | + xargs -I {} -P "$parallelism" bash -c 'process_directory "{}"' diff --git a/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py b/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py index e8bc9a86..ad0d6ad1 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py @@ -34,9 +34,7 @@ def parse_diagnostics_msgs(rosbag_dir: str, target_list: list) -> dict: storage_id = "sqlite3" elif len(list(Path(rosbag_dir).rglob("*.mcap"))) > 0: storage_id = "mcap" - assert ( - storage_id is not None - ), f"Error: {rosbag_dir} is not a valid rosbag directory." + assert storage_id is not None, f"Error: {rosbag_dir} is not a valid rosbag directory." storage_options = rosbag2_py.StorageOptions( uri=str(rosbag_dir), storage_id=storage_id, From a19ba8a5fb61791a6c3fb50ec3e80ba916d58b21 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:18:56 +0900 Subject: [PATCH 04/18] Rewrote in python Signed-off-by: Shintaro Sakoda --- .../CMakeLists.txt | 2 +- .../README.md | 2 +- .../scripts/analyze_rosbags_parallel.py | 70 +++++++++++++++++++ .../scripts/analyze_rosbags_parallel.sh | 30 -------- 4 files changed, 72 insertions(+), 32 deletions(-) create mode 100755 localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py delete mode 100755 localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh diff --git a/localization/autoware_localization_evaluation_scripts/CMakeLists.txt b/localization/autoware_localization_evaluation_scripts/CMakeLists.txt index befcd0d2..bdfb5ea9 100644 --- a/localization/autoware_localization_evaluation_scripts/CMakeLists.txt +++ b/localization/autoware_localization_evaluation_scripts/CMakeLists.txt @@ -5,7 +5,7 @@ find_package(autoware_cmake REQUIRED) autoware_package() install(PROGRAMS - scripts/analyze_rosbags_parallel.sh + scripts/analyze_rosbags_parallel.py scripts/compare_trajectories.py scripts/extract_pose_from_rosbag.py scripts/plot_diagnostics.py diff --git a/localization/autoware_localization_evaluation_scripts/README.md b/localization/autoware_localization_evaluation_scripts/README.md index bbfcc2c0..a6f2af98 100644 --- a/localization/autoware_localization_evaluation_scripts/README.md +++ b/localization/autoware_localization_evaluation_scripts/README.md @@ -121,6 +121,6 @@ $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state_ ## analyze_rosbags_parallel.sh ```bash -ros2 run autoware_localization_evaluation_scripts analyze_rosbags_parallel.sh \ +ros2 run autoware_localization_evaluation_scripts analyze_rosbags_parallel.py \ ``` diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py new file mode 100755 index 00000000..817d1405 --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -0,0 +1,70 @@ +#!/usr/bin/env python3 +"""A script to analyze rosbags in parallel.""" + +import argparse +import subprocess +from pathlib import Path +from multiprocessing import Pool + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("result_dir", type=Path) + parser.add_argument("--parallel_num", type=int, default=1) + return parser.parse_args() + + +def process_directory(directory: Path): + target_rosbag = directory / "result_bag" + compare_result_dir = directory / "compare_trajectories" + compare_result_dir.mkdir(parents=True, exist_ok=True) + + subprocess.run( + [ + "ros2", + "run", + "autoware_localization_evaluation_scripts", + "plot_diagnostics.py", + str(target_rosbag), + ], + check=True, + ) + + subprocess.run( + [ + "ros2", + "run", + "autoware_localization_evaluation_scripts", + "extract_pose_from_rosbag.py", + str(target_rosbag), + "--save_dir", + str(compare_result_dir), + "--target_topics", + "/localization/kinematic_state", + "/localization/reference_kinematic_state", + ], + check=True, + ) + + subprocess.run( + [ + "ros2", + "run", + "autoware_localization_evaluation_scripts", + "compare_trajectories.py", + str(compare_result_dir / "localization__kinematic_state.tsv"), + str(compare_result_dir / "localization__reference_kinematic_state.tsv"), + ], + check=True, + ) + + +if __name__ == "__main__": + args = parse_args() + result_dir = args.result_dir + parallel_num = args.parallel_num + + directories = sorted([d for d in args.result_dir.iterdir() if d.is_dir()]) + + with Pool(args.parallel_num) as pool: + pool.map(process_directory, directories) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh deleted file mode 100755 index 63e33091..00000000 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.sh +++ /dev/null @@ -1,30 +0,0 @@ -#!/bin/bash -set -ux - -result_dir=$1 -parallelism=${2:-1} - -process_directory() { - local dir=$1 - - target_rosbag="$dir/result_bag" - compare_result_dir="$dir/compare_trajectories" - mkdir -p "$compare_result_dir" - - ros2 run autoware_localization_evaluation_scripts plot_diagnostics.py "$target_rosbag" - - ros2 run autoware_localization_evaluation_scripts extract_pose_from_rosbag.py \ - "$target_rosbag" \ - --save_dir "$compare_result_dir" \ - --target_topics "/localization/kinematic_state" \ - "/localization/reference_kinematic_state" - - ros2 run autoware_localization_evaluation_scripts compare_trajectories.py \ - "$compare_result_dir/localization__kinematic_state.tsv" \ - "$compare_result_dir/localization__reference_kinematic_state.tsv" -} - -export -f process_directory - -find "$result_dir" -mindepth 1 -maxdepth 1 -type d | sort | - xargs -I {} -P "$parallelism" bash -c 'process_directory "{}"' From 098d9f747392728ed1707732c52482f43ee55b8d Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Feb 2025 09:19:40 +0000 Subject: [PATCH 05/18] style(pre-commit): autofix --- .../scripts/analyze_rosbags_parallel.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index 817d1405..9dbe2ae2 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -2,9 +2,9 @@ """A script to analyze rosbags in parallel.""" import argparse -import subprocess -from pathlib import Path from multiprocessing import Pool +from pathlib import Path +import subprocess def parse_args() -> argparse.Namespace: From d22cc081aa229389d66d3cb9d6791493f0a1ded4 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:20:35 +0900 Subject: [PATCH 06/18] Fixed README.md Signed-off-by: Shintaro Sakoda --- .../autoware_localization_evaluation_scripts/README.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/README.md b/localization/autoware_localization_evaluation_scripts/README.md index a6f2af98..33cdf8c4 100644 --- a/localization/autoware_localization_evaluation_scripts/README.md +++ b/localization/autoware_localization_evaluation_scripts/README.md @@ -118,9 +118,9 @@ $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state_ 0 directories, 4 files ``` -## analyze_rosbags_parallel.sh +## analyze_rosbags_parallel.py ```bash ros2 run autoware_localization_evaluation_scripts analyze_rosbags_parallel.py \ - + --parallel_num 2 ``` From 5ae490d39d87df0a0a00c9a706d4c2c6a7111da0 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:27:56 +0900 Subject: [PATCH 07/18] Create a main function of plot_diagnostics.py Signed-off-by: Shintaro Sakoda --- .../scripts/analyze_rosbags_parallel.py | 12 ++---------- .../scripts/plot_diagnostics.py | 13 ++++++++----- 2 files changed, 10 insertions(+), 15 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index 9dbe2ae2..2776b70f 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -5,6 +5,7 @@ from multiprocessing import Pool from pathlib import Path import subprocess +import plot_diagnostics def parse_args() -> argparse.Namespace: @@ -19,16 +20,7 @@ def process_directory(directory: Path): compare_result_dir = directory / "compare_trajectories" compare_result_dir.mkdir(parents=True, exist_ok=True) - subprocess.run( - [ - "ros2", - "run", - "autoware_localization_evaluation_scripts", - "plot_diagnostics.py", - str(target_rosbag), - ], - check=True, - ) + plot_diagnostics.main(rosbag_path=target_rosbag) subprocess.run( [ diff --git a/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py b/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py index ad0d6ad1..5c017ed5 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py @@ -72,11 +72,7 @@ def parse_diagnostics_msgs(rosbag_dir: str, target_list: list) -> dict: return data_dict -if __name__ == "__main__": - args = parse_args() - rosbag_path = args.rosbag_path - save_dir = args.save_dir - +def main(rosbag_path: Path, save_dir: Path = None) -> None: if save_dir is None: if rosbag_path.is_dir(): # if specified directory containing db3 files save_dir = rosbag_path.parent / "diagnostics_result" @@ -307,3 +303,10 @@ def parse_diagnostics_msgs(rosbag_dir: str, target_list: list) -> dict: plt.tight_layout() save_path = save_dir / f"{diag_name_to_filename(diag_name)}.png" plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) + + +if __name__ == "__main__": + args = parse_args() + rosbag_path = args.rosbag_path + save_dir = args.save_dir + main(rosbag_path, save_dir) From cb18d38fd4fb3d2b62a9afcb1b51c4ce407d3ecc Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Feb 2025 09:29:58 +0000 Subject: [PATCH 08/18] style(pre-commit): autofix --- .../scripts/analyze_rosbags_parallel.py | 1 + 1 file changed, 1 insertion(+) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index 2776b70f..0a4fde98 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -5,6 +5,7 @@ from multiprocessing import Pool from pathlib import Path import subprocess + import plot_diagnostics From 24c418ee89d84ef7a3620cd27be96273b02d5308 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:32:15 +0900 Subject: [PATCH 09/18] Created a main function of extract_pose_from_rosbag.py Signed-off-by: Shintaro Sakoda --- .../scripts/analyze_rosbags_parallel.py | 16 +++++----------- .../scripts/extract_pose_from_rosbag.py | 15 +++++++++------ 2 files changed, 14 insertions(+), 17 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index 0a4fde98..737f31e1 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -6,6 +6,7 @@ from pathlib import Path import subprocess +import extract_pose_from_rosbag import plot_diagnostics @@ -23,20 +24,13 @@ def process_directory(directory: Path): plot_diagnostics.main(rosbag_path=target_rosbag) - subprocess.run( - [ - "ros2", - "run", - "autoware_localization_evaluation_scripts", - "extract_pose_from_rosbag.py", - str(target_rosbag), - "--save_dir", - str(compare_result_dir), - "--target_topics", + extract_pose_from_rosbag.main( + rosbag_path=target_rosbag, + target_topics=[ "/localization/kinematic_state", "/localization/reference_kinematic_state", ], - check=True, + save_dir=compare_result_dir, ) subprocess.run( diff --git a/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py b/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py index 80e2bac7..2d82c5ed 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py @@ -15,12 +15,7 @@ def parse_args() -> argparse.Namespace: return parser.parse_args() -if __name__ == "__main__": - args = parse_args() - rosbag_path = args.rosbag_path - target_topics = args.target_topics - save_dir = args.save_dir - +def main(rosbag_path: Path, target_topics: list, save_dir: Path = None) -> None: if save_dir is None: if rosbag_path.is_dir(): # if specified directory containing db3 files save_dir = rosbag_path.parent / "pose_tsv" @@ -41,3 +36,11 @@ def parse_args() -> argparse.Namespace: ) print(f"Saved pose tsv files to {save_dir}") + + +if __name__ == "__main__": + args = parse_args() + rosbag_path = args.rosbag_path + target_topics = args.target_topics + save_dir = args.save_dir + main(rosbag_path, target_topics, save_dir) From 8b9cbc9eee29cb52b5256fccf632002038b4f90e Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:35:53 +0900 Subject: [PATCH 10/18] Created a main function of compare_trajectories.py Signed-off-by: Shintaro Sakoda --- .../scripts/analyze_rosbags_parallel.py | 15 ++++----------- .../scripts/compare_trajectories.py | 13 ++++++++----- 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index 737f31e1..ffed1728 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -4,8 +4,8 @@ import argparse from multiprocessing import Pool from pathlib import Path -import subprocess +import compare_trajectories import extract_pose_from_rosbag import plot_diagnostics @@ -33,16 +33,9 @@ def process_directory(directory: Path): save_dir=compare_result_dir, ) - subprocess.run( - [ - "ros2", - "run", - "autoware_localization_evaluation_scripts", - "compare_trajectories.py", - str(compare_result_dir / "localization__kinematic_state.tsv"), - str(compare_result_dir / "localization__reference_kinematic_state.tsv"), - ], - check=True, + compare_trajectories.main( + subject_tsv=compare_result_dir / "localization__kinematic_state.tsv", + reference_tsv=compare_result_dir / "localization__reference_kinematic_state.tsv", ) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py b/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py index 10ab5dd4..5455a678 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py @@ -17,11 +17,7 @@ def parse_args() -> argparse.Namespace: return parser.parse_args() -if __name__ == "__main__": - args = parse_args() - subject_tsv = args.subject_tsv - reference_tsv = args.reference_tsv - +def main(subject_tsv: Path, reference_tsv: Path) -> None: result_name = subject_tsv.stem save_dir = subject_tsv.parent / f"{result_name}_result" save_dir.mkdir(parents=True, exist_ok=True) @@ -136,3 +132,10 @@ def parse_args() -> argparse.Namespace: ) print(f"saved to {save_dir}/relative_pose.png") plt.close() + + +if __name__ == "__main__": + args = parse_args() + subject_tsv = args.subject_tsv + reference_tsv = args.reference_tsv + main(subject_tsv, reference_tsv) From 7ffbc36101c5495f5e9c23a5bdfc0842b480e930 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:37:35 +0900 Subject: [PATCH 11/18] Removed unused variables Signed-off-by: Shintaro Sakoda --- .../scripts/compare_trajectories.py | 6 ------ 1 file changed, 6 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py b/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py index 5455a678..c58a4092 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py @@ -57,12 +57,6 @@ def main(subject_tsv: Path, reference_tsv: Path) -> None: assert len(df_sub) == len(df_ref), f"len(df_pr)={len(df_sub)}, len(df_gt)={len(df_ref)}" - # calc mean error - diff_x = df_sub["position.x"].to_numpy() - df_ref["position.x"].to_numpy() - diff_y = df_sub["position.y"].to_numpy() - df_ref["position.y"].to_numpy() - diff_z = df_sub["position.z"].to_numpy() - df_ref["position.z"].to_numpy() - diff_meter = (diff_x**2 + diff_y**2 + diff_z**2) ** 0.5 - # calc relative pose df_relative = calc_relative_pose(df_sub, df_ref) df_relative.to_csv(f"{save_dir}/relative_pose.tsv", sep="\t", index=False) From 79aa0ec93d19d4b2dda8bb9d557361836a394adc Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:44:41 +0900 Subject: [PATCH 12/18] Fixed topic_reference Signed-off-by: Shintaro Sakoda --- .../scripts/analyze_rosbags_parallel.py | 22 ++++++++++++++----- .../scripts/extract_pose_from_rosbag.py | 6 ++++- 2 files changed, 21 insertions(+), 7 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index ffed1728..282d7839 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -14,28 +14,33 @@ def parse_args() -> argparse.Namespace: parser = argparse.ArgumentParser() parser.add_argument("result_dir", type=Path) parser.add_argument("--parallel_num", type=int, default=1) + parser.add_argument("--topic_subject", type=str, default="/localization/kinematic_state") + parser.add_argument("--topic_reference", type=str, default="/localization/pose_estimator/pose_with_covariance") return parser.parse_args() -def process_directory(directory: Path): +def process_directory(directory: Path, topic_subject: str, topic_reference: str) -> None: target_rosbag = directory / "result_bag" compare_result_dir = directory / "compare_trajectories" compare_result_dir.mkdir(parents=True, exist_ok=True) plot_diagnostics.main(rosbag_path=target_rosbag) + save_name_subject = extract_pose_from_rosbag.topic_name_to_save_name(topic_subject) + save_name_reference = extract_pose_from_rosbag.topic_name_to_save_name(topic_reference) + extract_pose_from_rosbag.main( rosbag_path=target_rosbag, target_topics=[ - "/localization/kinematic_state", - "/localization/reference_kinematic_state", + topic_subject, + topic_reference, ], save_dir=compare_result_dir, ) compare_trajectories.main( - subject_tsv=compare_result_dir / "localization__kinematic_state.tsv", - reference_tsv=compare_result_dir / "localization__reference_kinematic_state.tsv", + subject_tsv=compare_result_dir / f"{save_name_subject}.tsv", + reference_tsv=compare_result_dir / f"{save_name_reference}.tsv", ) @@ -43,8 +48,13 @@ def process_directory(directory: Path): args = parse_args() result_dir = args.result_dir parallel_num = args.parallel_num + topic_subject = args.topic_subject + topic_reference = args.topic_reference directories = sorted([d for d in args.result_dir.iterdir() if d.is_dir()]) with Pool(args.parallel_num) as pool: - pool.map(process_directory, directories) + pool.starmap( + process_directory, + [(d, topic_subject, topic_reference) for d in directories], + ) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py b/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py index 2d82c5ed..cdaf2ddd 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py @@ -15,6 +15,10 @@ def parse_args() -> argparse.Namespace: return parser.parse_args() +def topic_name_to_save_name(topic_name: str) -> str: + return "__".join(topic_name.split("/")[1:]) + + def main(rosbag_path: Path, target_topics: list, save_dir: Path = None) -> None: if save_dir is None: if rosbag_path.is_dir(): # if specified directory containing db3 files @@ -26,7 +30,7 @@ def main(rosbag_path: Path, target_topics: list, save_dir: Path = None) -> None: df_dict = parse_rosbag(str(rosbag_path), target_topics) for target_topic in target_topics: - save_name = "__".join(target_topic.split("/")[1:]) + save_name = topic_name_to_save_name(target_topic) df = df_dict[target_topic] df.to_csv( f"{save_dir}/{save_name}.tsv", From 9d61f15456c98faa9518cd0dd10ab46db9a7485e Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:48:05 +0900 Subject: [PATCH 13/18] Updated README.md Signed-off-by: Shintaro Sakoda --- .../README.md | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/localization/autoware_localization_evaluation_scripts/README.md b/localization/autoware_localization_evaluation_scripts/README.md index 33cdf8c4..b30d2bf2 100644 --- a/localization/autoware_localization_evaluation_scripts/README.md +++ b/localization/autoware_localization_evaluation_scripts/README.md @@ -124,3 +124,15 @@ $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state_ ros2 run autoware_localization_evaluation_scripts analyze_rosbags_parallel.py \ --parallel_num 2 ``` + +[Example] + +```bash +$ ros2 run autoware_localization_evaluation_scripts analyze_rosbags_parallel.py \ + $HOME/driving_log_replayer_v2/out/ \ + --parallel_num 2 \ + --topic_subject "/localization/kinematic_state" \ + --topic_reference "/localization/pose_estimator/pose_with_covariance" +``` + +This command execute above three analysis for the target directory. From a64480a549d684c24d40c70b633300ae61a15cc4 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 27 Feb 2025 09:50:30 +0000 Subject: [PATCH 14/18] style(pre-commit): autofix --- .../scripts/analyze_rosbags_parallel.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index 282d7839..862117b1 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -15,7 +15,9 @@ def parse_args() -> argparse.Namespace: parser.add_argument("result_dir", type=Path) parser.add_argument("--parallel_num", type=int, default=1) parser.add_argument("--topic_subject", type=str, default="/localization/kinematic_state") - parser.add_argument("--topic_reference", type=str, default="/localization/pose_estimator/pose_with_covariance") + parser.add_argument( + "--topic_reference", type=str, default="/localization/pose_estimator/pose_with_covariance" + ) return parser.parse_args() From e14d8dcebbbeb845aef1c805ffd3b91eac92325d Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 27 Feb 2025 18:53:34 +0900 Subject: [PATCH 15/18] Updated README.md Signed-off-by: Shintaro Sakoda --- localization/autoware_localization_evaluation_scripts/README.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/autoware_localization_evaluation_scripts/README.md b/localization/autoware_localization_evaluation_scripts/README.md index b30d2bf2..95f33160 100644 --- a/localization/autoware_localization_evaluation_scripts/README.md +++ b/localization/autoware_localization_evaluation_scripts/README.md @@ -135,4 +135,4 @@ $ ros2 run autoware_localization_evaluation_scripts analyze_rosbags_parallel.py --topic_reference "/localization/pose_estimator/pose_with_covariance" ``` -This command execute above three analysis for the target directory. +This command performs the above three analyses on the subdirectories of the target directory. From 40f6e426ed7170a6cbe5f2a13bc765e59d4b2b95 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Fri, 28 Feb 2025 09:59:55 +0900 Subject: [PATCH 16/18] Added "and not d.is_symlink()" Signed-off-by: Shintaro Sakoda --- .../scripts/analyze_rosbags_parallel.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index 862117b1..a20dab62 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -53,7 +53,7 @@ def process_directory(directory: Path, topic_subject: str, topic_reference: str) topic_subject = args.topic_subject topic_reference = args.topic_reference - directories = sorted([d for d in args.result_dir.iterdir() if d.is_dir()]) + directories = sorted([d for d in args.result_dir.iterdir() if d.is_dir() and not d.is_symlink()]) with Pool(args.parallel_num) as pool: pool.starmap( From f3f26eadb793ebb4e8fdd74d7e4c7cc438d86aa3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 28 Feb 2025 01:02:12 +0000 Subject: [PATCH 17/18] style(pre-commit): autofix --- .../scripts/analyze_rosbags_parallel.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py index a20dab62..40eb190c 100755 --- a/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -53,7 +53,9 @@ def process_directory(directory: Path, topic_subject: str, topic_reference: str) topic_subject = args.topic_subject topic_reference = args.topic_reference - directories = sorted([d for d in args.result_dir.iterdir() if d.is_dir() and not d.is_symlink()]) + directories = sorted( + [d for d in args.result_dir.iterdir() if d.is_dir() and not d.is_symlink()] + ) with Pool(args.parallel_num) as pool: pool.starmap( From abc8557efab791ae321d5313b80e93e5e09df96e Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Fri, 28 Feb 2025 10:04:03 +0900 Subject: [PATCH 18/18] Removed a storage option Signed-off-by: Shintaro Sakoda --- .../autoware_localization_evaluation_scripts/README.md | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/localization/autoware_localization_evaluation_scripts/README.md b/localization/autoware_localization_evaluation_scripts/README.md index 95f33160..d037da7b 100644 --- a/localization/autoware_localization_evaluation_scripts/README.md +++ b/localization/autoware_localization_evaluation_scripts/README.md @@ -24,8 +24,7 @@ $HOME/driving_log_replayer_v2/out/latest/result_bag ```bash ros2 run autoware_localization_evaluation_scripts plot_diagnostics.py \ \ - --save_dir=/your/path (default:rosbag_path/../) \ - --storage=mcap (default:sqlite3) + --save_dir=/your/path (default:rosbag_path/../) ``` [Example]