From 7f3a3a7f5e42b6675fffe54cdb69b56f9e82c87c Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 26 Feb 2024 16:39:12 +0900 Subject: [PATCH 01/22] Added convert_rosbag_for_ndt_evaluation.py Signed-off-by: Shintaro Sakoda --- .../convert_rosbag_for_ndt_evaluation.py | 133 ++++++++++++++++++ 1 file changed, 133 insertions(+) create mode 100644 localization/scripts/convert_rosbag_for_ndt_evaluation.py diff --git a/localization/scripts/convert_rosbag_for_ndt_evaluation.py b/localization/scripts/convert_rosbag_for_ndt_evaluation.py new file mode 100644 index 00000000..f7686e7d --- /dev/null +++ b/localization/scripts/convert_rosbag_for_ndt_evaluation.py @@ -0,0 +1,133 @@ +""" Convert rosbag for NDT evaluation. +(1) Check if the rosbag is suitable for ndt evaluation +(2) Filter topics +(3) Rename the topic name of the reference kinematic state to /localization/reference_kinematic_state +""" + +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +import argparse +import pathlib +import pandas as pd + + +REQUIRED_FPS = { + "/localization/kinematic_state": 40, + "/localization/util/downsample/pointcloud": 9, + "/sensing/vehicle_velocity_converter/twist_with_covariance": 20, + "/sensing/imu/imu_data": 20, + "/tf_static": 0, + "/sensing/gnss/pose_with_covariance": -1, # optional topic + "/initialpose": -1, # optional topic +} + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument('rosbag_path', type=pathlib.Path) + parser.add_argument('--reference_topic_name', type=str, + default='/localization/kinematic_state', + choices=['/localization/kinematic_state', '/awsim/ground_truth/localization/kinematic_state']) + return parser.parse_args() + + +def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: + """ Check if the rosbag is suitable for ndt evaluation """ + print(f"{duration=:.1f} sec") + + df = pd.DataFrame({ + "topic": list(topic_name_to_msg_list.keys()), + "count": [len(msg_list) for msg_list in topic_name_to_msg_list.values()], + }) + df["fps"] = df["count"] / duration + df["enough_fps"] = (df["fps"] > df["topic"].map(REQUIRED_FPS)) + print(df) + + # check + # All topics must have enough fps + assert df["enough_fps"].all(), \ + f"NG! FPS is not enough in {df[df['enough_fps'] == False]['topic'].values}" + + # Either /sensing/gnss/pose_with_covariance or /initialpose must be included. + assert len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) >= 1 or len(topic_name_to_msg_list["/initialpose"]) >= 1, \ + "NG! Neither /sensing/gnss/pose_with_covariance nor /initialpose is found." + + # [Warning] Vehicle should be stopping still for about the first 10 seconds + for msg in topic_name_to_msg_list["/localization/kinematic_state"]: + if msg.header.stamp.sec > 10: + break + twist = msg.twist.twist + ok = twist.linear.x < 0.1 and twist.linear.y < 0.1 and twist.linear.z < 0.1 \ + and twist.angular.x < 0.1 and twist.angular.y < 0.1 and twist.angular.z < 0.1 + if not ok: + print(f"Warning: Vehicle is not stopping. time = {msg.header.stamp.sec}") + break + + print("OK") + + +if __name__ == "__main__": + args = parse_args() + rosbag_path = args.rosbag_path + reference_topic_name = args.reference_topic_name + + # prepare option + serialization_format = 'cdr' + converter_options = rosbag2_py.ConverterOptions( + input_serialization_format=serialization_format, + output_serialization_format=serialization_format) + + # prepare reader + reader = rosbag2_py.SequentialReader() + storage_options = rosbag2_py.StorageOptions( + uri=str(rosbag_path), storage_id='sqlite3') + reader.open(storage_options, converter_options) + + # filter topics + target_topics = list(REQUIRED_FPS.keys()) + if reference_topic_name not in target_topics: + target_topics.append(reference_topic_name) + storage_filter = rosbag2_py.StorageFilter(topics=target_topics) + reader.set_filter(storage_filter) + + # get map of topic name and type + type_map = {} + for topic_type in reader.get_all_topics_and_types(): + if topic_type.name in target_topics: + type_map[topic_type.name] = topic_type.type + + # read rosbag + topic_name_to_msg_list = {topic: [] for topic in target_topics} + tuple_list = [] + while reader.has_next(): + (topic_name, data, timestamp_rosbag) = reader.read_next() + tuple_list.append((topic_name, data, timestamp_rosbag)) + + msg_type = get_message(type_map[topic_name]) + msg = deserialize_message(data, msg_type) + topic_name_to_msg_list[topic_name].append(msg) + duration = (tuple_list[-1][2] - tuple_list[0][2]) * 1e-9 + + # check + check_rosbag(duration, topic_name_to_msg_list) + + # write rosbag + rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent + filtered_rosbag_path = rosbag_dir / "input_bag" + writer = rosbag2_py.SequentialWriter() + storage_options = rosbag2_py.StorageOptions( + uri=str(filtered_rosbag_path), storage_id='sqlite3') + writer.open(storage_options, converter_options) + for topic_name, topic_type in type_map.items(): + if topic_name == reference_topic_name: + topic_name = '/localization/reference_kinematic_state' + topic_info = rosbag2_py.TopicMetadata( + name=topic_name, type=topic_type, serialization_format=serialization_format) + writer.create_topic(topic_info) + for topic_name, data, timestamp_rosbag in tuple_list: + if topic_name == reference_topic_name: + topic_name = '/localization/reference_kinematic_state' + writer.write(topic_name, data, timestamp_rosbag) + + print(f"rosbag is saved at {filtered_rosbag_path}") From f6299a3854f80fb4acd556b702cc84b99596d3d0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Feb 2024 08:04:38 +0000 Subject: [PATCH 02/22] style(pre-commit): autofix --- .../convert_rosbag_for_ndt_evaluation.py | 77 +++++++++++-------- 1 file changed, 47 insertions(+), 30 deletions(-) diff --git a/localization/scripts/convert_rosbag_for_ndt_evaluation.py b/localization/scripts/convert_rosbag_for_ndt_evaluation.py index f7686e7d..ddf4ebcc 100644 --- a/localization/scripts/convert_rosbag_for_ndt_evaluation.py +++ b/localization/scripts/convert_rosbag_for_ndt_evaluation.py @@ -4,13 +4,13 @@ (3) Rename the topic name of the reference kinematic state to /localization/reference_kinematic_state """ -import rosbag2_py -from rclpy.serialization import deserialize_message -from rosidl_runtime_py.utilities import get_message import argparse import pathlib -import pandas as pd +import pandas as pd +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message REQUIRED_FPS = { "/localization/kinematic_state": 40, @@ -19,47 +19,64 @@ "/sensing/imu/imu_data": 20, "/tf_static": 0, "/sensing/gnss/pose_with_covariance": -1, # optional topic - "/initialpose": -1, # optional topic + "/initialpose": -1, # optional topic } def parse_args(): parser = argparse.ArgumentParser() - parser.add_argument('rosbag_path', type=pathlib.Path) - parser.add_argument('--reference_topic_name', type=str, - default='/localization/kinematic_state', - choices=['/localization/kinematic_state', '/awsim/ground_truth/localization/kinematic_state']) + parser.add_argument("rosbag_path", type=pathlib.Path) + parser.add_argument( + "--reference_topic_name", + type=str, + default="/localization/kinematic_state", + choices=[ + "/localization/kinematic_state", + "/awsim/ground_truth/localization/kinematic_state", + ], + ) return parser.parse_args() def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: - """ Check if the rosbag is suitable for ndt evaluation """ + """Check if the rosbag is suitable for ndt evaluation""" print(f"{duration=:.1f} sec") - df = pd.DataFrame({ - "topic": list(topic_name_to_msg_list.keys()), - "count": [len(msg_list) for msg_list in topic_name_to_msg_list.values()], - }) + df = pd.DataFrame( + { + "topic": list(topic_name_to_msg_list.keys()), + "count": [len(msg_list) for msg_list in topic_name_to_msg_list.values()], + } + ) df["fps"] = df["count"] / duration - df["enough_fps"] = (df["fps"] > df["topic"].map(REQUIRED_FPS)) + df["enough_fps"] = df["fps"] > df["topic"].map(REQUIRED_FPS) print(df) # check # All topics must have enough fps - assert df["enough_fps"].all(), \ - f"NG! FPS is not enough in {df[df['enough_fps'] == False]['topic'].values}" + assert df[ + "enough_fps" + ].all(), f"NG! FPS is not enough in {df[df['enough_fps'] == False]['topic'].values}" # Either /sensing/gnss/pose_with_covariance or /initialpose must be included. - assert len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) >= 1 or len(topic_name_to_msg_list["/initialpose"]) >= 1, \ - "NG! Neither /sensing/gnss/pose_with_covariance nor /initialpose is found." + assert ( + len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) >= 1 + or len(topic_name_to_msg_list["/initialpose"]) >= 1 + ), "NG! Neither /sensing/gnss/pose_with_covariance nor /initialpose is found." # [Warning] Vehicle should be stopping still for about the first 10 seconds for msg in topic_name_to_msg_list["/localization/kinematic_state"]: if msg.header.stamp.sec > 10: break twist = msg.twist.twist - ok = twist.linear.x < 0.1 and twist.linear.y < 0.1 and twist.linear.z < 0.1 \ - and twist.angular.x < 0.1 and twist.angular.y < 0.1 and twist.angular.z < 0.1 + ok = ( + twist.linear.x < 0.1 + and twist.linear.y < 0.1 + and twist.linear.z < 0.1 + and twist.angular.x < 0.1 + and twist.angular.y < 0.1 + and twist.angular.z < 0.1 + ) if not ok: print(f"Warning: Vehicle is not stopping. time = {msg.header.stamp.sec}") break @@ -73,15 +90,15 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: reference_topic_name = args.reference_topic_name # prepare option - serialization_format = 'cdr' + serialization_format = "cdr" converter_options = rosbag2_py.ConverterOptions( input_serialization_format=serialization_format, - output_serialization_format=serialization_format) + output_serialization_format=serialization_format, + ) # prepare reader reader = rosbag2_py.SequentialReader() - storage_options = rosbag2_py.StorageOptions( - uri=str(rosbag_path), storage_id='sqlite3') + storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3") reader.open(storage_options, converter_options) # filter topics @@ -116,18 +133,18 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent filtered_rosbag_path = rosbag_dir / "input_bag" writer = rosbag2_py.SequentialWriter() - storage_options = rosbag2_py.StorageOptions( - uri=str(filtered_rosbag_path), storage_id='sqlite3') + storage_options = rosbag2_py.StorageOptions(uri=str(filtered_rosbag_path), storage_id="sqlite3") writer.open(storage_options, converter_options) for topic_name, topic_type in type_map.items(): if topic_name == reference_topic_name: - topic_name = '/localization/reference_kinematic_state' + topic_name = "/localization/reference_kinematic_state" topic_info = rosbag2_py.TopicMetadata( - name=topic_name, type=topic_type, serialization_format=serialization_format) + name=topic_name, type=topic_type, serialization_format=serialization_format + ) writer.create_topic(topic_info) for topic_name, data, timestamp_rosbag in tuple_list: if topic_name == reference_topic_name: - topic_name = '/localization/reference_kinematic_state' + topic_name = "/localization/reference_kinematic_state" writer.write(topic_name, data, timestamp_rosbag) print(f"rosbag is saved at {filtered_rosbag_path}") From c31d8ae507ea80cc355cf9ef212061313467a044 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 26 Feb 2024 17:14:00 +0900 Subject: [PATCH 03/22] Fixed comments Signed-off-by: Shintaro Sakoda --- localization/scripts/convert_rosbag_for_ndt_evaluation.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/localization/scripts/convert_rosbag_for_ndt_evaluation.py b/localization/scripts/convert_rosbag_for_ndt_evaluation.py index ddf4ebcc..d1ae32e7 100644 --- a/localization/scripts/convert_rosbag_for_ndt_evaluation.py +++ b/localization/scripts/convert_rosbag_for_ndt_evaluation.py @@ -1,4 +1,5 @@ -""" Convert rosbag for NDT evaluation. +"""Convert rosbag for NDT evaluation. + (1) Check if the rosbag is suitable for ndt evaluation (2) Filter topics (3) Rename the topic name of the reference kinematic state to /localization/reference_kinematic_state @@ -39,7 +40,7 @@ def parse_args(): def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: - """Check if the rosbag is suitable for ndt evaluation""" + """Check if the rosbag is suitable for ndt evaluation.""" print(f"{duration=:.1f} sec") df = pd.DataFrame( From 8c26f14972db559b9bc8b80877c88f05a0afa442 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 26 Feb 2024 17:31:38 +0900 Subject: [PATCH 04/22] Fixed directory name and add README.md Signed-off-by: Shintaro Sakoda --- localization/ndt_evaluation/README.md | 60 +++++++++++++++++++ .../convert_rosbag_for_ndt_evaluation.py | 0 2 files changed, 60 insertions(+) create mode 100644 localization/ndt_evaluation/README.md rename localization/{scripts => ndt_evaluation}/convert_rosbag_for_ndt_evaluation.py (100%) diff --git a/localization/ndt_evaluation/README.md b/localization/ndt_evaluation/README.md new file mode 100644 index 00000000..5b38d4f1 --- /dev/null +++ b/localization/ndt_evaluation/README.md @@ -0,0 +1,60 @@ +# NDT Evaluation + +This directory contains tools for evaluating the performance of the NDT localization. + +## Step1: Record rosbag + +In order to use point clouds from different LiDARs in the future, it is useful to record raw point clouds and so on. + +```bash +$ source ~/autoware/install/setup.bash +$ ros2 topic list \ + | grep -e ^/sensing -e ^/vehicle -e ^/localization -e ^/tf_static -e ^/initialpose \ + > topic_list.txt +$ ros2 bag record $(cat topic_list.txt) +``` + +## Step2: Apply `convert_rosbag_for_ndt_evaluation.py` + +* Check whether the necessary topics are available +* Extract only the necessary topics +* Change the topic name of the reference trajectory + +```bash +python3 convert_rosbag_for_ndt_evaluation.py /path/to/recorded_rosbag +``` + +## Step3: Execute DrivingLogReplayer + +Setup [DrivingLogReplayer](https://tier4.github.io/driving_log_replayer/quick_start/installation/) and copy the converted rosbag (input_bad) to `~/driving_log_replayer_data/localization` + +```bash +~/driving_log_replayer_data/localization$ tree -L 2 +. +├── evaluation_sample +│   ├── input_bag +│   └── scenario.yaml +└── sample + ├── input_bag + └── scenario.yaml + +4 directories, 2 files +``` + +Then execute. + +```bash +dlr simulation run -p localization -l "play_rate:=0.5" +``` + +```bash +<< show test result >> +test case 1 / 2 : use case: evaluation_sample +-------------------------------------------------- +TestResult: Passed +Passed: Convergence (Success): 964 / 964 -> 100.00%, Reliability (Success): NVTL Sequential NG Count: 0 (Total Test: 974, Average: 3.07964, StdDev: 0.09657), NDT Availability (Success): NDT available +test case 2 / 2 : use case: sample +-------------------------------------------------- +TestResult: Passed +Passed: Convergence (Success): 294 / 295 -> 99.66%, Reliability (Success): NVTL Sequential NG Count: 0 (Total Test: 295, Average: 2.47750, StdDev: 0.04174), NDT Availability (Success): NDT available +``` diff --git a/localization/scripts/convert_rosbag_for_ndt_evaluation.py b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py similarity index 100% rename from localization/scripts/convert_rosbag_for_ndt_evaluation.py rename to localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py From f3591efaf64f3cd7bc97bd0746b538729270580e Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Feb 2024 08:31:54 +0000 Subject: [PATCH 05/22] style(pre-commit): autofix --- localization/ndt_evaluation/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/localization/ndt_evaluation/README.md b/localization/ndt_evaluation/README.md index 5b38d4f1..d7c0d089 100644 --- a/localization/ndt_evaluation/README.md +++ b/localization/ndt_evaluation/README.md @@ -16,9 +16,9 @@ $ ros2 bag record $(cat topic_list.txt) ## Step2: Apply `convert_rosbag_for_ndt_evaluation.py` -* Check whether the necessary topics are available -* Extract only the necessary topics -* Change the topic name of the reference trajectory +- Check whether the necessary topics are available +- Extract only the necessary topics +- Change the topic name of the reference trajectory ```bash python3 convert_rosbag_for_ndt_evaluation.py /path/to/recorded_rosbag From 3f1ed5e6f28964df4adba94f1217fd277c10fcd9 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 26 Feb 2024 18:56:08 +0900 Subject: [PATCH 06/22] Added plot_box.py Signed-off-by: Shintaro Sakoda --- localization/ndt_evaluation/plot_box.py | 238 ++++++++++++++++++++++++ 1 file changed, 238 insertions(+) create mode 100644 localization/ndt_evaluation/plot_box.py diff --git a/localization/ndt_evaluation/plot_box.py b/localization/ndt_evaluation/plot_box.py new file mode 100644 index 00000000..e7f7172d --- /dev/null +++ b/localization/ndt_evaluation/plot_box.py @@ -0,0 +1,238 @@ +import numpy as np +import matplotlib.pyplot as plt +import tf_transformations +import rosbag2_py +import argparse +import pathlib +from rosidl_runtime_py.utilities import get_message +from rclpy.serialization import deserialize_message +import pandas as pd +from scipy.spatial.transform import Rotation, Slerp +import geometry_msgs.msg +import nav_msgs.msg + + +def euler_from_quaternion(quaternion): + return tf_transformations.euler_from_quaternion([quaternion.qx, quaternion.qy, quaternion.qz, quaternion.qw]) + + +def extract_pose_data(msg, msg_type): + if msg_type == geometry_msgs.msg.Pose: + return msg + elif msg_type == geometry_msgs.msg.PoseStamped: + return msg.pose + elif msg_type == geometry_msgs.msg.PoseWithCovarianceStamped: + return msg.pose.pose + elif msg_type == nav_msgs.msg.Odometry: + return msg.pose.pose + else: + raise ValueError(f"Unsupported message type: {msg_type}") + + +def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.DataFrame: + POSITIONS_KEY = ['x', 'y', 'z'] + ORIENTATIONS_KEY = ['qw', 'qx', 'qy', 'qz'] + target_index = 0 + df_index = 0 + data_dict = { + 'x': [], + 'y': [], + 'z': [], + 'qx': [], + 'qy': [], + 'qz': [], + 'qw': [], + 'timestamp': [], + } + 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][POSITIONS_KEY] + next_position = df_pose.iloc[df_index + 1][POSITIONS_KEY] + target_position = curr_position * curr_weight + next_position * next_weight + + curr_orientation = df_pose.iloc[df_index][ORIENTATIONS_KEY] + next_orientation = df_pose.iloc[df_index + 1][ORIENTATIONS_KEY] + 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_timestamp[target_index]) + data_dict['x'].append(target_position[0]) + data_dict['y'].append(target_position[1]) + data_dict['z'].append(target_position[2]) + data_dict['qw'].append(target_orientation[0]) + data_dict['qx'].append(target_orientation[1]) + data_dict['qy'].append(target_orientation[2]) + data_dict['qz'].append(target_orientation[3]) + target_index += 1 + result_df = pd.DataFrame(data_dict) + return result_df + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument('rosbag_path', type=pathlib.Path) + parser.add_argument('--pose_topic1', type=str, default="/localization/pose_estimator/pose") + parser.add_argument('--pose_topic2', type=str, default="/sensing/gnss/pose") + return parser.parse_args() + + +if __name__ == '__main__': + args = parse_args() + rosbag_path = args.rosbag_path + pose_topic1 = args.pose_topic1 + pose_topic2 = args.pose_topic2 + + serialization_format = 'cdr' + storage_options = rosbag2_py.StorageOptions( + uri=str(rosbag_path), storage_id='sqlite3') + 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))} + + target_topics = [pose_topic1, pose_topic2] + storage_filter = rosbag2_py.StorageFilter(topics=target_topics) + reader.set_filter(storage_filter) + + pose1_data = list() + pose2_data = list() + + while reader.has_next(): + (topic_name, data, timestamp_rosbag) = reader.read_next() + msg_type = get_message(type_map[topic_name]) + msg = deserialize_message(data, msg_type) + timestamp_header = int(msg.header.stamp.sec) + \ + int(msg.header.stamp.nanosec) * 1e-9 + if topic_name == pose_topic1: + pose = extract_pose_data(msg, msg_type) + pose1_data.append({ + 'timestamp': timestamp_header, + 'x': pose.position.x, + 'y': pose.position.y, + 'z': pose.position.z, + 'qw': pose.orientation.w, + 'qx': pose.orientation.x, + 'qy': pose.orientation.y, + 'qz': pose.orientation.z, + }) + elif topic_name == pose_topic2: + pose = extract_pose_data(msg, msg_type) + pose2_data.append({ + 'timestamp': timestamp_header, + 'x': pose.position.x, + 'y': pose.position.y, + 'z': pose.position.z, + 'qw': pose.orientation.w, + 'qx': pose.orientation.x, + 'qy': pose.orientation.y, + 'qz': pose.orientation.z, + }) + else: + assert False, f"Unknown topic: {topic_name}" + + # Processing data + df_pose1 = pd.DataFrame(pose1_data) + df_pose2 = pd.DataFrame(pose2_data) + + # Synchronize timestamps + df_pose2 = interpolate_pose(df_pose2, df_pose1['timestamp']) + + assert len(df_pose1) == len(df_pose2), \ + f"Lengths of pose1({len(df_pose1)}) and pose2({len(df_pose1)}) are different" + + df_length = len(pose1_data) + translation_error = [] + yaw_error = [] + distance_traveled = [0] + + for i in range(1, df_length): + pose1_pos = df_pose1.iloc[i][['x', 'y', 'z']] + pose2_pos = df_pose2.iloc[i][['x', 'y', 'z']] + translation_error.append(np.linalg.norm(pose1_pos - pose2_pos)) + + pose1_yaw = euler_from_quaternion(df_pose1.iloc[i])[2] + pose2_yaw = euler_from_quaternion(df_pose2.iloc[i])[2] + yaw_error.append(abs(pose1_yaw - pose2_yaw)) + + prev_pose1_pos = df_pose1.iloc[i-1][['x', 'y', 'z']] + distance_traveled.append(distance_traveled[-1] + np.linalg.norm(pose1_pos - prev_pose1_pos)) + + num_subdivisions = 5 + max_distance = max(distance_traveled) + distance_interval = max_distance / num_subdivisions + + # Categorize the distance traveled into subdivisions + distance_categories = np.digitize( + distance_traveled, np.arange(0, max_distance, distance_interval)) + + # Ensure that the arrays are of the same length + length = min(len(distance_categories), len(translation_error), len(yaw_error)) + distance_categories = distance_categories[:length] + translation_error = translation_error[:length] + yaw_error = yaw_error[:length] + + # Now create the DataFrames + df_translation_error = pd.DataFrame({ + 'DistanceCategory': distance_categories, + 'DistanceTraveled': distance_categories * distance_interval, + 'TranslationError': np.array(translation_error) + }) + + print(df_translation_error) + + df_yaw_error = pd.DataFrame({ + 'DistanceCategory': distance_categories, + 'DistanceTraveled': distance_categories * distance_interval, + 'YawError': np.array(yaw_error) * 180 / np.pi + }) + + # Create a single figure for both subplots + fig, axs = plt.subplots(1, 2, figsize=(15, 6)) # 1 row, 2 columns + + # Translation Error Box Plot + df_translation_error.boxplot(column='TranslationError', + by='DistanceCategory', grid=False, ax=axs[0]) + axs[0].set_title('Translation Error vs Distance Traveled') + axs[0].set_xlabel('Distance traveled [m]') + axs[0].set_ylabel('Translation error [m]') + axs[0].set_xticklabels( + [f"{(i + 1) * distance_interval:.2f}" for i in range(num_subdivisions)]) + + # Yaw Error Box Plot + df_yaw_error.boxplot(column='YawError', by='DistanceCategory', grid=False, ax=axs[1]) + axs[1].set_title('Yaw Error vs Distance Traveled') + axs[1].set_xlabel('Distance traveled [m]') + axs[1].set_ylabel('Yaw error [deg]') + axs[1].set_xticklabels( + [f"{(i + 1) * distance_interval:.2f}" for i in range(num_subdivisions)]) + + plt.suptitle('') + + # Adjust layout + plt.tight_layout() + + # rosbag path may be the path to the db3 file, or it may be the path to the directory containing it + save_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent + + save_path = save_dir / "performance_box.png" + plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + print(f"Saved to {save_path}") + plt.close() From 16fb56748b65f951b7703a3d68f94c2a214daa56 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 26 Feb 2024 09:57:49 +0000 Subject: [PATCH 07/22] style(pre-commit): autofix --- localization/ndt_evaluation/plot_box.py | 198 +++++++++++++----------- 1 file changed, 105 insertions(+), 93 deletions(-) diff --git a/localization/ndt_evaluation/plot_box.py b/localization/ndt_evaluation/plot_box.py index e7f7172d..ee9aca53 100644 --- a/localization/ndt_evaluation/plot_box.py +++ b/localization/ndt_evaluation/plot_box.py @@ -1,19 +1,23 @@ -import numpy as np -import matplotlib.pyplot as plt -import tf_transformations -import rosbag2_py import argparse import pathlib -from rosidl_runtime_py.utilities import get_message -from rclpy.serialization import deserialize_message -import pandas as pd -from scipy.spatial.transform import Rotation, Slerp + import geometry_msgs.msg +import matplotlib.pyplot as plt import nav_msgs.msg +import numpy as np +import pandas as pd +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message +from scipy.spatial.transform import Rotation +from scipy.spatial.transform import Slerp +import tf_transformations def euler_from_quaternion(quaternion): - return tf_transformations.euler_from_quaternion([quaternion.qx, quaternion.qy, quaternion.qz, quaternion.qw]) + return tf_transformations.euler_from_quaternion( + [quaternion.qx, quaternion.qy, quaternion.qz, quaternion.qw] + ) def extract_pose_data(msg, msg_type): @@ -30,23 +34,23 @@ def extract_pose_data(msg, msg_type): def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.DataFrame: - POSITIONS_KEY = ['x', 'y', 'z'] - ORIENTATIONS_KEY = ['qw', 'qx', 'qy', 'qz'] + POSITIONS_KEY = ["x", "y", "z"] + ORIENTATIONS_KEY = ["qw", "qx", "qy", "qz"] target_index = 0 df_index = 0 data_dict = { - 'x': [], - 'y': [], - 'z': [], - 'qx': [], - 'qy': [], - 'qz': [], - 'qw': [], - 'timestamp': [], + "x": [], + "y": [], + "z": [], + "qx": [], + "qy": [], + "qz": [], + "qw": [], + "timestamp": [], } 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'] + 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 @@ -65,18 +69,17 @@ def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.D next_orientation = df_pose.iloc[df_index + 1][ORIENTATIONS_KEY] 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])) + 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_timestamp[target_index]) - data_dict['x'].append(target_position[0]) - data_dict['y'].append(target_position[1]) - data_dict['z'].append(target_position[2]) - data_dict['qw'].append(target_orientation[0]) - data_dict['qx'].append(target_orientation[1]) - data_dict['qy'].append(target_orientation[2]) - data_dict['qz'].append(target_orientation[3]) + data_dict["timestamp"].append(target_timestamp[target_index]) + data_dict["x"].append(target_position[0]) + data_dict["y"].append(target_position[1]) + data_dict["z"].append(target_position[2]) + data_dict["qw"].append(target_orientation[0]) + data_dict["qx"].append(target_orientation[1]) + data_dict["qy"].append(target_orientation[2]) + data_dict["qz"].append(target_orientation[3]) target_index += 1 result_df = pd.DataFrame(data_dict) return result_df @@ -84,23 +87,24 @@ def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.D def parse_args(): parser = argparse.ArgumentParser() - parser.add_argument('rosbag_path', type=pathlib.Path) - parser.add_argument('--pose_topic1', type=str, default="/localization/pose_estimator/pose") - parser.add_argument('--pose_topic2', type=str, default="/sensing/gnss/pose") + parser.add_argument("rosbag_path", type=pathlib.Path) + parser.add_argument("--pose_topic1", type=str, default="/localization/pose_estimator/pose") + parser.add_argument("--pose_topic2", type=str, default="/sensing/gnss/pose") return parser.parse_args() -if __name__ == '__main__': +if __name__ == "__main__": args = parse_args() rosbag_path = args.rosbag_path pose_topic1 = args.pose_topic1 pose_topic2 = args.pose_topic2 - serialization_format = 'cdr' - storage_options = rosbag2_py.StorageOptions( - uri=str(rosbag_path), storage_id='sqlite3') + serialization_format = "cdr" + storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3") converter_options = rosbag2_py.ConverterOptions( - input_serialization_format=serialization_format, output_serialization_format=serialization_format) + input_serialization_format=serialization_format, + output_serialization_format=serialization_format, + ) reader = rosbag2_py.SequentialReader() reader.open(storage_options, converter_options) @@ -119,32 +123,35 @@ def parse_args(): (topic_name, data, timestamp_rosbag) = reader.read_next() msg_type = get_message(type_map[topic_name]) msg = deserialize_message(data, msg_type) - timestamp_header = int(msg.header.stamp.sec) + \ - int(msg.header.stamp.nanosec) * 1e-9 + timestamp_header = int(msg.header.stamp.sec) + int(msg.header.stamp.nanosec) * 1e-9 if topic_name == pose_topic1: pose = extract_pose_data(msg, msg_type) - pose1_data.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + pose1_data.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) elif topic_name == pose_topic2: pose = extract_pose_data(msg, msg_type) - pose2_data.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + pose2_data.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) else: assert False, f"Unknown topic: {topic_name}" @@ -153,10 +160,11 @@ def parse_args(): df_pose2 = pd.DataFrame(pose2_data) # Synchronize timestamps - df_pose2 = interpolate_pose(df_pose2, df_pose1['timestamp']) + df_pose2 = interpolate_pose(df_pose2, df_pose1["timestamp"]) - assert len(df_pose1) == len(df_pose2), \ - f"Lengths of pose1({len(df_pose1)}) and pose2({len(df_pose1)}) are different" + assert len(df_pose1) == len( + df_pose2 + ), f"Lengths of pose1({len(df_pose1)}) and pose2({len(df_pose1)}) are different" df_length = len(pose1_data) translation_error = [] @@ -164,15 +172,15 @@ def parse_args(): distance_traveled = [0] for i in range(1, df_length): - pose1_pos = df_pose1.iloc[i][['x', 'y', 'z']] - pose2_pos = df_pose2.iloc[i][['x', 'y', 'z']] + pose1_pos = df_pose1.iloc[i][["x", "y", "z"]] + pose2_pos = df_pose2.iloc[i][["x", "y", "z"]] translation_error.append(np.linalg.norm(pose1_pos - pose2_pos)) pose1_yaw = euler_from_quaternion(df_pose1.iloc[i])[2] pose2_yaw = euler_from_quaternion(df_pose2.iloc[i])[2] yaw_error.append(abs(pose1_yaw - pose2_yaw)) - prev_pose1_pos = df_pose1.iloc[i-1][['x', 'y', 'z']] + prev_pose1_pos = df_pose1.iloc[i - 1][["x", "y", "z"]] distance_traveled.append(distance_traveled[-1] + np.linalg.norm(pose1_pos - prev_pose1_pos)) num_subdivisions = 5 @@ -181,7 +189,8 @@ def parse_args(): # Categorize the distance traveled into subdivisions distance_categories = np.digitize( - distance_traveled, np.arange(0, max_distance, distance_interval)) + distance_traveled, np.arange(0, max_distance, distance_interval) + ) # Ensure that the arrays are of the same length length = min(len(distance_categories), len(translation_error), len(yaw_error)) @@ -190,41 +199,44 @@ def parse_args(): yaw_error = yaw_error[:length] # Now create the DataFrames - df_translation_error = pd.DataFrame({ - 'DistanceCategory': distance_categories, - 'DistanceTraveled': distance_categories * distance_interval, - 'TranslationError': np.array(translation_error) - }) + df_translation_error = pd.DataFrame( + { + "DistanceCategory": distance_categories, + "DistanceTraveled": distance_categories * distance_interval, + "TranslationError": np.array(translation_error), + } + ) print(df_translation_error) - df_yaw_error = pd.DataFrame({ - 'DistanceCategory': distance_categories, - 'DistanceTraveled': distance_categories * distance_interval, - 'YawError': np.array(yaw_error) * 180 / np.pi - }) + df_yaw_error = pd.DataFrame( + { + "DistanceCategory": distance_categories, + "DistanceTraveled": distance_categories * distance_interval, + "YawError": np.array(yaw_error) * 180 / np.pi, + } + ) # Create a single figure for both subplots fig, axs = plt.subplots(1, 2, figsize=(15, 6)) # 1 row, 2 columns # Translation Error Box Plot - df_translation_error.boxplot(column='TranslationError', - by='DistanceCategory', grid=False, ax=axs[0]) - axs[0].set_title('Translation Error vs Distance Traveled') - axs[0].set_xlabel('Distance traveled [m]') - axs[0].set_ylabel('Translation error [m]') - axs[0].set_xticklabels( - [f"{(i + 1) * distance_interval:.2f}" for i in range(num_subdivisions)]) + df_translation_error.boxplot( + column="TranslationError", by="DistanceCategory", grid=False, ax=axs[0] + ) + axs[0].set_title("Translation Error vs Distance Traveled") + axs[0].set_xlabel("Distance traveled [m]") + axs[0].set_ylabel("Translation error [m]") + axs[0].set_xticklabels([f"{(i + 1) * distance_interval:.2f}" for i in range(num_subdivisions)]) # Yaw Error Box Plot - df_yaw_error.boxplot(column='YawError', by='DistanceCategory', grid=False, ax=axs[1]) - axs[1].set_title('Yaw Error vs Distance Traveled') - axs[1].set_xlabel('Distance traveled [m]') - axs[1].set_ylabel('Yaw error [deg]') - axs[1].set_xticklabels( - [f"{(i + 1) * distance_interval:.2f}" for i in range(num_subdivisions)]) + df_yaw_error.boxplot(column="YawError", by="DistanceCategory", grid=False, ax=axs[1]) + axs[1].set_title("Yaw Error vs Distance Traveled") + axs[1].set_xlabel("Distance traveled [m]") + axs[1].set_ylabel("Yaw error [deg]") + axs[1].set_xticklabels([f"{(i + 1) * distance_interval:.2f}" for i in range(num_subdivisions)]) - plt.suptitle('') + plt.suptitle("") # Adjust layout plt.tight_layout() @@ -233,6 +245,6 @@ def parse_args(): save_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent save_path = save_dir / "performance_box.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() From a7495f0513f1368e3cec0b5e320332a72f0c6942 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 26 Feb 2024 19:04:16 +0900 Subject: [PATCH 08/22] Fixed initialize of list Signed-off-by: Shintaro Sakoda --- localization/ndt_evaluation/plot_box.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/localization/ndt_evaluation/plot_box.py b/localization/ndt_evaluation/plot_box.py index ee9aca53..07fc3cd5 100644 --- a/localization/ndt_evaluation/plot_box.py +++ b/localization/ndt_evaluation/plot_box.py @@ -116,8 +116,8 @@ def parse_args(): storage_filter = rosbag2_py.StorageFilter(topics=target_topics) reader.set_filter(storage_filter) - pose1_data = list() - pose2_data = list() + pose1_data = [] + pose2_data = [] while reader.has_next(): (topic_name, data, timestamp_rosbag) = reader.read_next() From 89831729b6de12f67d4d3af8e252248b61c705a2 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 27 Feb 2024 10:52:40 +0900 Subject: [PATCH 09/22] Fixed record command Signed-off-by: Shintaro Sakoda --- localization/ndt_evaluation/README.md | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/localization/ndt_evaluation/README.md b/localization/ndt_evaluation/README.md index d7c0d089..32e36b65 100644 --- a/localization/ndt_evaluation/README.md +++ b/localization/ndt_evaluation/README.md @@ -4,14 +4,15 @@ This directory contains tools for evaluating the performance of the NDT localiza ## Step1: Record rosbag -In order to use point clouds from different LiDARs in the future, it is useful to record raw point clouds and so on. - ```bash -$ source ~/autoware/install/setup.bash -$ ros2 topic list \ - | grep -e ^/sensing -e ^/vehicle -e ^/localization -e ^/tf_static -e ^/initialpose \ - > topic_list.txt -$ ros2 bag record $(cat topic_list.txt) +ros2 bag record \ + /localization/kinematic_state \ + /localization/util/downsample/pointcloud \ + /sensing/vehicle_velocity_converter/twist_with_covariance \ + /sensing/imu/imu_data \ + /tf_static \ + /sensing/gnss/pose_with_covariance \ + /initialpose ``` ## Step2: Apply `convert_rosbag_for_ndt_evaluation.py` From 105d62492395ce4a16dbbdf8c778aa6c2b860e0a Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 27 Feb 2024 13:43:36 +0900 Subject: [PATCH 10/22] Added plot scripts Signed-off-by: Shintaro Sakoda --- localization/ndt_evaluation/.gitignore | 1 + .../ndt_evaluation/interpolate_pose.py | 64 +++++++++ .../ndt_evaluation/plot_exe_time_ms.py | 108 ++++++++++++++++ .../ndt_evaluation/plot_pose_instability.py | 121 ++++++++++++++++++ 4 files changed, 294 insertions(+) create mode 100644 localization/ndt_evaluation/.gitignore create mode 100644 localization/ndt_evaluation/interpolate_pose.py create mode 100644 localization/ndt_evaluation/plot_exe_time_ms.py create mode 100644 localization/ndt_evaluation/plot_pose_instability.py diff --git a/localization/ndt_evaluation/.gitignore b/localization/ndt_evaluation/.gitignore new file mode 100644 index 00000000..0d20b648 --- /dev/null +++ b/localization/ndt_evaluation/.gitignore @@ -0,0 +1 @@ +*.pyc diff --git a/localization/ndt_evaluation/interpolate_pose.py b/localization/ndt_evaluation/interpolate_pose.py new file mode 100644 index 00000000..007f29e9 --- /dev/null +++ b/localization/ndt_evaluation/interpolate_pose.py @@ -0,0 +1,64 @@ +import pandas as pd +from scipy.spatial.transform import Rotation, 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 + Constraints) + * df_pose and target_timestamp must be sorted by timestamp + * df_pose must have timestamps with a larger interval than target_timestamp + * i.e. df_pose[0] <= target_timestamp[0] and target_timestamp[-1] <= df_pose[-1] + * len(df_pose) > len(target_timestamp) + 出力) + * DataFrame with same columns as df_pose and length same as target_timestamp + """ + POSITIONS_KEY = ['x', 'y', 'z'] + ORIENTATIONS_KEY = ['qw', 'qx', 'qy', 'qz'] + target_index = 0 + df_index = 0 + data_dict = { + 'x': [], + 'y': [], + 'z': [], + 'qx': [], + 'qy': [], + 'qz': [], + 'qw': [], + 'timestamp': [], + } + 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][POSITIONS_KEY] + next_position = df_pose.iloc[df_index + 1][POSITIONS_KEY] + target_position = curr_position * curr_weight + next_position * next_weight + + curr_orientation = df_pose.iloc[df_index][ORIENTATIONS_KEY] + next_orientation = df_pose.iloc[df_index + 1][ORIENTATIONS_KEY] + 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_timestamp[target_index]) + data_dict['x'].append(target_position[0]) + data_dict['y'].append(target_position[1]) + data_dict['z'].append(target_position[2]) + data_dict['qw'].append(target_orientation[0]) + data_dict['qx'].append(target_orientation[1]) + data_dict['qy'].append(target_orientation[2]) + data_dict['qz'].append(target_orientation[3]) + target_index += 1 + result_df = pd.DataFrame(data_dict) + return result_df diff --git a/localization/ndt_evaluation/plot_exe_time_ms.py b/localization/ndt_evaluation/plot_exe_time_ms.py new file mode 100644 index 00000000..505d1a0a --- /dev/null +++ b/localization/ndt_evaluation/plot_exe_time_ms.py @@ -0,0 +1,108 @@ +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +import argparse +import matplotlib.pyplot as plt +import pathlib +import pandas as pd +from interpolate_pose import interpolate_pose + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument('rosbag_path', type=pathlib.Path) + return parser.parse_args() + + +if __name__ == "__main__": + args = parse_args() + rosbag_path = args.rosbag_path + serialization_format = 'cdr' + storage_options = rosbag2_py.StorageOptions( + uri=str(rosbag_path), storage_id='sqlite3') + 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))} + + target_topics = [ + "/localization/pose_estimator/exe_time_ms", + "/localization/pose_estimator/pose", + ] + storage_filter = rosbag2_py.StorageFilter(topics=target_topics) + reader.set_filter(storage_filter) + + ndt_exe_time_ms = list() + ndt_pose_list = list() + + while reader.has_next(): + (topic, data, timestamp_rosbag) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + if topic == "/localization/pose_estimator/exe_time_ms": + timestamp_header = int(msg.stamp.sec) + \ + int(msg.stamp.nanosec) * 1e-9 + ndt_exe_time_ms.append({ + 'timestamp': timestamp_header, + 'value': msg.data, + }) + elif topic == "/localization/pose_estimator/pose": + timestamp_header = int(msg.header.stamp.sec) + \ + int(msg.header.stamp.nanosec) * 1e-9 + pose = msg.pose + ndt_pose_list.append({ + 'timestamp': timestamp_header, + 'x': pose.position.x, + 'y': pose.position.y, + 'z': pose.position.z, + 'qw': pose.orientation.w, + 'qx': pose.orientation.x, + 'qy': pose.orientation.y, + 'qz': pose.orientation.z, + }) + else: + assert False, f"Unknown topic: {topic}" + + print(f"{len(ndt_exe_time_ms)=}") + print(f"{len(ndt_pose_list)=}") + + df_ndt_exe_time_ms = pd.DataFrame(ndt_exe_time_ms) + df_ndt_pose = pd.DataFrame(ndt_pose_list) + + df_ndt_pose = interpolate_pose( + df_ndt_pose, df_ndt_exe_time_ms['timestamp']) + print(f"{len(df_ndt_exe_time_ms)=}") + print(f"{len(df_ndt_pose)=}") + + # rosbag path may be the path to the db3 file, or it may be the path to the directory containing it + save_dir = (rosbag_path.parent if rosbag_path.is_dir() + else rosbag_path.parent.parent) / "exe_time_ms" + save_dir.mkdir(exist_ok=True) + + # plot exe_time_ms + plt.plot(df_ndt_exe_time_ms['timestamp'], + df_ndt_exe_time_ms['value']) + plt.xlabel("timestamp") + plt.ylabel("exe_time [ms]") + save_path = save_dir / "exe_time_ms.png" + plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + print(f"Saved to {save_path}") + plt.close() + + # plot exe_time_ms on trajectory of ndt_pose + plt.scatter(df_ndt_pose['x'], df_ndt_pose['y'], + c=df_ndt_exe_time_ms['value'].values, cmap='viridis') + plt.colorbar(label='pose_instability_diff_pose[m]') + plt.xlabel("x[m]") + plt.ylabel("y[m]") + plt.axis('equal') + save_path = save_dir / "exe_time_ms_on_ndt_pose.png" + plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + print(f"Saved to {save_path}") + plt.close() diff --git a/localization/ndt_evaluation/plot_pose_instability.py b/localization/ndt_evaluation/plot_pose_instability.py new file mode 100644 index 00000000..6dfc10e4 --- /dev/null +++ b/localization/ndt_evaluation/plot_pose_instability.py @@ -0,0 +1,121 @@ +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message +import argparse +import matplotlib.pyplot as plt +import numpy as np +import pathlib +import pandas as pd +from interpolate_pose import interpolate_pose + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument('rosbag_path', type=pathlib.Path) + return parser.parse_args() + + +if __name__ == "__main__": + args = parse_args() + rosbag_path = args.rosbag_path + serialization_format = 'cdr' + storage_options = rosbag2_py.StorageOptions( + uri=str(rosbag_path), storage_id='sqlite3') + 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))} + + target_topics = [ + "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose", + "/localization/pose_twist_fusion_filter/kinematic_state", + ] + storage_filter = rosbag2_py.StorageFilter(topics=target_topics) + reader.set_filter(storage_filter) + + pose_instability_diff_pose_list = list() + ekf_pose_list = list() + + while reader.has_next(): + (topic, data, timestamp_rosbag) = reader.read_next() + msg_type = get_message(type_map[topic]) + msg = deserialize_message(data, msg_type) + timestamp_header = int(msg.header.stamp.sec) + \ + int(msg.header.stamp.nanosec) * 1e-9 + if topic == "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose": + pose = msg.pose + pose_instability_diff_pose_list.append({ + 'timestamp': timestamp_header, + 'x': pose.position.x, + 'y': pose.position.y, + 'z': pose.position.z, + 'qw': pose.orientation.w, + 'qx': pose.orientation.x, + 'qy': pose.orientation.y, + 'qz': pose.orientation.z, + }) + elif topic == "/localization/pose_twist_fusion_filter/kinematic_state": + pose = msg.pose.pose + ekf_pose_list.append({ + 'timestamp': timestamp_header, + 'x': pose.position.x, + 'y': pose.position.y, + 'z': pose.position.z, + 'qw': pose.orientation.w, + 'qx': pose.orientation.x, + 'qy': pose.orientation.y, + 'qz': pose.orientation.z, + }) + else: + assert False, f"Unknown topic: {topic}" + + print(f"{len(pose_instability_diff_pose_list)=}") + print(f"{len(ekf_pose_list)=}") + + df_pose_instability_diff_pose = pd.DataFrame( + pose_instability_diff_pose_list) + df_ekf_pose = pd.DataFrame(ekf_pose_list) + + df_ekf_pose = interpolate_pose( + df_ekf_pose, df_pose_instability_diff_pose['timestamp']) + print(f"{len(df_pose_instability_diff_pose)=}") + print(f"{len(df_ekf_pose)=}") + + # rosbag path may be the path to the db3 file, or it may be the path to the directory containing it + save_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent + + # plot pose_instability_diff_pose + plt.plot(df_pose_instability_diff_pose['timestamp'], + df_pose_instability_diff_pose['x'], label='x') + plt.plot(df_pose_instability_diff_pose['timestamp'], + df_pose_instability_diff_pose['y'], label='y') + plt.plot(df_pose_instability_diff_pose['timestamp'], + df_pose_instability_diff_pose['z'], label='z') + plt.xlabel("timestamp") + plt.ylabel("pose_instability_diff_pose[m]") + plt.legend() + save_path = save_dir / "pose_instability_diff_pose.png" + plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + print(f"Saved to {save_path}") + plt.close() + + # plot norm of pose_instability_diff_pose on trajectory of ekf_pose + diff_pose = df_pose_instability_diff_pose[['x', 'y', 'z']].values + diff_pose_norm = np.linalg.norm(diff_pose, axis=1) + + plt.scatter(df_ekf_pose['x'], df_ekf_pose['y'], + c=diff_pose_norm, cmap='viridis') + plt.colorbar(label='pose_instability_diff_pose[m]') + plt.xlabel("x[m]") + plt.ylabel("y[m]") + plt.axis('equal') + save_path = save_dir / "pose_instability_diff_pose_on_ekf_pose.png" + plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + print(f"Saved to {save_path}") + plt.close() From 6db34520bb655592057a7b39e477c6e1011a95f5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 27 Feb 2024 04:44:03 +0000 Subject: [PATCH 11/22] style(pre-commit): autofix --- .../ndt_evaluation/interpolate_pose.py | 48 ++++---- .../ndt_evaluation/plot_exe_time_ms.py | 86 +++++++------- .../ndt_evaluation/plot_pose_instability.py | 110 +++++++++--------- 3 files changed, 126 insertions(+), 118 deletions(-) diff --git a/localization/ndt_evaluation/interpolate_pose.py b/localization/ndt_evaluation/interpolate_pose.py index 007f29e9..6cddeff6 100644 --- a/localization/ndt_evaluation/interpolate_pose.py +++ b/localization/ndt_evaluation/interpolate_pose.py @@ -1,9 +1,10 @@ import pandas as pd -from scipy.spatial.transform import Rotation, Slerp +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 + """Interpolate each pose in df_pose to match the timestamp in target_timestamp Constraints) * df_pose and target_timestamp must be sorted by timestamp * df_pose must have timestamps with a larger interval than target_timestamp @@ -12,23 +13,23 @@ def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.D 出力) * DataFrame with same columns as df_pose and length same as target_timestamp """ - POSITIONS_KEY = ['x', 'y', 'z'] - ORIENTATIONS_KEY = ['qw', 'qx', 'qy', 'qz'] + POSITIONS_KEY = ["x", "y", "z"] + ORIENTATIONS_KEY = ["qw", "qx", "qy", "qz"] target_index = 0 df_index = 0 data_dict = { - 'x': [], - 'y': [], - 'z': [], - 'qx': [], - 'qy': [], - 'qz': [], - 'qw': [], - 'timestamp': [], + "x": [], + "y": [], + "z": [], + "qx": [], + "qy": [], + "qz": [], + "qw": [], + "timestamp": [], } 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'] + 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 @@ -47,18 +48,17 @@ def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.D next_orientation = df_pose.iloc[df_index + 1][ORIENTATIONS_KEY] 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])) + 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_timestamp[target_index]) - data_dict['x'].append(target_position[0]) - data_dict['y'].append(target_position[1]) - data_dict['z'].append(target_position[2]) - data_dict['qw'].append(target_orientation[0]) - data_dict['qx'].append(target_orientation[1]) - data_dict['qy'].append(target_orientation[2]) - data_dict['qz'].append(target_orientation[3]) + data_dict["timestamp"].append(target_timestamp[target_index]) + data_dict["x"].append(target_position[0]) + data_dict["y"].append(target_position[1]) + data_dict["z"].append(target_position[2]) + data_dict["qw"].append(target_orientation[0]) + data_dict["qx"].append(target_orientation[1]) + data_dict["qy"].append(target_orientation[2]) + data_dict["qz"].append(target_orientation[3]) target_index += 1 result_df = pd.DataFrame(data_dict) return result_df diff --git a/localization/ndt_evaluation/plot_exe_time_ms.py b/localization/ndt_evaluation/plot_exe_time_ms.py index 505d1a0a..4c77898e 100644 --- a/localization/ndt_evaluation/plot_exe_time_ms.py +++ b/localization/ndt_evaluation/plot_exe_time_ms.py @@ -1,35 +1,35 @@ -import rosbag2_py -from rclpy.serialization import deserialize_message -from rosidl_runtime_py.utilities import get_message import argparse -import matplotlib.pyplot as plt import pathlib -import pandas as pd + from interpolate_pose import interpolate_pose +import matplotlib.pyplot as plt +import pandas as pd +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message def parse_args(): parser = argparse.ArgumentParser() - parser.add_argument('rosbag_path', type=pathlib.Path) + parser.add_argument("rosbag_path", type=pathlib.Path) return parser.parse_args() if __name__ == "__main__": args = parse_args() rosbag_path = args.rosbag_path - serialization_format = 'cdr' - storage_options = rosbag2_py.StorageOptions( - uri=str(rosbag_path), storage_id='sqlite3') + serialization_format = "cdr" + storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3") converter_options = rosbag2_py.ConverterOptions( input_serialization_format=serialization_format, - output_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))} + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} target_topics = [ "/localization/pose_estimator/exe_time_ms", @@ -46,26 +46,28 @@ def parse_args(): msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) if topic == "/localization/pose_estimator/exe_time_ms": - timestamp_header = int(msg.stamp.sec) + \ - int(msg.stamp.nanosec) * 1e-9 - ndt_exe_time_ms.append({ - 'timestamp': timestamp_header, - 'value': msg.data, - }) + timestamp_header = int(msg.stamp.sec) + int(msg.stamp.nanosec) * 1e-9 + ndt_exe_time_ms.append( + { + "timestamp": timestamp_header, + "value": msg.data, + } + ) elif topic == "/localization/pose_estimator/pose": - timestamp_header = int(msg.header.stamp.sec) + \ - int(msg.header.stamp.nanosec) * 1e-9 + timestamp_header = int(msg.header.stamp.sec) + int(msg.header.stamp.nanosec) * 1e-9 pose = msg.pose - ndt_pose_list.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + ndt_pose_list.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) else: assert False, f"Unknown topic: {topic}" @@ -75,34 +77,34 @@ def parse_args(): df_ndt_exe_time_ms = pd.DataFrame(ndt_exe_time_ms) df_ndt_pose = pd.DataFrame(ndt_pose_list) - df_ndt_pose = interpolate_pose( - df_ndt_pose, df_ndt_exe_time_ms['timestamp']) + df_ndt_pose = interpolate_pose(df_ndt_pose, df_ndt_exe_time_ms["timestamp"]) print(f"{len(df_ndt_exe_time_ms)=}") print(f"{len(df_ndt_pose)=}") # rosbag path may be the path to the db3 file, or it may be the path to the directory containing it - save_dir = (rosbag_path.parent if rosbag_path.is_dir() - else rosbag_path.parent.parent) / "exe_time_ms" + save_dir = ( + rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent + ) / "exe_time_ms" save_dir.mkdir(exist_ok=True) # plot exe_time_ms - plt.plot(df_ndt_exe_time_ms['timestamp'], - df_ndt_exe_time_ms['value']) + plt.plot(df_ndt_exe_time_ms["timestamp"], df_ndt_exe_time_ms["value"]) plt.xlabel("timestamp") plt.ylabel("exe_time [ms]") save_path = save_dir / "exe_time_ms.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() # plot exe_time_ms on trajectory of ndt_pose - plt.scatter(df_ndt_pose['x'], df_ndt_pose['y'], - c=df_ndt_exe_time_ms['value'].values, cmap='viridis') - plt.colorbar(label='pose_instability_diff_pose[m]') + plt.scatter( + df_ndt_pose["x"], df_ndt_pose["y"], c=df_ndt_exe_time_ms["value"].values, cmap="viridis" + ) + plt.colorbar(label="pose_instability_diff_pose[m]") plt.xlabel("x[m]") plt.ylabel("y[m]") - plt.axis('equal') + plt.axis("equal") save_path = save_dir / "exe_time_ms_on_ndt_pose.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() diff --git a/localization/ndt_evaluation/plot_pose_instability.py b/localization/ndt_evaluation/plot_pose_instability.py index 6dfc10e4..66d11639 100644 --- a/localization/ndt_evaluation/plot_pose_instability.py +++ b/localization/ndt_evaluation/plot_pose_instability.py @@ -1,36 +1,36 @@ -import rosbag2_py -from rclpy.serialization import deserialize_message -from rosidl_runtime_py.utilities import get_message import argparse +import pathlib + +from interpolate_pose import interpolate_pose import matplotlib.pyplot as plt import numpy as np -import pathlib import pandas as pd -from interpolate_pose import interpolate_pose +from rclpy.serialization import deserialize_message +import rosbag2_py +from rosidl_runtime_py.utilities import get_message def parse_args(): parser = argparse.ArgumentParser() - parser.add_argument('rosbag_path', type=pathlib.Path) + parser.add_argument("rosbag_path", type=pathlib.Path) return parser.parse_args() if __name__ == "__main__": args = parse_args() rosbag_path = args.rosbag_path - serialization_format = 'cdr' - storage_options = rosbag2_py.StorageOptions( - uri=str(rosbag_path), storage_id='sqlite3') + serialization_format = "cdr" + storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3") converter_options = rosbag2_py.ConverterOptions( input_serialization_format=serialization_format, - output_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))} + type_map = {topic_types[i].name: topic_types[i].type for i in range(len(topic_types))} target_topics = [ "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose", @@ -46,44 +46,48 @@ def parse_args(): (topic, data, timestamp_rosbag) = reader.read_next() msg_type = get_message(type_map[topic]) msg = deserialize_message(data, msg_type) - timestamp_header = int(msg.header.stamp.sec) + \ - int(msg.header.stamp.nanosec) * 1e-9 - if topic == "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose": + timestamp_header = int(msg.header.stamp.sec) + int(msg.header.stamp.nanosec) * 1e-9 + if ( + topic + == "/localization/pose_twist_fusion_filter/pose_instability_detector/debug/diff_pose" + ): pose = msg.pose - pose_instability_diff_pose_list.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + pose_instability_diff_pose_list.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) elif topic == "/localization/pose_twist_fusion_filter/kinematic_state": pose = msg.pose.pose - ekf_pose_list.append({ - 'timestamp': timestamp_header, - 'x': pose.position.x, - 'y': pose.position.y, - 'z': pose.position.z, - 'qw': pose.orientation.w, - 'qx': pose.orientation.x, - 'qy': pose.orientation.y, - 'qz': pose.orientation.z, - }) + ekf_pose_list.append( + { + "timestamp": timestamp_header, + "x": pose.position.x, + "y": pose.position.y, + "z": pose.position.z, + "qw": pose.orientation.w, + "qx": pose.orientation.x, + "qy": pose.orientation.y, + "qz": pose.orientation.z, + } + ) else: assert False, f"Unknown topic: {topic}" print(f"{len(pose_instability_diff_pose_list)=}") print(f"{len(ekf_pose_list)=}") - df_pose_instability_diff_pose = pd.DataFrame( - pose_instability_diff_pose_list) + df_pose_instability_diff_pose = pd.DataFrame(pose_instability_diff_pose_list) df_ekf_pose = pd.DataFrame(ekf_pose_list) - df_ekf_pose = interpolate_pose( - df_ekf_pose, df_pose_instability_diff_pose['timestamp']) + df_ekf_pose = interpolate_pose(df_ekf_pose, df_pose_instability_diff_pose["timestamp"]) print(f"{len(df_pose_instability_diff_pose)=}") print(f"{len(df_ekf_pose)=}") @@ -91,31 +95,33 @@ def parse_args(): save_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent # plot pose_instability_diff_pose - plt.plot(df_pose_instability_diff_pose['timestamp'], - df_pose_instability_diff_pose['x'], label='x') - plt.plot(df_pose_instability_diff_pose['timestamp'], - df_pose_instability_diff_pose['y'], label='y') - plt.plot(df_pose_instability_diff_pose['timestamp'], - df_pose_instability_diff_pose['z'], label='z') + plt.plot( + df_pose_instability_diff_pose["timestamp"], df_pose_instability_diff_pose["x"], label="x" + ) + plt.plot( + df_pose_instability_diff_pose["timestamp"], df_pose_instability_diff_pose["y"], label="y" + ) + plt.plot( + df_pose_instability_diff_pose["timestamp"], df_pose_instability_diff_pose["z"], label="z" + ) plt.xlabel("timestamp") plt.ylabel("pose_instability_diff_pose[m]") plt.legend() save_path = save_dir / "pose_instability_diff_pose.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() # plot norm of pose_instability_diff_pose on trajectory of ekf_pose - diff_pose = df_pose_instability_diff_pose[['x', 'y', 'z']].values + diff_pose = df_pose_instability_diff_pose[["x", "y", "z"]].values diff_pose_norm = np.linalg.norm(diff_pose, axis=1) - plt.scatter(df_ekf_pose['x'], df_ekf_pose['y'], - c=diff_pose_norm, cmap='viridis') - plt.colorbar(label='pose_instability_diff_pose[m]') + plt.scatter(df_ekf_pose["x"], df_ekf_pose["y"], c=diff_pose_norm, cmap="viridis") + plt.colorbar(label="pose_instability_diff_pose[m]") plt.xlabel("x[m]") plt.ylabel("y[m]") - plt.axis('equal') + plt.axis("equal") save_path = save_dir / "pose_instability_diff_pose_on_ekf_pose.png" - plt.savefig(save_path, bbox_inches='tight', pad_inches=0.05) + plt.savefig(save_path, bbox_inches="tight", pad_inches=0.05) print(f"Saved to {save_path}") plt.close() From 2c30ddf498f7d6cd9e6201b275fb52e8ac556f63 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 27 Feb 2024 14:49:46 +0900 Subject: [PATCH 12/22] Fixed to use interpolate_pose Signed-off-by: Shintaro Sakoda --- localization/ndt_evaluation/plot_box.py | 53 +------------------------ 1 file changed, 1 insertion(+), 52 deletions(-) diff --git a/localization/ndt_evaluation/plot_box.py b/localization/ndt_evaluation/plot_box.py index 07fc3cd5..48cd7148 100644 --- a/localization/ndt_evaluation/plot_box.py +++ b/localization/ndt_evaluation/plot_box.py @@ -12,6 +12,7 @@ from scipy.spatial.transform import Rotation from scipy.spatial.transform import Slerp import tf_transformations +from interpolate_pose import interpolate_pose def euler_from_quaternion(quaternion): @@ -33,58 +34,6 @@ def extract_pose_data(msg, msg_type): raise ValueError(f"Unsupported message type: {msg_type}") -def interpolate_pose(df_pose: pd.DataFrame, target_timestamp: pd.Series) -> pd.DataFrame: - POSITIONS_KEY = ["x", "y", "z"] - ORIENTATIONS_KEY = ["qw", "qx", "qy", "qz"] - target_index = 0 - df_index = 0 - data_dict = { - "x": [], - "y": [], - "z": [], - "qx": [], - "qy": [], - "qz": [], - "qw": [], - "timestamp": [], - } - 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][POSITIONS_KEY] - next_position = df_pose.iloc[df_index + 1][POSITIONS_KEY] - target_position = curr_position * curr_weight + next_position * next_weight - - curr_orientation = df_pose.iloc[df_index][ORIENTATIONS_KEY] - next_orientation = df_pose.iloc[df_index + 1][ORIENTATIONS_KEY] - 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_timestamp[target_index]) - data_dict["x"].append(target_position[0]) - data_dict["y"].append(target_position[1]) - data_dict["z"].append(target_position[2]) - data_dict["qw"].append(target_orientation[0]) - data_dict["qx"].append(target_orientation[1]) - data_dict["qy"].append(target_orientation[2]) - data_dict["qz"].append(target_orientation[3]) - target_index += 1 - result_df = pd.DataFrame(data_dict) - return result_df - - def parse_args(): parser = argparse.ArgumentParser() parser.add_argument("rosbag_path", type=pathlib.Path) From 6ca7c4c6610187ce7d6f12a896c47362282de37c Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 27 Feb 2024 05:50:05 +0000 Subject: [PATCH 13/22] style(pre-commit): autofix --- localization/ndt_evaluation/plot_box.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_evaluation/plot_box.py b/localization/ndt_evaluation/plot_box.py index 48cd7148..6aa1da92 100644 --- a/localization/ndt_evaluation/plot_box.py +++ b/localization/ndt_evaluation/plot_box.py @@ -2,6 +2,7 @@ import pathlib import geometry_msgs.msg +from interpolate_pose import interpolate_pose import matplotlib.pyplot as plt import nav_msgs.msg import numpy as np @@ -12,7 +13,6 @@ from scipy.spatial.transform import Rotation from scipy.spatial.transform import Slerp import tf_transformations -from interpolate_pose import interpolate_pose def euler_from_quaternion(quaternion): From 70d403fe560daf0a1fcb271ebbff7a596ef0d5f8 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 27 Feb 2024 14:59:27 +0900 Subject: [PATCH 14/22] Fixed import Signed-off-by: Shintaro Sakoda --- localization/ndt_evaluation/plot_box.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/localization/ndt_evaluation/plot_box.py b/localization/ndt_evaluation/plot_box.py index 6aa1da92..083bffc2 100644 --- a/localization/ndt_evaluation/plot_box.py +++ b/localization/ndt_evaluation/plot_box.py @@ -1,6 +1,5 @@ import argparse import pathlib - import geometry_msgs.msg from interpolate_pose import interpolate_pose import matplotlib.pyplot as plt @@ -10,8 +9,6 @@ from rclpy.serialization import deserialize_message import rosbag2_py from rosidl_runtime_py.utilities import get_message -from scipy.spatial.transform import Rotation -from scipy.spatial.transform import Slerp import tf_transformations From dfcefd66f77987449d94e8d43057dac96099e1c2 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 27 Feb 2024 15:18:30 +0900 Subject: [PATCH 15/22] Added fix_timestamp_awsim_rosbag.py Signed-off-by: Shintaro Sakoda --- .../fix_timestamp_awsim_rosbag.py | 71 +++++++++++++++++++ 1 file changed, 71 insertions(+) create mode 100644 localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py diff --git a/localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py b/localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py new file mode 100644 index 00000000..d2242b0c --- /dev/null +++ b/localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py @@ -0,0 +1,71 @@ +"""Fix timestamp of a rosbag which is recorded by AWSIM. + +When a rosbag is recorded by AWSIM without '--use-sim-time' option, +the timestamp in the header of each message is simulated time, but the timestamp in the rosbag file is the wall time. +This script fixes the timestamp in the rosbag file to match the simulated time. +""" + +import argparse +import pathlib +import rosbag2_py +from rclpy.serialization import deserialize_message +from rosidl_runtime_py.utilities import get_message + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument("rosbag_path", type=pathlib.Path) + return parser.parse_args() + + +if __name__ == "__main__": + args = parse_args() + rosbag_path = args.rosbag_path + + serialization_format = "cdr" + storage_options = rosbag2_py.StorageOptions(uri=str(rosbag_path), storage_id="sqlite3") + 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))} + + tuple_list = [] + + first_timestamp = None + + while reader.has_next(): + (topic_name, data, timestamp_rosbag) = reader.read_next() + + msg_type = get_message(type_map[topic_name]) + msg = deserialize_message(data, msg_type) + if hasattr(msg, "header"): + timestamp_header = int(int(msg.header.stamp.sec) * 1e9 + int(msg.header.stamp.nanosec)) + if first_timestamp is None: + first_timestamp = timestamp_header + else: + # /tf_static does not have header, so use the first timestamp - 1 + # (/tf_static should be at the beginning of the rosbag file) + timestamp_header = 0 if first_timestamp is None else first_timestamp - 1 + tuple_list.append((topic_name, data, timestamp_header)) + + # write rosbag + rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent + filtered_rosbag_path = rosbag_dir / "input_bag_sim_time" + writer = rosbag2_py.SequentialWriter() + storage_options = rosbag2_py.StorageOptions(uri=str(filtered_rosbag_path), storage_id="sqlite3") + writer.open(storage_options, converter_options) + for topic_name, topic_type in type_map.items(): + topic_info = rosbag2_py.TopicMetadata( + name=topic_name, type=topic_type, serialization_format=serialization_format + ) + writer.create_topic(topic_info) + for topic_name, data, timestamp_rosbag in tuple_list: + writer.write(topic_name, data, timestamp_rosbag) + + print(f"rosbag is saved at {filtered_rosbag_path}") From 9cfab6c3db5f952654a2d620fffaff514cc66ce0 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 27 Feb 2024 06:18:45 +0000 Subject: [PATCH 16/22] style(pre-commit): autofix --- localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py | 3 ++- localization/ndt_evaluation/plot_box.py | 1 + 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py b/localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py index d2242b0c..02033e9d 100644 --- a/localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py +++ b/localization/ndt_evaluation/fix_timestamp_awsim_rosbag.py @@ -7,8 +7,9 @@ import argparse import pathlib -import rosbag2_py + from rclpy.serialization import deserialize_message +import rosbag2_py from rosidl_runtime_py.utilities import get_message diff --git a/localization/ndt_evaluation/plot_box.py b/localization/ndt_evaluation/plot_box.py index 083bffc2..6e9b4152 100644 --- a/localization/ndt_evaluation/plot_box.py +++ b/localization/ndt_evaluation/plot_box.py @@ -1,5 +1,6 @@ import argparse import pathlib + import geometry_msgs.msg from interpolate_pose import interpolate_pose import matplotlib.pyplot as plt From 154749bd563b81d5734de9650304ea79795a6d22 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 5 Mar 2024 11:02:20 +0900 Subject: [PATCH 17/22] Updated convert_rosbag_for_ndt_evaluation.py Signed-off-by: Shintaro Sakoda --- .../convert_rosbag_for_ndt_evaluation.py | 66 ++++++++----------- 1 file changed, 28 insertions(+), 38 deletions(-) diff --git a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py index d1ae32e7..8f0905f0 100644 --- a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py +++ b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py @@ -13,14 +13,19 @@ import rosbag2_py from rosidl_runtime_py.utilities import get_message -REQUIRED_FPS = { +REQUIRED_FPS_PATTERN_A = { "/localization/kinematic_state": 40, - "/localization/util/downsample/pointcloud": 9, + "/localization/util/downsample/pointcloud": 7.5, "/sensing/vehicle_velocity_converter/twist_with_covariance": 20, "/sensing/imu/imu_data": 20, "/tf_static": 0, "/sensing/gnss/pose_with_covariance": -1, # optional topic - "/initialpose": -1, # optional topic +} + +REQUIRED_FPS_PATTERN_B = { + "/localization/kinematic_state": 40, + "/localization/util/downsample/pointcloud": 7.5, + "/localization/twist_estimator/twist_with_covariance": 20, } @@ -39,7 +44,7 @@ def parse_args(): return parser.parse_args() -def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: +def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: dict) -> bool: """Check if the rosbag is suitable for ndt evaluation.""" print(f"{duration=:.1f} sec") @@ -49,40 +54,11 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: "count": [len(msg_list) for msg_list in topic_name_to_msg_list.values()], } ) + df = df[df['topic'].isin(required_fps.keys())] df["fps"] = df["count"] / duration - df["enough_fps"] = df["fps"] > df["topic"].map(REQUIRED_FPS) + df["enough_fps"] = df["fps"] > df["topic"].map(required_fps) print(df) - - # check - # All topics must have enough fps - assert df[ - "enough_fps" - ].all(), f"NG! FPS is not enough in {df[df['enough_fps'] == False]['topic'].values}" - - # Either /sensing/gnss/pose_with_covariance or /initialpose must be included. - assert ( - len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) >= 1 - or len(topic_name_to_msg_list["/initialpose"]) >= 1 - ), "NG! Neither /sensing/gnss/pose_with_covariance nor /initialpose is found." - - # [Warning] Vehicle should be stopping still for about the first 10 seconds - for msg in topic_name_to_msg_list["/localization/kinematic_state"]: - if msg.header.stamp.sec > 10: - break - twist = msg.twist.twist - ok = ( - twist.linear.x < 0.1 - and twist.linear.y < 0.1 - and twist.linear.z < 0.1 - and twist.angular.x < 0.1 - and twist.angular.y < 0.1 - and twist.angular.z < 0.1 - ) - if not ok: - print(f"Warning: Vehicle is not stopping. time = {msg.header.stamp.sec}") - break - - print("OK") + return df["enough_fps"].all() if __name__ == "__main__": @@ -103,7 +79,8 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: reader.open(storage_options, converter_options) # filter topics - target_topics = list(REQUIRED_FPS.keys()) + target_topics = list(REQUIRED_FPS_PATTERN_A.keys()) + list(REQUIRED_FPS_PATTERN_B.keys()) + target_topics = list(set(target_topics)) if reference_topic_name not in target_topics: target_topics.append(reference_topic_name) storage_filter = rosbag2_py.StorageFilter(topics=target_topics) @@ -128,7 +105,16 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: duration = (tuple_list[-1][2] - tuple_list[0][2]) * 1e-9 # check - check_rosbag(duration, topic_name_to_msg_list) + save_topics = list() + if check_rosbag(duration, topic_name_to_msg_list, REQUIRED_FPS_PATTERN_A): + save_topics = list(REQUIRED_FPS_PATTERN_A.keys()) + print("OK as pattern A") + elif check_rosbag(duration, topic_name_to_msg_list, REQUIRED_FPS_PATTERN_B): + save_topics = list(REQUIRED_FPS_PATTERN_B.keys()) + print("OK as pattern B") + else: + print("The rosbag is not suitable for ndt evaluation") + exit() # write rosbag rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent @@ -137,6 +123,8 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: storage_options = rosbag2_py.StorageOptions(uri=str(filtered_rosbag_path), storage_id="sqlite3") writer.open(storage_options, converter_options) for topic_name, topic_type in type_map.items(): + if topic_name not in save_topics: + continue if topic_name == reference_topic_name: topic_name = "/localization/reference_kinematic_state" topic_info = rosbag2_py.TopicMetadata( @@ -144,6 +132,8 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict) -> None: ) writer.create_topic(topic_info) for topic_name, data, timestamp_rosbag in tuple_list: + if topic_name not in save_topics: + continue if topic_name == reference_topic_name: topic_name = "/localization/reference_kinematic_state" writer.write(topic_name, data, timestamp_rosbag) From 55f920bede210e5a3270f59ec1a8ca454ee3f0d9 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 5 Mar 2024 02:13:34 +0000 Subject: [PATCH 18/22] style(pre-commit): autofix --- .../ndt_evaluation/convert_rosbag_for_ndt_evaluation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py index 8f0905f0..a0f15651 100644 --- a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py +++ b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py @@ -54,7 +54,7 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di "count": [len(msg_list) for msg_list in topic_name_to_msg_list.values()], } ) - df = df[df['topic'].isin(required_fps.keys())] + df = df[df["topic"].isin(required_fps.keys())] df["fps"] = df["count"] / duration df["enough_fps"] = df["fps"] > df["topic"].map(required_fps) print(df) From 7cf0ce0abc65a46349736742b1cbdefbec877809 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Tue, 5 Mar 2024 11:33:33 +0900 Subject: [PATCH 19/22] Added initialpose Signed-off-by: Shintaro Sakoda --- .../convert_rosbag_for_ndt_evaluation.py | 26 ++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py index a0f15651..40688709 100644 --- a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py +++ b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py @@ -9,9 +9,10 @@ import pathlib import pandas as pd -from rclpy.serialization import deserialize_message +from rclpy.serialization import deserialize_message, serialize_message import rosbag2_py from rosidl_runtime_py.utilities import get_message +from geometry_msgs.msg import PoseWithCovarianceStamped REQUIRED_FPS_PATTERN_A = { "/localization/kinematic_state": 40, @@ -41,6 +42,9 @@ def parse_args(): "/awsim/ground_truth/localization/kinematic_state", ], ) + parser.add_argument( + "--add_initialpose", + action="store_true") return parser.parse_args() @@ -116,6 +120,26 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di print("The rosbag is not suitable for ndt evaluation") exit() + # get first msg from /localization/kinematic_state + first_reference = topic_name_to_msg_list[reference_topic_name][0] + + # if there is not gnss topic or selected from args, add initialpose + save_initialpose = ( + len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) == 0 or args.add_initialpose) + if save_initialpose: + print("Add /initialpose") + type_map["/initialpose"] = "geometry_msgs/msg/PoseWithCovarianceStamped" + save_topics.append("/initialpose") + stamp = first_reference.header.stamp + msg = PoseWithCovarianceStamped() + msg.header.stamp = stamp + msg.pose = first_reference.pose + data = serialize_message(msg) + timestamp = int(stamp.sec * 1e9 + stamp.nanosec) + tuple_list.append( + ("/initialpose", data, timestamp) + ) + # write rosbag rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent filtered_rosbag_path = rosbag_dir / "input_bag" From 4d344a615cf2de0337f00c92b704f2529a064f3f Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Tue, 5 Mar 2024 02:33:49 +0000 Subject: [PATCH 20/22] style(pre-commit): autofix --- .../convert_rosbag_for_ndt_evaluation.py | 17 ++++++++--------- 1 file changed, 8 insertions(+), 9 deletions(-) diff --git a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py index 40688709..d2750f00 100644 --- a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py +++ b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py @@ -8,11 +8,12 @@ import argparse import pathlib +from geometry_msgs.msg import PoseWithCovarianceStamped import pandas as pd -from rclpy.serialization import deserialize_message, serialize_message +from rclpy.serialization import deserialize_message +from rclpy.serialization import serialize_message import rosbag2_py from rosidl_runtime_py.utilities import get_message -from geometry_msgs.msg import PoseWithCovarianceStamped REQUIRED_FPS_PATTERN_A = { "/localization/kinematic_state": 40, @@ -42,9 +43,7 @@ def parse_args(): "/awsim/ground_truth/localization/kinematic_state", ], ) - parser.add_argument( - "--add_initialpose", - action="store_true") + parser.add_argument("--add_initialpose", action="store_true") return parser.parse_args() @@ -125,7 +124,9 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di # if there is not gnss topic or selected from args, add initialpose save_initialpose = ( - len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) == 0 or args.add_initialpose) + len(topic_name_to_msg_list["/sensing/gnss/pose_with_covariance"]) == 0 + or args.add_initialpose + ) if save_initialpose: print("Add /initialpose") type_map["/initialpose"] = "geometry_msgs/msg/PoseWithCovarianceStamped" @@ -136,9 +137,7 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di msg.pose = first_reference.pose data = serialize_message(msg) timestamp = int(stamp.sec * 1e9 + stamp.nanosec) - tuple_list.append( - ("/initialpose", data, timestamp) - ) + tuple_list.append(("/initialpose", data, timestamp)) # write rosbag rosbag_dir = rosbag_path.parent if rosbag_path.is_dir() else rosbag_path.parent.parent From 90aa6b31609a67dad91c16a1b350c4654d114420 Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Thu, 7 Mar 2024 13:56:44 +0900 Subject: [PATCH 21/22] FIxed header Signed-off-by: Shintaro Sakoda --- .../ndt_evaluation/convert_rosbag_for_ndt_evaluation.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py index d2750f00..e73d7c65 100644 --- a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py +++ b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py @@ -133,7 +133,7 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di save_topics.append("/initialpose") stamp = first_reference.header.stamp msg = PoseWithCovarianceStamped() - msg.header.stamp = stamp + msg.header = first_reference.header msg.pose = first_reference.pose data = serialize_message(msg) timestamp = int(stamp.sec * 1e9 + stamp.nanosec) From ae38c33b45991ddb2477983f26b9d154e90f3b8c Mon Sep 17 00:00:00 2001 From: Shintaro Sakoda Date: Mon, 25 Mar 2024 08:41:49 +0900 Subject: [PATCH 22/22] Fixed to use lidar timestamp Signed-off-by: Shintaro Sakoda --- .../ndt_evaluation/convert_rosbag_for_ndt_evaluation.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py index e73d7c65..9f669ba3 100644 --- a/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py +++ b/localization/ndt_evaluation/convert_rosbag_for_ndt_evaluation.py @@ -98,6 +98,7 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di # read rosbag topic_name_to_msg_list = {topic: [] for topic in target_topics} tuple_list = [] + topic_name_to_rosbag_timestamp = {topic: [] for topic in target_topics} while reader.has_next(): (topic_name, data, timestamp_rosbag) = reader.read_next() tuple_list.append((topic_name, data, timestamp_rosbag)) @@ -105,6 +106,7 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di msg_type = get_message(type_map[topic_name]) msg = deserialize_message(data, msg_type) topic_name_to_msg_list[topic_name].append(msg) + topic_name_to_rosbag_timestamp[topic_name].append(timestamp_rosbag) duration = (tuple_list[-1][2] - tuple_list[0][2]) * 1e-9 # check @@ -136,7 +138,8 @@ def check_rosbag(duration: float, topic_name_to_msg_list: dict, required_fps: di msg.header = first_reference.header msg.pose = first_reference.pose data = serialize_message(msg) - timestamp = int(stamp.sec * 1e9 + stamp.nanosec) + first_pcd = topic_name_to_rosbag_timestamp[reference_topic_name][0] + timestamp = first_pcd + int(0.5 * 1e9) tuple_list.append(("/initialpose", data, timestamp)) # write rosbag