diff --git a/map/autoware_tp_manager/scripts/tp_checker.py b/map/autoware_tp_manager/scripts/tp_checker.py index aa4e833b..7cd151be 100755 --- a/map/autoware_tp_manager/scripts/tp_checker.py +++ b/map/autoware_tp_manager/scripts/tp_checker.py @@ -211,14 +211,14 @@ def __set_color_based_on_mark(self, mark) -> int: rgba = struct.unpack("I", tmp_rgb)[0] return rgba - + def __find_candidate_segments(self, stamp, t_pose): - if not(self.scan_df is None) and self.scan_df.shape[0] > 0: + if not (self.scan_df is None) and self.scan_df.shape[0] > 0: sid = tpu.stamp_search(stamp, self.scan_df, self.scan_df.shape[0]) if sid < 0: return None - + closest_scan = pc2.read_points(self.scan_df[sid, 1], skip_nans=True) segment_set = set() @@ -232,11 +232,11 @@ def __find_candidate_segments(self, stamp, t_pose): seg_idx = str(sx) + "_" + str(sy) segment_set.add(seg_idx) - + return segment_set else: segment_set = set() - # If scans are not available, use range query + # If scans are not available, use range query lx = int((t_pose[3, 0] - self.range) / self.resolution) * int(self.resolution) ux = int((t_pose[3, 0] + self.range) / self.resolution) * int(self.resolution) ly = int((t_pose[3, 1] - self.range) / self.resolution) * int(self.resolution) @@ -248,7 +248,6 @@ def __find_candidate_segments(self, stamp, t_pose): return segment_set - def __mark_changed_poses(self): progress_bar = tqdm.tqdm(total=self.pose_df.shape[0]) @@ -280,7 +279,7 @@ def __mark_changed_poses(self): if segment_set is None: continue - + tp_sum = 0.0 valid_segment_num = 0 @@ -381,7 +380,7 @@ def processing( tp_topic: str, scan_topic: str, mode: str, - range: float + range: float, ): if range > 0: self.range = range @@ -465,5 +464,11 @@ def processing( checker = TPChecker() checker.processing( - args.score_path, args.bag_path, args.pose_topic, args.tp_topic, args.scan_topic, args.mode, args.range + args.score_path, + args.bag_path, + args.pose_topic, + args.tp_topic, + args.scan_topic, + args.mode, + args.range, ) diff --git a/map/autoware_tp_manager/scripts/tp_collector.py b/map/autoware_tp_manager/scripts/tp_collector.py index 6217a2f2..122af800 100755 --- a/map/autoware_tp_manager/scripts/tp_collector.py +++ b/map/autoware_tp_manager/scripts/tp_collector.py @@ -115,7 +115,7 @@ def __find_candidate_segments(self, stamp, t_pose): if sid < 0: return None - + closest_scan = pc2.read_points(self.scan_df[sid, 1], skip_nans=True) segment_set = set() @@ -129,21 +129,21 @@ def __find_candidate_segments(self, stamp, t_pose): seg_idx = str(sx) + "_" + str(sy) segment_set.add(seg_idx) - + return segment_set else: - # If scans are not available, use range query + # If scans are not available, use range query lx = int((t_pose[3, 0] - 50.0) / self.resolution) * int(self.resolution) ux = int((t_pose[3, 0] + 50.0) / self.resolution) * int(self.resolution) ly = int((t_pose[3, 1] - 50.0) / self.resolution) * int(self.resolution) uy = int((t_pose[3, 1] + 50.0) / self.resolution) * int(self.resolution) - for sx in range(start = lx, stop = ux, step = 1): - for sy in range(start = ly, stop = uy, step = 1): + for sx in range(start=lx, stop=ux, step=1): + for sy in range(start=ly, stop=uy, step=1): segment_set.add(str(sx) + "_" + str(sy)) return segment_set - + def __update_tp_by_scan(self): progress_bar = tqdm.tqdm(total=self.scan_df.shape[0]) @@ -232,7 +232,7 @@ def __update_tp_by_pose(self): if segment_set is None: continue - + for key in segment_set: if key in self.segment_dict: i = self.segment_dict[key] @@ -246,7 +246,7 @@ def __update_tp_by_pose(self): def __update_avg_tp(self): - if not(self.scan_df is None) and self.scan_df.shape[0] > 0: + if not (self.scan_df is None) and self.scan_df.shape[0] > 0: self.__update_tp_by_scan() else: self.__update_tp_by_pose() diff --git a/map/autoware_tp_manager/scripts/tp_utility.py b/map/autoware_tp_manager/scripts/tp_utility.py index 6d6e10b1..56c0483f 100644 --- a/map/autoware_tp_manager/scripts/tp_utility.py +++ b/map/autoware_tp_manager/scripts/tp_utility.py @@ -26,6 +26,7 @@ from tier4_debug_msgs.msg import Float32Stamped import tqdm + # Some utility functions that is shared by both the tp collector and tp checker # Convert a pose to a 4x4 transformation matrix def __pose_to_mat(pose: Pose) -> np.array: @@ -99,7 +100,7 @@ def parse_rosbag(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str) msg_count_by_topic = __get_message_count(bag_path) progress_bar = tqdm.tqdm(total=msg_count_by_topic["total"]) - if not(pose_topic in msg_count_by_topic) or (msg_count_by_topic[pose_topic] == 0): + if not (pose_topic in msg_count_by_topic) or (msg_count_by_topic[pose_topic] == 0): print( "Error: the input rosbag contains no message from the topic {0}. Exit!".format( pose_topic @@ -111,7 +112,7 @@ def parse_rosbag(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str) pose_df = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object) pose_to_publish = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object) - if not(tp_topic in msg_count_by_topic) or (msg_count_by_topic[tp_topic] == 0): + if not (tp_topic in msg_count_by_topic) or (msg_count_by_topic[tp_topic] == 0): print( "Error: the input rosbag contains no message from the topic {0}. Exit!".format(tp_topic) ) @@ -119,16 +120,12 @@ def parse_rosbag(bag_path: str, pose_topic: str, tp_topic: str, scan_topic: str) else: tp_df = np.ndarray((msg_count_by_topic[tp_topic], 2), dtype=object) - if not(scan_topic in msg_count_by_topic) or (msg_count_by_topic[scan_topic] == 0): - print( - "The input rosbag contains no message from the topic {0}.!".format( - scan_topic - ) - ) + if not (scan_topic in msg_count_by_topic) or (msg_count_by_topic[scan_topic] == 0): + print("The input rosbag contains no message from the topic {0}.!".format(scan_topic)) scan_df = None else: scan_df = np.ndarray((msg_count_by_topic[scan_topic], 2), dtype=object) - + pose_df_idx = 0 tp_df_idx = 0 scan_df_idx = 0