style(pre-commit): autofix

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

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()

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,76 +46,82 @@ 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)=}")
 
     # 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.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()