From 3a75e610eb668352f71d40014353046eafc49f35 Mon Sep 17 00:00:00 2001 From: junlinp Date: Wed, 29 Apr 2026 18:28:40 +0800 Subject: [PATCH] feat(map): prioritize path search by sdf distance buckets --- tinynav/core/map_node.py | 52 +++++++++++++++++++++++++++++++++------- 1 file changed, 43 insertions(+), 9 deletions(-) diff --git a/tinynav/core/map_node.py b/tinynav/core/map_node.py index 70ba4b0..232866b 100644 --- a/tinynav/core/map_node.py +++ b/tinynav/core/map_node.py @@ -117,13 +117,35 @@ def search_close_to_sdf_map(start_index:tuple, sdf_map:np.ndarray, occupancy_map def search_within_sdf_map( start:tuple, goal:tuple, sdf_map:np.ndarray, occupancy_map:np.ndarray, resolution: float): start = tuple(start.flatten()) if isinstance(start, np.ndarray) else start goal = tuple(goal.flatten()) if isinstance(goal, np.ndarray) else goal - open_heap = [(sdf_map[start] + heuristic(start, goal, resolution), start)] - open_heap_set = set() - open_heap_set.add(start) + sdf_bins = [0.2, 0.5, 1.0, 2.0, 5.0, 10.0] + + def get_queue_index(sdf_value: float) -> int: + for idx, threshold in enumerate(sdf_bins): + if sdf_value < threshold: + return idx + return len(sdf_bins) + + open_heaps = [[] for _ in range(len(sdf_bins) + 1)] + open_sets = [set() for _ in range(len(sdf_bins) + 1)] + start_queue_idx = get_queue_index(float(sdf_map[start])) + heapq.heappush(open_heaps[start_queue_idx], (heuristic(start, goal, resolution), start)) + open_sets[start_queue_idx].add(start) parent = {start: start} visited = set() - while len(open_heap) > 0: - current_cost, current = heapq.heappop(open_heap) + + while True: + queue_idx = -1 + for i, q in enumerate(open_heaps): + if len(q) > 0: + queue_idx = i + break + if queue_idx == -1: + break + + current_cost, current = heapq.heappop(open_heaps[queue_idx]) + open_sets[queue_idx].remove(current) + if current in visited: + continue visited.add(current) if current == goal: return reconstruct_path_sdf(parent, current) @@ -136,9 +158,18 @@ def search_within_sdf_map( start:tuple, goal:tuple, sdf_map:np.ndarray, occupanc if (0 <= neighbor[0] < sdf_map.shape[0] and 0 <= neighbor[1] < sdf_map.shape[1] and 0 <= neighbor[2] < sdf_map.shape[2]): - if neighbor not in open_heap_set and neighbor not in visited and occupancy_map[neighbor] != 2 and sdf_map[neighbor] < 0.2: - open_heap_set.add(neighbor) - heapq.heappush(open_heap, (heuristic(neighbor, goal, resolution) + sdf_map[neighbor], neighbor)) + if neighbor in visited or occupancy_map[neighbor] == 2: + continue + neighbor_sdf = float(sdf_map[neighbor]) + neighbor_queue_idx = get_queue_index(neighbor_sdf) + if neighbor in open_sets[neighbor_queue_idx]: + continue + open_sets[neighbor_queue_idx].add(neighbor) + heapq.heappush( + open_heaps[neighbor_queue_idx], + (heuristic(neighbor, goal, resolution), neighbor), + ) + if neighbor not in parent: parent[neighbor] = current return [] @@ -672,6 +703,10 @@ def generate_nav_path_in_map(self, pose_in_map: np.ndarray, target_poi: np.ndarr sdf_start_sdf = sdf_start_path[-1] sdf_goal_sdf = sdf_goal_path[-1] path_sdf = search_within_sdf_map(sdf_start_sdf, sdf_goal_sdf, self.sdf_map, self.occupancy_map, resolution) + if len(path_sdf) == 0: + self.get_logger().warning( + f"search_within_sdf_map returned empty path: start_idx={tuple(sdf_start_sdf)}, goal_idx={tuple(sdf_goal_sdf)}" + ) path = sdf_start_path + path_sdf + sdf_goal_path[::-1] if len(path) > 0: converted_path = np.array(path) * resolution + occupancy_map_origin @@ -701,4 +736,3 @@ def main(args=None): if __name__ == '__main__': main() -