feat: add analyze_rosbags_parallel.py (#217)

* Fixed storage_id

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

* Added analyze_rosbags_parallel.sh

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

* style(pre-commit): autofix

* Rewrote in python

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

* style(pre-commit): autofix

* Fixed README.md

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

* Create a main function of plot_diagnostics.py

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

* style(pre-commit): autofix

* Created a main function of extract_pose_from_rosbag.py

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

* Created a main function of compare_trajectories.py

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

* Removed unused variables

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

* Fixed topic_reference

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

* Updated README.md

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

* style(pre-commit): autofix

* Updated README.md

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

* Added "and not d.is_symlink()"

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

* style(pre-commit): autofix

* Removed a storage option

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

@@ -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

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 \
    <rosbag_path> \
-   --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 \
+   <result_dir> --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.

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],
+        )

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)

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)

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)