diff --git a/map/autoware_tp_manager/scripts/tp_checker.py b/map/autoware_tp_manager/scripts/tp_checker.py index 68c32e3a..aa4e833b 100755 --- a/map/autoware_tp_manager/scripts/tp_checker.py +++ b/map/autoware_tp_manager/scripts/tp_checker.py @@ -44,6 +44,7 @@ def __init__(self): self.changed_dir = None # A directory contains the segments that need to be examined self.result_csv = None # Path to the result CSV file self.tp_path = None # Path to the file that contains TPs of map segments + self.range = 50.0 def __initialize(self, score_dir: str): if not os.path.exists(score_dir): @@ -210,6 +211,43 @@ 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: + 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() + + for p in closest_scan: + tp = tpu.transform_p(p, t_pose) + + # Hash the point to find the segment containing it + sx = int(tp[0] / self.resolution) * int(self.resolution) + sy = int(tp[1] / self.resolution) * int(self.resolution) + 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 + 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) + uy = int((t_pose[3, 1] + self.range) / self.resolution) * int(self.resolution) + + for sx in range(lx, ux + 1, 1): + for sy in range(ly, uy + 1, 1): + segment_set.add(str(sx) + "_" + str(sy)) + + return segment_set + def __mark_changed_poses(self): progress_bar = tqdm.tqdm(total=self.pose_df.shape[0]) @@ -229,30 +267,20 @@ def __mark_changed_poses(self): # print("Invalid pose. Skip!") continue - # Find the closest scan and tp - sid = tpu.stamp_search(stamp, self.scan_df, self.scan_df.shape[0]) + # Find the closest tp tid = tpu.stamp_search(stamp, self.tp_df, self.tp_df.shape[0]) - if sid < 0 or tid < 0: - print("No closest scan and tp were found. Skip!") + if tid < 0: + print("No closest tp were found. Skip!") continue - - 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 - segment_set = set() - - for p in closest_scan: - tp = tpu.transform_p(p, pose) - - # Hash the point to find the segment containing it - sx = int(tp[0] / self.resolution) * int(self.resolution) - sy = int(tp[1] / self.resolution) * int(self.resolution) - seg_idx = str(sx) + "_" + str(sy) - - segment_set.add(seg_idx) + # Find the candidate segments that cover the pose + segment_set = self.__find_candidate_segments(stamp, pose) + if segment_set is None: + continue + tp_sum = 0.0 valid_segment_num = 0 @@ -353,7 +381,10 @@ def processing( tp_topic: str, scan_topic: str, mode: str, + range: float ): + if range > 0: + self.range = range self.__initialize(score_path) self.__get_pcd_segments_and_scores() @@ -411,6 +442,13 @@ def processing( required=False, type=str, ) + parser.add_argument( + "--range", + help="The range to query segments that cover a pose", + default=50.0, + required=False, + type=float, + ) args = parser.parse_args() @@ -421,10 +459,11 @@ def processing( print("Topic of Transformation Probability: {0}".format(args.tp_topic)) print("Topic of scan data: {0}".format(args.scan_topic)) print("Mode of checking: {0}".format(args.mode)) + print("Range to query map segments that cover a pose: {0}".format(args.range)) # Run 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, args.range ) diff --git a/map/autoware_tp_manager/scripts/tp_collector.py b/map/autoware_tp_manager/scripts/tp_collector.py index b44cdff3..6217a2f2 100755 --- a/map/autoware_tp_manager/scripts/tp_collector.py +++ b/map/autoware_tp_manager/scripts/tp_collector.py @@ -51,8 +51,6 @@ def __initialize(self, pcd_map_dir: str, output_path: str, resolution: float): self.output_path = output_path self.resolution = resolution - print("Type resolution = {0}".format(type(self.resolution))) - if not os.path.exists(pcd_map_dir): print("Error: {0} does not exist!".format(pcd_map_dir)) exit() @@ -111,7 +109,42 @@ def __get_pcd_segments_and_scores(self): for index, row in enumerate(reader): self.segment_df[index, [1, 2]] = [float(row[1]), 1] - def __update_avg_tp(self): + def __find_candidate_segments(self, stamp, t_pose): + if 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() + + for p in closest_scan: + tp = tpu.transform_p(p, t_pose) + + # Hash the point to find the segment containing it + sx = int(tp[0] / self.resolution) * int(self.resolution) + sy = int(tp[1] / self.resolution) * int(self.resolution) + seg_idx = str(sx) + "_" + str(sy) + + segment_set.add(seg_idx) + + return segment_set + else: + # 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): + 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]) for i in range(self.scan_df.shape[0]): @@ -158,6 +191,66 @@ def __update_avg_tp(self): progress_bar.close() + def __update_tp_by_pose(self): + # If scans are not available, examine pose and update TPs of segments in vicinity + progress_bar = tqdm.tqdm(total=self.pose_df.shape[0]) + + for i in range(self.pose_df.shape[0]): + progress_bar.update(1) + stamp, pose = self.pose_df[i, :] + # Transposed pose for multiplication, as the points are in a row-major table + pose = pose.T + # Skip invalid poses + # print("Pose at {0} = {1}".format(i, pose)) + if pose is None: + # print("None pose. Skip!") + continue + + if pose[3, 0] == 0 and pose[3, 1] == 0 and pose[3, 2] == 0: + # print("Invalid pose. Skip!") + continue + + # Find the closest tp + tid = tpu.stamp_search(stamp, self.tp_df, self.tp_df.shape[0]) + + if tid < 0: + print("No closest tp were found. Skip!") + continue + closest_tp = self.tp_df[tid, 1] + + # Use range query to find segments that cover the current pose + segment_set = set() + + lx = int((pose[3, 0] - 50.0) / self.resolution) * int(self.resolution) + ux = int((pose[3, 0] + 50.0) / self.resolution) * int(self.resolution) + ly = int((pose[3, 1] - 50.0) / self.resolution) * int(self.resolution) + uy = int((pose[3, 1] + 50.0) / self.resolution) * int(self.resolution) + + for sx in range(lx, ux + 1): + for sy in range(ly, uy + 1, 1): + segment_set.add(str(sx) + "_" + str(sy)) + + if segment_set is None: + continue + + for key in segment_set: + 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, + ] + + progress_bar.close() + + def __update_avg_tp(self): + + 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() + # Save the segment TPs def __save_tps(self): print("Saving TP to files") diff --git a/map/autoware_tp_manager/scripts/tp_utility.py b/map/autoware_tp_manager/scripts/tp_utility.py index 3628c432..6d6e10b1 100644 --- a/map/autoware_tp_manager/scripts/tp_utility.py +++ b/map/autoware_tp_manager/scripts/tp_utility.py @@ -26,64 +26,6 @@ from tier4_debug_msgs.msg import Float32Stamped import tqdm - -class FixQueue: - def __init__(self): - self.queue_limit = 100 # Max number of items in the queue - self.data_queue = None # Queue of data - self.head = 0 # Index to append a new item (enqueue index) - self.tail = 0 # Index to start remove/process items (dequeue index) - self.current_size = 0 # Number of items in the queue - - def enqueue(self, item): - if self.is_full(): - print("Cannot add new item. Queue is full!") - return False - - if self.is_empty() or self.data_queue.shape[1] != len(item): - self.data_queue = np.ndarray((self.queue_limit, len(item)), dtype=object) - self.data_queue[self.tail, :] = item - self.tail += 1 - - if self.tail == self.queue_limit: - self.tail = 0 - - self.current_size += 1 - - return True - - def drop(self, limitless=False, callback_func=None, *args): - if limitless: - end_id = self.tail - else: - end_id = self.head + int(self.queue_limit / 2) - - if self.head < end_id: - drop_list = np.arange(self.head, end_id) - else: - drop_list = np.arange(self.head, end_id + self.queue_limit) % self.queue_limit - - for i in drop_list: - # Process the item at index i - callback_func(self.data_queue[i], *args) - - self.current_size -= len(drop_list) - self.head = end_id - - if self.head == self.queue_limit: - self.head = 0 - - def is_full(self) -> bool: - if self.current_size == self.queue_limit: - return True - return False - - def is_empty(self) -> bool: - if self.current_size == 0: - return True - return False - - # 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: @@ -157,27 +99,36 @@ 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 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 ) ) - if msg_count_by_topic[tp_topic] == 0: + pose_df = None + pose_to_publish = None + else: + 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): print( "Error: the input rosbag contains no message from the topic {0}. Exit!".format(tp_topic) ) - if msg_count_by_topic[scan_topic] == 0: + tp_df = None + 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( - "Error: the input rosbag contains no message from the topic {0}. Exit!".format( + "The input rosbag contains no message from the topic {0}.!".format( scan_topic ) ) - - pose_df = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object) - tp_df = np.ndarray((msg_count_by_topic[tp_topic], 2), dtype=object) - pose_to_publish = np.ndarray((msg_count_by_topic[pose_topic], 2), dtype=object) - scan_df = np.ndarray((msg_count_by_topic[scan_topic], 2), dtype=object) + 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