diff --git a/localization/autoware_localization_evaluation_scripts/CMakeLists.txt b/localization/autoware_localization_evaluation_scripts/CMakeLists.txt index 69f9597c..bdfb5ea9 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.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 95d43db9..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] @@ -117,3 +116,22 @@ $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state_ 0 directories, 4 files ``` + +## analyze_rosbags_parallel.py + +```bash +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 performs the above three analyses on the subdirectories of the target directory. 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..40eb190c --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/analyze_rosbags_parallel.py @@ -0,0 +1,64 @@ +#!/usr/bin/env python3 +"""A script to analyze rosbags in parallel.""" + +import argparse +from multiprocessing import Pool +from pathlib import Path + +import compare_trajectories +import extract_pose_from_rosbag +import plot_diagnostics + + +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, 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=[ + topic_subject, + topic_reference, + ], + save_dir=compare_result_dir, + ) + + compare_trajectories.main( + subject_tsv=compare_result_dir / f"{save_name_subject}.tsv", + reference_tsv=compare_result_dir / f"{save_name_reference}.tsv", + ) + + +if __name__ == "__main__": + 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() and not d.is_symlink()] + ) + + with Pool(args.parallel_num) as pool: + pool.starmap( + process_directory, + [(d, topic_subject, topic_reference) for d in directories], + ) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py b/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py index 10ab5dd4..c58a4092 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) @@ -61,12 +57,6 @@ def parse_args() -> argparse.Namespace: 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) @@ -136,3 +126,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) 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..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,12 +15,11 @@ 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 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 save_dir = rosbag_path.parent / "pose_tsv" @@ -31,7 +30,7 @@ def parse_args() -> argparse.Namespace: 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", @@ -41,3 +40,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) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py b/localization/autoware_localization_evaluation_scripts/scripts/plot_diagnostics.py index 82eac299..5c017ed5 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,17 @@ 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, @@ -67,12 +72,7 @@ def parse_diagnostics_msgs(rosbag_path: str, target_list: list, storage: str) -> return data_dict -if __name__ == "__main__": - args = parse_args() - rosbag_path = args.rosbag_path - save_dir = args.save_dir - storage = args.storage - +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" @@ -87,7 +87,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=}") @@ -303,3 +303,10 @@ def parse_diagnostics_msgs(rosbag_path: str, target_list: list, storage: str) -> 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)