From 7e9ab3e1ef7ea525852a5f54913966da088e78c3 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Mon, 3 Mar 2025 09:14:40 +0000 Subject: [PATCH] style(pre-commit): autofix --- map/autoware_tp_manager/scripts/tp_checker.py | 39 +++++++++++-------- .../scripts/tp_collector.py | 20 +++++----- map/autoware_tp_manager/scripts/tp_utility.py | 2 +- 3 files changed, 33 insertions(+), 28 deletions(-) diff --git a/map/autoware_tp_manager/scripts/tp_checker.py b/map/autoware_tp_manager/scripts/tp_checker.py index 45b1ce70..eb39a994 100755 --- a/map/autoware_tp_manager/scripts/tp_checker.py +++ b/map/autoware_tp_manager/scripts/tp_checker.py @@ -26,12 +26,14 @@ from rclpy.node import Node import sensor_msgs.msg as sensor_msgs import sensor_msgs_py.point_cloud2 as pc2 -from visualization_msgs.msg import Marker, MarkerArray import std_msgs.msg as std_msgs import tp_utility as tpu import tqdm +from visualization_msgs.msg import Marker +from visualization_msgs.msg import MarkerArray import yaml + class TPChecker(Node): def __init__(self): @@ -146,7 +148,6 @@ def __show(self): pc2_msg = pc2.create_cloud(header, fields, points) pcd_publisher = self.create_publisher(sensor_msgs.PointCloud2, "/autoware_tp_checker", 10) - # Publish poses self.pose_pub = self.create_publisher(MarkerArray, "/pose_result", 10) marker_array_msg = MarkerArray() @@ -159,7 +160,7 @@ def __show(self): marker = Marker() marker.header.stamp = self.get_clock().now().to_msg() - marker.header.frame_id = 'map' + marker.header.frame_id = "map" marker.id = i marker.type = Marker.SPHERE marker.action = Marker.ADD @@ -173,7 +174,7 @@ def __show(self): marker.scale.y = 2.0 marker.scale.z = 2.0 - marker.color.a = 1.0 + marker.color.a = 1.0 if self.mark[i] == 1: marker.color.r = 255.0 @@ -211,7 +212,7 @@ def __set_color_based_on_mark(self, mark) -> int: return rgba def __mark_changed_poses(self): - progress_bar = tqdm.tqdm(total = self.pose_df.shape[0]) + progress_bar = tqdm.tqdm(total=self.pose_df.shape[0]) for i in range(self.pose_df.shape[0]): progress_bar.update(1) @@ -236,7 +237,7 @@ def __mark_changed_poses(self): print("No closest scan and tp were found. Skip!") continue - closest_scan = pc2.read_points(self.scan_df[sid, 1], skip_nans = True) + closest_scan = pc2.read_points(self.scan_df[sid, 1], skip_nans=True) closest_tp = self.tp_df[tid, 1] # Transform the scan and find the segments that cover the transformed points @@ -265,19 +266,20 @@ def __mark_changed_poses(self): expected_tp = 0 # If the expected tp and the actual tp is quite different, mark the pose as changed - if (expected_tp > 0 and abs(expected_tp - closest_tp) / expected_tp >= 0.2) \ - or (expected_tp == 0 and closest_tp > 0): + if (expected_tp > 0 and abs(expected_tp - closest_tp) / expected_tp >= 0.2) or ( + expected_tp == 0 and closest_tp > 0 + ): self.mark[i] = 1 progress_bar.close() - + def __mark_changed_segments(self): - progress_bar = tqdm.tqdm(total = self.pose_df.shape[0]) + progress_bar = tqdm.tqdm(total=self.pose_df.shape[0]) for i in range(self.scan_df.shape[0]): progress_bar.update(1) stamp, tmp_scan = self.scan_df[i, :] - scan = pc2.read_points(tmp_scan, skip_nans = True) + scan = pc2.read_points(tmp_scan, skip_nans=True) # Find the closest pose and tp pid = tpu.stamp_search(stamp, self.pose_df, self.pose_df.shape[0]) @@ -343,10 +345,14 @@ def __save_results(self): print("Done. Saved TPs at {0}".format(self.output_path)) - def processing( - self, score_path: str, rosbag_path: str, pose_topic: str, tp_topic: str, scan_topic: str, - mode: str + self, + score_path: str, + rosbag_path: str, + pose_topic: str, + tp_topic: str, + scan_topic: str, + mode: str, ): self.__initialize(score_path) self.__get_pcd_segments_and_scores() @@ -365,7 +371,7 @@ def processing( self.__save_results() elif mode == "check_segment": self.__mark_changed_segments() - else: + else: print("Unrecognized mode! Exit!") exit() @@ -420,6 +426,5 @@ def processing( checker = TPChecker() checker.processing( - args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic, - args.mode + args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic, args.mode ) diff --git a/map/autoware_tp_manager/scripts/tp_collector.py b/map/autoware_tp_manager/scripts/tp_collector.py index ed89e136..beecf44e 100755 --- a/map/autoware_tp_manager/scripts/tp_collector.py +++ b/map/autoware_tp_manager/scripts/tp_collector.py @@ -23,10 +23,11 @@ import numpy as np import rclpy from rclpy.node import Node +import sensor_msgs_py.point_cloud2 as pc2 import tp_utility as tpu -import yaml import tqdm -import sensor_msgs_py.point_cloud2 as pc2 +import yaml + class TPCollector(Node): def __init__(self): @@ -111,12 +112,12 @@ def __get_pcd_segments_and_scores(self): self.segment_df[index, [1, 2]] = [float(row[1]), 1] def __update_avg_tp(self): - progress_bar = tqdm.tqdm(total = self.scan_df.shape[0]) + progress_bar = tqdm.tqdm(total=self.scan_df.shape[0]) for i in range(self.scan_df.shape[0]): progress_bar.update(1) stamp, tmp_scan = self.scan_df[i, :] - scan = pc2.read_points(tmp_scan, skip_nans = True) + scan = pc2.read_points(tmp_scan, skip_nans=True) # Find the closest pose and tp pid = tpu.stamp_search(stamp, self.pose_df, self.pose_df.shape[0]) @@ -150,8 +151,10 @@ def __update_avg_tp(self): if key in self.segment_dict: i = self.segment_dict[key] tp, counter = self.segment_df[i, [1, 2]] - self.segment_df[i, [1, 2]] = [tp + 1.0 / (counter + 1) * (closest_tp - tp), counter + 1] - + self.segment_df[i, [1, 2]] = [ + tp + 1.0 / (counter + 1) * (closest_tp - tp), + counter + 1, + ] progress_bar.close() @@ -182,10 +185,7 @@ def processing( self.__get_pcd_segments_and_scores() # Read the rosbag and get the ndt poses and corresponding tps - output_dict = tpu.parse_rosbag(rosbag_path, - pose_topic, - tp_topic, - scan_topic) + output_dict = tpu.parse_rosbag(rosbag_path, pose_topic, tp_topic, scan_topic) self.pose_df = output_dict["mat_pose"] self.tp_df = output_dict["tp"] self.scan_df = output_dict["scan"] diff --git a/map/autoware_tp_manager/scripts/tp_utility.py b/map/autoware_tp_manager/scripts/tp_utility.py index ee51bd38..a68820b2 100644 --- a/map/autoware_tp_manager/scripts/tp_utility.py +++ b/map/autoware_tp_manager/scripts/tp_utility.py @@ -214,4 +214,4 @@ def parse_rosbag(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str) progress_bar.close() - return {"mat_pose" : pose_df, "pose" : pose_to_publish, "tp" : tp_df, "scan" : scan_df} + return {"mat_pose": pose_df, "pose": pose_to_publish, "tp": tp_df, "scan": scan_df}