Adding BeamNG example

.gitignore

@@ -12,7 +12,6 @@ ighastar/src/Environments/analytical_bicycle.cpp
 ighastar/scripts/test_case_generation.py
 examples/ros_kinodynamic_example.py
 examples/ROS/
-examples/BeamNG/
 # Python
 __pycache__/
 *.py[cod]

examples/BeamNG/Configs/example.yaml

@@ -0,0 +1,118 @@
+MPPI_config:
+  ROLLOUTS: 1024 # 8192 if using 80 timesteps
+  TIMESTEPS: &TIMESTEPS 30 # 80
+  BINS: 1
+  u_per_command: 1
+
+Map_config:
+  map_name: "small_island"
+  map_size: &map_size 100
+  map_res: &map_res 0.25
+  map_res_hitl: 0.25
+  elevation_range: 32.0
+  layers:
+    color: 3
+    elevation: 1
+    semantics: 3
+    costmap: 1
+
+Dynamics_config:
+  wheelbase: &wheelbase 2.6
+  throttle_to_wheelspeed: &throttle_to_wheelspeed 20.0
+  steering_max: &steering_max 0.6
+  dt: &delta_T 0.05
+  D: 0.8
+  B: 6.8
+  C: 1.75
+  lf: 1.3
+  lr: 1.3
+  Iz: 1.7
+  LPF_tau: 0.5
+  LPF_st: 0.5
+  LPF_th: 0.5
+  res_coeff: 0.00
+  drag_coeff: 0.00
+  car_length: *wheelbase
+  car_width: &car_width 1.5
+  cg_height: &cg_height 0.75
+  patch_size: &patch_size 3.0
+  type: "slip3d"
+
+Sampling_config:
+  control_dim: 2
+  noise_0: 1.0 # steering noise
+  noise_1: 1.0
+  scaled_dt: 0.1 ## this is effectively the actuator bandwidth, larger values mean the actuator can move faster.
+  temperature: 0.5
+  max_thr: 0.8
+  min_thr: 0.0
+
+Cost_config:
+  pos_w: 1.0 ## weight on the terminal goal cost
+  speed_w: 0.1 ## weight with which target speed will be tracked
+  heading_w: 0.1
+  scaling_factor: 1.0
+  critical_RI: &RI 0.8
+  critical_vert_acc: &max_vert_acc 4.0
+  roll_ditch_w: 50.0
+  lethal_w: 50.0
+  car_bb_length: 2.6
+  car_bb_width: 1.6
+
+RPS_config:
+  cg_height: *cg_height
+  wheelbase: *wheelbase
+  track_width: *car_width
+  max_speed: *throttle_to_wheelspeed
+  steering_max: *steering_max
+  accel_gain: 0.5
+  roll_rate_gain: 0.5
+  steer_slack: 1.0
+
+Planner_config:
+  unstuck_expansions: 100000 # number of expansions to get the first plan
+  experiment_info_default:
+    resolution: 4.0 # starting resolution used for discretization.
+    epsilon: [5, 5, 3.14, 5.0] # ate, cte, heading, vel for goal region.
+    tolerance: 0.5 # Your perception (map) resolution should be at least this.
+    max_level: 5 # maximum level to which you expect the system to go; we use this to cache the hash values.
+    division_factor: 2  # factor by which the resolution increases every level.
+    max_expansions: 5000 # maximum number of expansions allowed.
+    hysteresis: 1000 # hysteresis threshold for for IGHA*-H
+    node_info:
+      node_type: "kinodynamic"
+      length: 2.6
+      width: 1.6 # actual car width is 1.5
+      map_res: *map_res
+      dt: *delta_T
+      timesteps: 10
+      step_size: 1.0
+      steering_list: [-15.0, -5.0,  0.0, 5.0, 15.0] # in degrees.
+      throttle_list: [-5.0, 0, 5.0]
+      del_theta: 90.0 # in degrees
+      del_vel: 2.5
+      RI: *RI
+      max_vert_acc: *max_vert_acc
+      min_vel: 0.0
+      max_vel: 15.0
+      max_theta: 25.0 # in degrees
+      gear_switch_time: 1.0
+      hash_theta: False
+
+
+hysteresis: [100, -1] # -1 means HA*M
+scenarios: ['0'] # , '1']
+time_limit: [120.0]
+lookahead:  [50.0]
+wp_radius: [10.0]
+
+start_pos: [-67, 336, 34.5]
+start_quat: [0, 0, 0.3826834, 0.9238795]
+
+run_lockstep: True
+burn_time: 0.04
+smooth_map: False
+
+vehicle:
+  make: "sunburst"
+  model: "offroad"

examples/BeamNG/Configs/offroad.yaml

@@ -0,0 +1,54 @@
+# camera and lidar are very heavy from the rendering perspective especially if you have
+# a low end gaming laptop (such as the author, 4 GB 1050Ti GPU with an 8th gen i7 CPU)
+camera:
+  enable: False
+  width: 640  # Default width
+  height: 480  # Default height
+  fps: 30  # Default frames per second
+  fov: 87.0 # default fov of d400 series
+  pos: [1,0,0.7] ## use (0.15, 0.047, 0.02) for D455, (0.15, 0.025, 0.02) for d435
+  dir: [0, -1, 0]
+  up: [0, 0, 1]
+  rot: [0, 0, 0, 1] ## technically, the up and forward dir should be derivable from this, however, I need to talk to the BnG devs to figure out the conversion from REP103 to BnG
+  color_optical_frame: "camera_color_optical_frame"
+  depth_optical_frame: "camera_depth_optical_frame"
+  depth_frame: "camera_depth_frame"
+  camera_color_topic: "/camera/color/image_raw"
+  camera_depth_topic: "/camera/depth/image_rect_raw"
+  camera_color_info_topic: "/camera/color/camera_info"
+  camera_depth_info_topic: "/camera/depth/camera_info"
+  monitor_topic: "/camera/depth/image_rect_raw" ## this is the topic HAL uses for monitoring purposes
+  annotation: False
+lidar:
+  enable: False
+  rays_per_second_per_scan: 5000  # Default points
+  channels: 3  # Default height. in execution we cut the first and last channel if there are only 3 channels to simulate a 2D scanner
+  fps: 10  # Default frames per second
+  vertical_angle: 26.9 # this is only for simulation purposes
+  pos: [0.04, 0, 1.0] # default position of lidar wrt imu
+  rot: [0, 0, 0, 1]
+  dir: [0, -1, 0]
+  up: [0, 0, 1]
+  frame: "laser_frame"
+  max_distance: 10.0
+  scan_topic: "/scan"
+  monitor_topic: "/scan"
+  pc_topic: "converted_pc"
+vesc:
+  fps: 50.0
+  erpm_gain: 3166
+  topic: "sensors/core"
+  steering_degrees: 260
+mavros:
+  pos: [0,0,0.8] # default position on the hound
+  fps: 50
+  monitor_topic: "/mavros/imu/data_raw"
+  pose_topic: "/mavros/local_position/pose"
+  odom_topic: "/mavros/local_position/odom"
+  state_topic: "/mavros/state"
+  gps_topic: "/mavros/gpsstatus/gps1/raw"
+  notification_topic: "/mavros/play_tune"
+  channel_topic: "mavros/rc/in"
+  raw_input_topic: '/mavros/manual_control/send'
+  frame: "base_link"
+  failure_action: "rosrun mavros mavsys rate --all 50"

examples/BeamNG/IGHAStarMP.py

@@ -0,0 +1,187 @@
+import torch.multiprocessing as mp
+import numpy as np
+import time
+import traceback
+from queue import Empty
+from typing import Any, Optional, Tuple, Dict
+
+
+class IGHAStarMP:
+    """
+    Multiprocessing wrapper for the IGHA* planner. Runs the planner in a separate process and communicates via queues.
+    """
+
+    def __init__(self, configs: Dict[str, Any]) -> None:
+        mp.set_start_method("spawn", force=True)  # Safe for CUDA
+        self.query_queue = mp.Queue(5)
+        self.result_queue = mp.Queue(5)
+        self.process = mp.Process(
+            target=self._planner_process,
+            args=(configs, self.query_queue, self.result_queue),
+        )
+        self.process.start()
+        self.path = None
+        self.success = False
+        self.completed = True
+        self.expansion_counter = 0
+
+    def _planner_process(
+        self, configs: Dict[str, Any], query_queue: Any, result_queue: Any
+    ) -> None:
+        """
+        Planner process: loads the CUDA/C++ kernel and runs the IGHA* planner in response to queries.
+        """
+        import torch
+        import numpy as np
+        from ighastar.scripts.common_utils import create_planner
+
+        planner = create_planner(configs["Planner_config"])
+        print("[IGHAStarMP] Planner loaded.")
+
+        map_res = configs["experiment_info_default"]["node_info"]["map_res"]
+        offset = None
+        avg_dt = 0.1
+        stream = torch.cuda.Stream()
+        while True:
+            task = query_queue.get()
+            if task is None:
+                time.sleep(0.1)
+                continue
+            (
+                map_center,
+                start_state,
+                goal_,
+                costmap,
+                heightmap,
+                hysteresis,
+                expansion_limit,
+                stop,
+            ) = task
+            try:
+                map_size = costmap.shape
+                bitmap = torch.ones((map_size[0], map_size[1], 2), dtype=torch.float32)
+                bitmap[..., 0] = torch.from_numpy(costmap)
+                bitmap[..., 1] = torch.from_numpy(heightmap)
+                if offset is None:
+                    offset = map_res * np.array(bitmap.shape[:2]) * 0.5
+                start = torch.zeros(4, dtype=torch.float32)
+                goal = torch.zeros(4, dtype=torch.float32)
+                start[:2] = torch.from_numpy(start_state[:2] + offset - map_center)
+                goal[:2] = torch.from_numpy(goal_[:2] + offset - map_center)
+                start[2] = start_state[5]
+                start[3] = float(np.linalg.norm(start_state[6:8]))
+                if start[3] > 1.0 and start_state[6] > 0.5:
+                    V = start_state[6:8]
+                    theta = start_state[5]
+                    dx = V[0] * np.cos(theta) - V[1] * np.sin(theta)
+                    dy = V[0] * np.sin(theta) + V[1] * np.cos(theta)
+                    start[2] = np.arctan2(dy, dx)
+                dx = goal[0] - start[0]
+                dy = goal[1] - start[1]
+                goal[2] = torch.atan2(dy, dx)
+                goal[3] = 0.0 if stop else 10.0
+                now = time.perf_counter()
+                with torch.cuda.stream(stream):
+                    success = planner.search(
+                        start, goal, bitmap, expansion_limit, hysteresis, True
+                    )  # ignore goal validity
+                    (
+                        avg_successor_time,
+                        avg_goal_check_time,
+                        avg_overhead_time,
+                        avg_g_update_time,
+                        switches,
+                        max_level_profile,
+                        Q_v_size,
+                        expansion_counter,
+                        expansion_list,
+                    ) = planner.get_profiler_info()
+
+                end = time.perf_counter()
+                avg_dt = 0.9 * avg_dt + 0.1 * (end - now)
+                print(
+                    f"[IGHAStarMP] Avg_dt: {avg_dt:.3f}s, Expansions: {expansion_counter}, Success: {success}"
+                )
+                if success:
+                    path = planner.get_best_path().numpy()
+                    path = np.flip(path, axis=0)
+                    path[..., :2] -= offset
+                    path[..., :2] += map_center
+                    result_queue.put((True, True, path, expansion_counter))
+                else:
+                    result_queue.put((False, True, None, expansion_counter))
+            except Exception as e:
+                print(f"[IGHAStarMP] Planner error: {e}")
+                traceback.print_exc()
+                result_queue.put((False, True, None, 3))
+
+    def set_query(
+        self,
+        map_center: np.ndarray,
+        start_state: np.ndarray,
+        goal_: np.ndarray,
+        costmap: np.ndarray,
+        heightmap: np.ndarray,
+        hysteresis: float,
+        expansion_limit: int,
+        stop: bool = False,
+        disable: bool = False,
+    ) -> None:
+        """
+        Submit a new planning query. Returns immediately; results are available via update().
+        """
+        if disable:
+            return
+        if np.linalg.norm(start_state[:2] - goal_[:2]) < 5.0:
+            print("[IGHAStarMP] Start and goal are too close, skipping query.")
+            return
+        if not self.completed:
+            return
+        self.query_queue.put(
+            (
+                map_center,
+                start_state,
+                goal_,
+                costmap,
+                heightmap,
+                hysteresis,
+                expansion_limit,
+                stop,
+            )
+        )
+        self.completed = False
+        self.success = False
+
+    def reset(self) -> None:
+        """Reset planner state."""
+        self.path = None
+        self.success = False
+        self.completed = True
+        self.expansion_counter = 0
+
+    def update(self) -> Tuple[bool, Optional[np.ndarray], int]:
+        """
+        Call this periodically in your main loop to check for planner results.
+        Returns (success, path, expansion_counter).
+        """
+        try:
+            (
+                self.success,
+                self.completed,
+                path,
+                expansions,
+            ) = self.result_queue.get_nowait()
+            self.expansion_counter = expansions
+            if self.success:
+                self.path = path
+                return self.success, self.path, self.expansion_counter
+            else:
+                return False, self.path, self.expansion_counter
+        except Empty:
+            return False, self.path, self.expansion_counter
+
+    def shutdown(self) -> None:
+        """Shut down the planner process."""
+        self.query_queue.put(None)
+        self.process.terminate()
+        self.process.join()