From 295fc01481422ac939f288bfcdbf03d07c666089 Mon Sep 17 00:00:00 2001 From: SakodaShintaro Date: Thu, 27 Feb 2025 17:36:13 +0900 Subject: [PATCH] feat: add `compare_trajectories.py` and `extract_pose_from_rosbag.py` (#216) * Added compare_trajectories.py and extract_pose_from_rosbag.py Signed-off-by: Shintaro Sakoda * style(pre-commit): autofix * Rewrite as a literal Signed-off-by: Shintaro Sakoda * Added "cspell: ignore rotvec" Signed-off-by: Shintaro Sakoda --------- Signed-off-by: Shintaro Sakoda Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> --- .../CMakeLists.txt | 2 + .../README.md | 58 ++++ .../scripts/compare_trajectories.py | 138 +++++++++ .../scripts/extract_pose_from_rosbag.py | 43 +++ .../scripts/utils/calc_relative_pose.py | 64 +++++ .../scripts/utils/interpolate_pose.py | 73 +++++ .../scripts/utils/parse_functions.py | 261 ++++++++++++++++++ 7 files changed, 639 insertions(+) create mode 100755 localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py create mode 100755 localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py create mode 100644 localization/autoware_localization_evaluation_scripts/scripts/utils/calc_relative_pose.py create mode 100644 localization/autoware_localization_evaluation_scripts/scripts/utils/interpolate_pose.py create mode 100644 localization/autoware_localization_evaluation_scripts/scripts/utils/parse_functions.py diff --git a/localization/autoware_localization_evaluation_scripts/CMakeLists.txt b/localization/autoware_localization_evaluation_scripts/CMakeLists.txt index 1dfb9180..69f9597c 100644 --- a/localization/autoware_localization_evaluation_scripts/CMakeLists.txt +++ b/localization/autoware_localization_evaluation_scripts/CMakeLists.txt @@ -5,6 +5,8 @@ find_package(autoware_cmake REQUIRED) autoware_package() install(PROGRAMS + scripts/compare_trajectories.py + scripts/extract_pose_from_rosbag.py scripts/plot_diagnostics.py DESTINATION lib/${PROJECT_NAME} ) diff --git a/localization/autoware_localization_evaluation_scripts/README.md b/localization/autoware_localization_evaluation_scripts/README.md index 035c892a..95d43db9 100644 --- a/localization/autoware_localization_evaluation_scripts/README.md +++ b/localization/autoware_localization_evaluation_scripts/README.md @@ -59,3 +59,61 @@ $HOME/driving_log_replayer_v2/out/latest/diagnostics_result [Example : ndt_scan_matcher__scan_matching_status.png] ![ndt_scan_matcher__scan_matching_status.png](./media/ndt_scan_matcher__scan_matching_status.png) + +## extract_pose_from_rosbag.py + +```bash +ros2 run autoware_localization_evaluation_scripts extract_pose_from_rosbag.py \ + \ + --save_dir=/your/path (default:rosbag_path/../) +``` + +[Example] + +```bash +$ ros2 run autoware_localization_evaluation_scripts extract_pose_from_rosbag.py \ + $HOME/driving_log_replayer_v2/out/latest/result_bag \ + --target_topics "/localization/kinematic_state" \ + "/localization/pose_estimator/pose_with_covariance" +``` + +This command outputs tsv files for each pose. + +The file names are the topic names with “/” replaced with “\_\_”. + +```bash +$ tree $HOME/driving_log_replayer_v2/out/latest/pose_tsv +$HOME/driving_log_replayer_v2/out/latest/pose_tsv +├── localization__kinematic_state.tsv +└── localization__pose_estimator__pose_with_covariance.tsv + +0 directories, 2 files +``` + +## compare_trajectories.py + +```bash +ros2 run autoware_localization_evaluation_scripts compare_trajectories.py \ + +``` + +[Example] + +```bash +$ ros2 run autoware_localization_evaluation_scripts compare_trajectories.py \ + $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state.tsv \ + $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__pose_estimator__pose_with_covariance.tsv +``` + +This command outputs tsv files for each pose. + +```bash +$ tree $HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state_result +$HOME/driving_log_replayer_v2/out/latest/pose_tsv/localization__kinematic_state_result +├── compare_trajectories.png +├── relative_pose.png +├── relative_pose.tsv +└── relative_pose_summary.tsv + +0 directories, 4 files +``` diff --git a/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py b/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py new file mode 100755 index 00000000..10ab5dd4 --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/compare_trajectories.py @@ -0,0 +1,138 @@ +#!/usr/bin/env python3 +"""A script to compare two trajectories.""" + +import argparse +from pathlib import Path + +import matplotlib.pyplot as plt +import pandas as pd +from utils.calc_relative_pose import calc_relative_pose +from utils.interpolate_pose import interpolate_pose + + +def parse_args() -> argparse.Namespace: + parser = argparse.ArgumentParser() + parser.add_argument("subject_tsv", type=Path) + parser.add_argument("reference_tsv", type=Path) + return parser.parse_args() + + +if __name__ == "__main__": + args = parse_args() + subject_tsv = args.subject_tsv + reference_tsv = args.reference_tsv + + result_name = subject_tsv.stem + save_dir = subject_tsv.parent / f"{result_name}_result" + save_dir.mkdir(parents=True, exist_ok=True) + + df_sub = pd.read_csv(subject_tsv, sep="\t") + df_ref = pd.read_csv(reference_tsv, sep="\t") + + # plot + plt.plot(df_sub["position.x"], df_sub["position.y"], label="subject") + plt.plot(df_ref["position.x"], df_ref["position.y"], label="reference") + plt.legend() + plt.axis("equal") + plt.xlabel("x [m]") + plt.ylabel("y [m]") + plt.savefig( + f"{save_dir}/compare_trajectories.png", + bbox_inches="tight", + pad_inches=0.05, + dpi=300, + ) + plt.close() + + # sort by timestamp + df_sub = df_sub.sort_values(by="timestamp") + df_ref = df_ref.sort_values(by="timestamp") + + # interpolate + timestamp = df_sub["timestamp"] + ok_mask = (timestamp > df_ref["timestamp"].min()) * (timestamp < df_ref["timestamp"].max()) + df_sub = df_sub[ok_mask] + timestamp = timestamp[ok_mask] + df_ref = interpolate_pose(df_ref, timestamp) + + # reset index + df_sub = df_sub.reset_index(drop=True) + df_ref = df_ref.reset_index(drop=True) + + 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) + + x_diff_mean = df_relative["position.x"].abs().mean() + y_diff_mean = df_relative["position.y"].abs().mean() + z_diff_mean = df_relative["position.z"].abs().mean() + angle_x_diff_mean = df_relative["angle.x"].abs().mean() + angle_y_diff_mean = df_relative["angle.y"].abs().mean() + angle_z_diff_mean = df_relative["angle.z"].abs().mean() + error_norm = df_relative["position.norm"] + df_summary = pd.DataFrame( + { + "x_diff_mean": [x_diff_mean], + "y_diff_mean": [y_diff_mean], + "z_diff_mean": [z_diff_mean], + "error_mean": [error_norm.mean()], + "roll_diff_mean": [angle_x_diff_mean], + "pitch_diff_mean": [angle_y_diff_mean], + "yaw_diff_mean": [angle_z_diff_mean], + }, + ) + df_summary.to_csv( + f"{save_dir}/relative_pose_summary.tsv", + sep="\t", + index=False, + float_format="%.4f", + ) + print(f"mean error: {error_norm.mean():.3f} m") + + plot_target_list = ["position", "angle"] + GUIDELINE_POSITION = 0.5 # [m] + GUIDELINE_ANGLE = 0.5 # [degree] + + for i, plot_target in enumerate(plot_target_list): + plt.subplot(2, 1, i + 1) + plt.plot(df_relative[f"{plot_target}.x"], label="x") + plt.plot(df_relative[f"{plot_target}.y"], label="y") + plt.plot(df_relative[f"{plot_target}.z"], label="z") + guide = GUIDELINE_POSITION if plot_target == "position" else GUIDELINE_ANGLE + unit = "degree" if plot_target == "angle" else "m" + plt.plot( + [0, len(df_relative)], + [guide, guide], + linestyle="dashed", + color="red", + label=f"guideline = {guide} [{unit}]", + ) + plt.plot( + [0, len(df_relative)], + [-guide, -guide], + linestyle="dashed", + color="red", + ) + bottom, top = plt.ylim() + plt.ylim(bottom=min(bottom, -guide * 2), top=max(top, guide * 2)) + plt.legend(loc="upper left", bbox_to_anchor=(1, 1)) + plt.xlabel("frame number") + plt.ylabel(f"relative {plot_target} [{unit}]") + plt.grid() + plt.tight_layout() + plt.savefig( + f"{save_dir}/relative_pose.png", + bbox_inches="tight", + pad_inches=0.05, + dpi=300, + ) + print(f"saved to {save_dir}/relative_pose.png") + plt.close() 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 new file mode 100755 index 00000000..80e2bac7 --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/extract_pose_from_rosbag.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 +"""A script to extract pose from rosbag and save as tsv.""" + +import argparse +from pathlib import Path + +from utils.parse_functions import parse_rosbag + + +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("--target_topics", type=str, required=True, nargs="+") + 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 + + if save_dir is None: + if rosbag_path.is_dir(): # if specified directory containing db3 files + save_dir = rosbag_path.parent / "pose_tsv" + else: # if specified db3 file directly + save_dir = rosbag_path.parent.parent / "pose_tsv" + save_dir.mkdir(parents=True, exist_ok=True) + + df_dict = parse_rosbag(str(rosbag_path), target_topics) + + for target_topic in target_topics: + save_name = "__".join(target_topic.split("/")[1:]) + df = df_dict[target_topic] + df.to_csv( + f"{save_dir}/{save_name}.tsv", + index=False, + sep="\t", + float_format="%.9f", + ) + + print(f"Saved pose tsv files to {save_dir}") diff --git a/localization/autoware_localization_evaluation_scripts/scripts/utils/calc_relative_pose.py b/localization/autoware_localization_evaluation_scripts/scripts/utils/calc_relative_pose.py new file mode 100644 index 00000000..81a40cef --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/utils/calc_relative_pose.py @@ -0,0 +1,64 @@ +"""A script to compare two pose_lists.""" + +import numpy as np +import pandas as pd +from scipy.spatial.transform import Rotation + +# cspell: ignore rotvec + + +def calc_relative_pose(df_prd: pd.DataFrame, df_ref: pd.DataFrame) -> pd.DataFrame: + """Calculate the relative position and orientation of df_prd with respect to df_ref.""" + position_keys = ["position.x", "position.y", "position.z"] + orientation_keys = [ + "orientation.x", + "orientation.y", + "orientation.z", + "orientation.w", + ] + assert len(df_prd) == len(df_ref) + + df_relative = df_prd.copy() + + # Calculate the relative orientation + rotation_prd = Rotation.from_quat(df_prd[orientation_keys].values) + rotation_ref = Rotation.from_quat(df_ref[orientation_keys].values) + df_relative[orientation_keys] = (rotation_prd * rotation_ref.inv()).as_quat() + + # Calculate the relative position + df_relative[position_keys] = df_prd[position_keys].to_numpy() - df_ref[position_keys].to_numpy() + # Rotate the relative position of each frame by rotation_true.inv() + # This makes the relative position based on the pose of df_ref + df_relative[position_keys] = rotation_ref.inv().apply( + df_relative[position_keys].values, + ) + + # Add norm + df_relative["position.norm"] = np.linalg.norm( + df_relative[position_keys].values, + axis=1, + ) + + # Add rpy angle + r = Rotation.from_quat( + df_relative[["orientation.x", "orientation.y", "orientation.z", "orientation.w"]], + ) + euler = r.as_euler("xyz", degrees=True) + df_relative["angle.x"] = euler[:, 0] + df_relative["angle.y"] = euler[:, 1] + df_relative["angle.z"] = euler[:, 2] + df_relative["angle.norm"] = np.linalg.norm(r.as_rotvec(), axis=1) + + # Arrange the order of columns + return df_relative[ + [ + "timestamp", + *position_keys, + "position.norm", + *orientation_keys, + "angle.x", + "angle.y", + "angle.z", + "angle.norm", + ] + ] diff --git a/localization/autoware_localization_evaluation_scripts/scripts/utils/interpolate_pose.py b/localization/autoware_localization_evaluation_scripts/scripts/utils/interpolate_pose.py new file mode 100644 index 00000000..573ebf7f --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/utils/interpolate_pose.py @@ -0,0 +1,73 @@ +"""A script to interpolate poses to match the timestamp in target_timestamp.""" + +import pandas as pd +from scipy.spatial.transform import Rotation +from scipy.spatial.transform import Slerp + + +def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.DataFrame: + """Interpolate each pose in df_pose to match the timestamp in target_timestamp.""" + # check monotonicity + assert df_pose["timestamp"].is_monotonic_increasing + assert target_timestamp.is_monotonic_increasing + + # check length + assert len(df_pose) > 0, f"{len(df_pose)=}" + assert len(target_timestamp) > 0, f"{len(target_timestamp)=}" + + # check range + assert df_pose.iloc[0]["timestamp"] <= target_timestamp.iloc[0] + assert target_timestamp.iloc[-1] <= df_pose.iloc[-1]["timestamp"] + + position_keys = ["position.x", "position.y", "position.z"] + orientation_keys = [ + "orientation.x", + "orientation.y", + "orientation.z", + "orientation.w", + ] + + df_pose = df_pose.reset_index(drop=True) + target_timestamp = target_timestamp.reset_index(drop=True) + + target_index = 0 + df_index = 0 + data_dict = { + "timestamp": [], + **{key: [] for key in position_keys}, + **{key: [] for key in orientation_keys}, + } + while df_index < len(df_pose) - 1 and target_index < len(target_timestamp): + curr_time = df_pose.iloc[df_index]["timestamp"] + next_time = df_pose.iloc[df_index + 1]["timestamp"] + target_time = target_timestamp[target_index] + + # Find a df_index that includes target_time + if not (curr_time <= target_time <= next_time): + df_index += 1 + continue + + curr_weight = (next_time - target_time) / (next_time - curr_time) + next_weight = 1.0 - curr_weight + + curr_position = df_pose.iloc[df_index][position_keys] + next_position = df_pose.iloc[df_index + 1][position_keys] + target_position = curr_position * curr_weight + next_position * next_weight + + curr_orientation = df_pose.iloc[df_index][orientation_keys] + next_orientation = df_pose.iloc[df_index + 1][orientation_keys] + curr_r = Rotation.from_quat(curr_orientation) + next_r = Rotation.from_quat(next_orientation) + slerp = Slerp([curr_time, next_time], Rotation.concatenate([curr_r, next_r])) + target_orientation = slerp([target_time]).as_quat()[0] + + data_dict["timestamp"].append(target_time) + data_dict[position_keys[0]].append(target_position[0]) + data_dict[position_keys[1]].append(target_position[1]) + data_dict[position_keys[2]].append(target_position[2]) + data_dict[orientation_keys[0]].append(target_orientation[0]) + data_dict[orientation_keys[1]].append(target_orientation[1]) + data_dict[orientation_keys[2]].append(target_orientation[2]) + data_dict[orientation_keys[3]].append(target_orientation[3]) + target_index += 1 + return pd.DataFrame(data_dict) diff --git a/localization/autoware_localization_evaluation_scripts/scripts/utils/parse_functions.py b/localization/autoware_localization_evaluation_scripts/scripts/utils/parse_functions.py new file mode 100644 index 00000000..4ce43f87 --- /dev/null +++ b/localization/autoware_localization_evaluation_scripts/scripts/utils/parse_functions.py @@ -0,0 +1,261 @@ +"""The library to parse the data from rosbag file.""" + +from collections import defaultdict +from pathlib import Path +import sys + +from cv_bridge import CvBridge +import pandas as pd +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message + + +def parse_rosbag(rosbag_dir: str, target_topic_list: list[str], limit: int = 0) -> 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=rosbag_dir, storage_id=storage_id) + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format, + ) + + reader = rosbag2_py.SequentialReader() + reader.open(storage_options, converter_options) + + topic_types = reader.get_all_topics_and_types() + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} + + storage_filter = rosbag2_py.StorageFilter(topics=target_topic_list) + reader.set_filter(storage_filter) + + topic_name_to_data = defaultdict(list) + parse_num = 0 + while reader.has_next(): + (topic, data, t) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + if topic in target_topic_list: + topic_name_to_data[topic].append(parse_msg(msg, msg_type)) + parse_num += 1 + if limit > 0 and parse_num >= limit: + break + for key in target_topic_list: + topic_name_to_data[key] = pd.DataFrame(topic_name_to_data[key]) + print(f"{key}: {len(topic_name_to_data[key])} msgs") + return topic_name_to_data + + +def parse_stamp(stamp): + return stamp.sec * int(1e9) + stamp.nanosec + + +def parse_msg(msg, msg_type): + class_name = msg_type.__class__.__name__.replace("Metaclass_", "") + try: + # parse_ + クラス名 の関数を動的に取得して実行 + parser = globals()[f"parse_{class_name}"] + return parser(msg) + except KeyError: + print(f"Error: {class_name} is not supported.") + sys.exit(0) + + +def parse_PoseStamped(msg): + return { + "timestamp": parse_stamp(msg.header.stamp), + "position.x": msg.pose.position.x, + "position.y": msg.pose.position.y, + "position.z": msg.pose.position.z, + "orientation.x": msg.pose.orientation.x, + "orientation.y": msg.pose.orientation.y, + "orientation.z": msg.pose.orientation.z, + "orientation.w": msg.pose.orientation.w, + } + + +def parse_PoseWithCovarianceStamped(msg): + return { + "timestamp": parse_stamp(msg.header.stamp), + "position.x": msg.pose.pose.position.x, + "position.y": msg.pose.pose.position.y, + "position.z": msg.pose.pose.position.z, + "orientation.x": msg.pose.pose.orientation.x, + "orientation.y": msg.pose.pose.orientation.y, + "orientation.z": msg.pose.pose.orientation.z, + "orientation.w": msg.pose.pose.orientation.w, + "covariance_position.xx": msg.pose.covariance[0], + "covariance_position.xy": msg.pose.covariance[1], + "covariance_position.xz": msg.pose.covariance[2], + "covariance_position.yx": msg.pose.covariance[6], + "covariance_position.yy": msg.pose.covariance[7], + "covariance_position.yz": msg.pose.covariance[8], + "covariance_position.zx": msg.pose.covariance[12], + "covariance_position.zy": msg.pose.covariance[13], + "covariance_position.zz": msg.pose.covariance[14], + "covariance_angle.x": msg.pose.covariance[21], + "covariance_angle.y": msg.pose.covariance[28], + "covariance_angle.z": msg.pose.covariance[35], + } + + +def parse_TwistWithCovarianceStamped(msg): + return { + "timestamp": parse_stamp(msg.header.stamp), + "linear_velocity.x": msg.twist.twist.linear.x, + "linear_velocity.y": msg.twist.twist.linear.y, + "linear_velocity.z": msg.twist.twist.linear.z, + "angular_velocity.x": msg.twist.twist.angular.x, + "angular_velocity.y": msg.twist.twist.angular.y, + "angular_velocity.z": msg.twist.twist.angular.z, + "covariance_position.xx": msg.twist.covariance[0], + "covariance_position.xy": msg.twist.covariance[1], + "covariance_position.xz": msg.twist.covariance[2], + "covariance_position.yx": msg.twist.covariance[6], + "covariance_position.yy": msg.twist.covariance[7], + "covariance_position.yz": msg.twist.covariance[8], + "covariance_position.zx": msg.twist.covariance[12], + "covariance_position.zy": msg.twist.covariance[13], + "covariance_position.zz": msg.twist.covariance[14], + "covariance_angle.x": msg.twist.covariance[21], + "covariance_angle.y": msg.twist.covariance[28], + "covariance_angle.z": msg.twist.covariance[35], + } + + +def parse_Odometry(msg): + return { + "timestamp": parse_stamp(msg.header.stamp), + "position.x": msg.pose.pose.position.x, + "position.y": msg.pose.pose.position.y, + "position.z": msg.pose.pose.position.z, + "orientation.x": msg.pose.pose.orientation.x, + "orientation.y": msg.pose.pose.orientation.y, + "orientation.z": msg.pose.pose.orientation.z, + "orientation.w": msg.pose.pose.orientation.w, + "covariance_position.xx": msg.pose.covariance[0], + "covariance_position.xy": msg.pose.covariance[1], + "covariance_position.xz": msg.pose.covariance[2], + "covariance_position.yx": msg.pose.covariance[6], + "covariance_position.yy": msg.pose.covariance[7], + "covariance_position.yz": msg.pose.covariance[8], + "covariance_position.zx": msg.pose.covariance[12], + "covariance_position.zy": msg.pose.covariance[13], + "covariance_position.zz": msg.pose.covariance[14], + "covariance_angle.x": msg.pose.covariance[21], + "covariance_angle.y": msg.pose.covariance[28], + "covariance_angle.z": msg.pose.covariance[35], + "linear_velocity.x": msg.twist.twist.linear.x, + "linear_velocity.y": msg.twist.twist.linear.y, + "linear_velocity.z": msg.twist.twist.linear.z, + "angular_velocity.x": msg.twist.twist.angular.x, + "angular_velocity.y": msg.twist.twist.angular.y, + "angular_velocity.z": msg.twist.twist.angular.z, + } + + +def parse_Float32Stamped(msg): + return { + "timestamp": parse_stamp(msg.stamp), + "data": msg.data, + } + + +def parse_Float64Stamped(msg): + return { + "timestamp": parse_stamp(msg.stamp), + "data": msg.data, + } + + +def parse_Int32Stamped(msg): + return { + "timestamp": parse_stamp(msg.stamp), + "data": msg.data, + } + + +def parse_MarkerArray(msg): + result_dict = {} + result_dict["timestamp"] = parse_stamp(msg.markers[0].header.stamp) + result_dict["marker"] = [] + for marker_msg in msg.markers: + one_marker = {} + one_marker["timestamp"] = parse_stamp(marker_msg.header.stamp) + one_marker["position.x"] = marker_msg.pose.position.x + one_marker["position.y"] = marker_msg.pose.position.y + one_marker["position.z"] = marker_msg.pose.position.z + one_marker["orientation.x"] = marker_msg.pose.orientation.x + one_marker["orientation.y"] = marker_msg.pose.orientation.y + one_marker["orientation.z"] = marker_msg.pose.orientation.z + one_marker["orientation.w"] = marker_msg.pose.orientation.w + result_dict["marker"].append(one_marker) + return result_dict + + +def parse_Image(msg): + image = CvBridge().imgmsg_to_cv2(msg, desired_encoding="passthrough") + return { + "timestamp": parse_stamp(msg.header.stamp), + "frame_id": msg.header.frame_id, + "image": image, + } + + +def parse_CompressedImage(msg): + image = CvBridge().compressed_imgmsg_to_cv2(msg, desired_encoding="passthrough") + return { + "timestamp": parse_stamp(msg.header.stamp), + "frame_id": msg.header.frame_id, + "image": image, + } + + +def parse_CameraInfo(msg): + return { + "timestamp": parse_stamp(msg.header.stamp), + "frame_id": msg.header.frame_id, + "width": msg.width, + "height": msg.height, + "D": msg.d, + "K": msg.k, + "R": msg.r, + "P": msg.p, + } + + +def parse_TFMessage(msg): + return { + "transforms": msg.transforms, + } + + +def parse_Imu(msg): + return { + "timestamp": parse_stamp(msg.header.stamp), + "frame_id": msg.header.frame_id, + "angular_velocity.x": msg.angular_velocity.x, + "angular_velocity.y": msg.angular_velocity.y, + "angular_velocity.z": msg.angular_velocity.z, + "linear_acceleration.x": msg.linear_acceleration.x, + "linear_acceleration.y": msg.linear_acceleration.y, + "linear_acceleration.z": msg.linear_acceleration.z, + "orientation.x": msg.orientation.x, + "orientation.y": msg.orientation.y, + "orientation.z": msg.orientation.z, + "orientation.w": msg.orientation.w, + } + + +def parse_VelocityReport(msg): + return { + "timestamp": parse_stamp(msg.header.stamp), + "longitudinal_velocity": msg.longitudinal_velocity, + "lateral_velocity": msg.lateral_velocity, + "heading_rate": msg.heading_rate, + }