|
| 1 | +import torch.multiprocessing as mp |
| 2 | +import numpy as np |
| 3 | +import time |
| 4 | +import traceback |
| 5 | +from queue import Empty |
| 6 | + |
| 7 | + |
| 8 | +class IGHAStarMP: |
| 9 | + """ |
| 10 | + Multiprocessing wrapper for the IGHA* planner. Runs the planner in a separate process and communicates via queues. |
| 11 | + """ |
| 12 | + |
| 13 | + def __init__(self, configs): |
| 14 | + mp.set_start_method("spawn", force=True) # Safe for CUDA |
| 15 | + self.query_queue = mp.Queue(5) |
| 16 | + self.result_queue = mp.Queue(5) |
| 17 | + self.process = mp.Process( |
| 18 | + target=self._planner_process, |
| 19 | + args=(configs, self.query_queue, self.result_queue), |
| 20 | + ) |
| 21 | + self.process.start() |
| 22 | + self.path = None |
| 23 | + self.success = False |
| 24 | + self.completed = True |
| 25 | + self.expansion_counter = 0 |
| 26 | + |
| 27 | + def _planner_process(self, configs, query_queue, result_queue): |
| 28 | + """ |
| 29 | + Planner process: loads the CUDA/C++ kernel and runs the IGHA* planner in response to queries. |
| 30 | + """ |
| 31 | + import torch |
| 32 | + import numpy as np |
| 33 | + import os |
| 34 | + import sys |
| 35 | + import pathlib |
| 36 | + |
| 37 | + BASE_DIR = pathlib.Path(__file__).resolve().parent |
| 38 | + sys.path.append(str(BASE_DIR / "src")) |
| 39 | + from torch.utils.cpp_extension import load |
| 40 | + |
| 41 | + env_name = configs["experiment_info_default"]["node_info"]["node_type"] |
| 42 | + env_macro = { |
| 43 | + "simple": "-DUSE_SIMPLE_ENV", |
| 44 | + "kinematic": "-DUSE_KINEMATIC_ENV", |
| 45 | + "kinodynamic": "-DUSE_KINODYNAMIC_ENV", |
| 46 | + }[env_name] |
| 47 | + folder_name = os.getcwd() |
| 48 | + for path in sys.path: |
| 49 | + folder_path = os.path.join(path, folder_name) |
| 50 | + if os.path.exists(folder_path): |
| 51 | + break |
| 52 | + cpp_path = f"{folder_path}/src/ighastar.cpp" |
| 53 | + cuda_path = f"{folder_path}/src/Environments/{env_name}.cu" |
| 54 | + header_path = f"{folder_path}/src/Environments" |
| 55 | + |
| 56 | + kernel = load( |
| 57 | + name="ighastar", |
| 58 | + sources=[cpp_path, cuda_path], |
| 59 | + extra_include_paths=[header_path], |
| 60 | + extra_cflags=["-std=c++17", "-O3", env_macro], |
| 61 | + extra_cuda_cflags=["-O3"], |
| 62 | + verbose=True, |
| 63 | + ) |
| 64 | + planner = kernel.IGHAStar(configs, False) |
| 65 | + print("[IGHAStarMP] Planner loaded.") |
| 66 | + |
| 67 | + map_res = configs["experiment_info_default"]["node_info"]["map_res"] |
| 68 | + offset = None |
| 69 | + avg_dt = 0.1 |
| 70 | + stream = torch.cuda.Stream() |
| 71 | + while True: |
| 72 | + task = query_queue.get() |
| 73 | + if task is None: |
| 74 | + time.sleep(0.1) |
| 75 | + continue |
| 76 | + ( |
| 77 | + map_center, |
| 78 | + start_state, |
| 79 | + goal_, |
| 80 | + costmap, |
| 81 | + heightmap, |
| 82 | + hysteresis, |
| 83 | + expansion_limit, |
| 84 | + stop, |
| 85 | + ) = task |
| 86 | + try: |
| 87 | + map_size = costmap.shape |
| 88 | + bitmap = torch.ones((map_size[0], map_size[1], 2), dtype=torch.float32) |
| 89 | + bitmap[..., 0] = torch.from_numpy(costmap) |
| 90 | + bitmap[..., 1] = torch.from_numpy(heightmap) |
| 91 | + if offset is None: |
| 92 | + offset = map_res * np.array(bitmap.shape[:2]) * 0.5 |
| 93 | + start = torch.zeros(4, dtype=torch.float32) |
| 94 | + goal = torch.zeros(4, dtype=torch.float32) |
| 95 | + start[:2] = torch.from_numpy(start_state[:2] + offset - map_center) |
| 96 | + goal[:2] = torch.from_numpy(goal_[:2] + offset - map_center) |
| 97 | + start[2] = start_state[5] |
| 98 | + start[3] = float(np.linalg.norm(start_state[6:8])) |
| 99 | + if start[3] > 1.0 and start_state[6] > 0.5: |
| 100 | + V = start_state[6:8] |
| 101 | + theta = start_state[5] |
| 102 | + dx = V[0] * np.cos(theta) - V[1] * np.sin(theta) |
| 103 | + dy = V[0] * np.sin(theta) + V[1] * np.cos(theta) |
| 104 | + start[2] = np.arctan2(dy, dx) |
| 105 | + dx = goal[0] - start[0] |
| 106 | + dy = goal[1] - start[1] |
| 107 | + goal[2] = torch.atan2(dy, dx) |
| 108 | + goal[3] = 0.0 if stop else 10.0 |
| 109 | + now = time.perf_counter() |
| 110 | + with torch.cuda.stream(stream): |
| 111 | + success = planner.search( |
| 112 | + start, goal, bitmap, expansion_limit, hysteresis, True |
| 113 | + ) # ignore goal validity |
| 114 | + ( |
| 115 | + avg_successor_time, |
| 116 | + avg_goal_check_time, |
| 117 | + avg_overhead_time, |
| 118 | + avg_g_update_time, |
| 119 | + switches, |
| 120 | + max_level_profile, |
| 121 | + Q_v_size, |
| 122 | + expansion_counter, |
| 123 | + expansion_list, |
| 124 | + ) = planner.get_profiler_info() |
| 125 | + |
| 126 | + end = time.perf_counter() |
| 127 | + avg_dt = 0.9 * avg_dt + 0.1 * (end - now) |
| 128 | + print( |
| 129 | + f"[IGHAStarMP] Avg_dt: {avg_dt:.3f}s, Expansions: {expansion_counter}, Success: {success}" |
| 130 | + ) |
| 131 | + if success: |
| 132 | + path = planner.get_best_path().numpy() |
| 133 | + path = np.flip(path, axis=0) |
| 134 | + path[..., :2] -= offset |
| 135 | + path[..., :2] += map_center |
| 136 | + result_queue.put((True, True, path, expansion_counter)) |
| 137 | + else: |
| 138 | + result_queue.put((False, True, None, expansion_counter)) |
| 139 | + except Exception as e: |
| 140 | + print(f"[IGHAStarMP] Planner error: {e}") |
| 141 | + traceback.print_exc() |
| 142 | + result_queue.put((False, True, None, 3)) |
| 143 | + |
| 144 | + def set_query( |
| 145 | + self, |
| 146 | + map_center, |
| 147 | + start_state, |
| 148 | + goal_, |
| 149 | + costmap, |
| 150 | + heightmap, |
| 151 | + hysteresis, |
| 152 | + expansion_limit, |
| 153 | + stop=False, |
| 154 | + disable=False, |
| 155 | + ): |
| 156 | + """ |
| 157 | + Submit a new planning query. Returns immediately; results are available via update(). |
| 158 | + """ |
| 159 | + if disable: |
| 160 | + return |
| 161 | + if np.linalg.norm(start_state[:2] - goal_[:2]) < 5.0: |
| 162 | + print("[IGHAStarMP] Start and goal are too close, skipping query.") |
| 163 | + return |
| 164 | + if not self.completed: |
| 165 | + return |
| 166 | + self.query_queue.put( |
| 167 | + ( |
| 168 | + map_center, |
| 169 | + start_state, |
| 170 | + goal_, |
| 171 | + costmap, |
| 172 | + heightmap, |
| 173 | + hysteresis, |
| 174 | + expansion_limit, |
| 175 | + stop, |
| 176 | + ) |
| 177 | + ) |
| 178 | + self.completed = False |
| 179 | + self.success = False |
| 180 | + |
| 181 | + def reset(self): |
| 182 | + """Reset planner state.""" |
| 183 | + self.path = None |
| 184 | + self.success = False |
| 185 | + self.completed = True |
| 186 | + self.expansion_counter = 0 |
| 187 | + |
| 188 | + def update(self): |
| 189 | + """ |
| 190 | + Call this periodically in your main loop to check for planner results. |
| 191 | + Returns (success, path, expansion_counter). |
| 192 | + """ |
| 193 | + try: |
| 194 | + ( |
| 195 | + self.success, |
| 196 | + self.completed, |
| 197 | + path, |
| 198 | + expansions, |
| 199 | + ) = self.result_queue.get_nowait() |
| 200 | + self.expansion_counter = expansions |
| 201 | + if self.success: |
| 202 | + self.path = path |
| 203 | + return self.success, self.path, self.expansion_counter |
| 204 | + else: |
| 205 | + return False, self.path, self.expansion_counter |
| 206 | + except Empty: |
| 207 | + return False, self.path, self.expansion_counter |
| 208 | + |
| 209 | + def shutdown(self): |
| 210 | + """Shut down the planner process.""" |
| 211 | + self.query_queue.put(None) |
| 212 | + self.process.terminate() |
| 213 | + self.process.join() |
0 commit comments