Cleaned up Kinodynamic RRT* implementation

Author: anandk1999

GEMstack/onboard/planning/collision.py

@@ -1,4 +1,3 @@
-import numpy as np
 from scipy.ndimage import distance_transform_edt
 
 def build_collision_lookup(grid, safety_margin=2, vehicle_width=1.0):

GEMstack/onboard/planning/kinodynamic_rrt_star.py

@@ -1,22 +1,16 @@
 import numpy as np
 import cv2
-import yaml
 
-def load_pgm_to_occupancy_grid(pgm_path, yaml_path):
+def load_pgm_to_occupancy_grid(pgm_path):
     """
     Load a PGM file and convert it to an occupancy grid.
     
     Args:
         pgm_path: Path to the PGM file
-        yaml_path: Path to the YAML metadata file
         
     Returns:
-        Tuple of (occupancy_grid, metadata)
+        2D Array occupancy_grid
     """
-    # # Load YAML metadata
-    with open(yaml_path, 'r') as f:
-        metadata = yaml.safe_load(f)
-    
     # Load PGM file
     image = cv2.imread(pgm_path, cv2.IMREAD_GRAYSCALE)
     if image is None:
@@ -25,13 +19,8 @@ def load_pgm_to_occupancy_grid(pgm_path, yaml_path):
     # Use a threshold to create binary occupancy grid
     thresh = 128
     occupancy_grid = (image >= thresh).astype(np.uint8)
-    
-    # In ROS maps, occupied pixels are typically black (0) and free pixels are white (255)
-    # If negate is True, we need to invert the grid
-    # if metadata.get('negate', 0):
-    #     occupancy_grid = 1 - occupancy_grid
 
-    return occupancy_grid, metadata
+    return occupancy_grid
 
 def downsample_occupancy_grid(grid, factor):
     """
@@ -47,44 +36,4 @@ def downsample_occupancy_grid(grid, factor):
     h, w = grid.shape
     nh, nw = h // factor, w // factor
     g = grid[:nh*factor, :nw*factor]
-    return g.reshape(nh, factor, nw, factor).max(axis=(1,3))
-
-def world_to_grid(x, y, theta, metadata):
-    """
-    Convert world coordinates to grid coordinates.
-    
-    Args:
-        x, y, theta: World coordinates
-        metadata: Map metadata
-        
-    Returns:
-        Tuple of (grid_x, grid_y, grid_theta)
-    """
-    origin_x, origin_y, _ = metadata['origin']
-    resolution = metadata['resolution']
-    
-    grid_x = int((x - origin_x) / resolution)
-    grid_y = int((y - origin_y) / resolution)
-    grid_theta = theta  # Heading remains the same
-    
-    return grid_x, grid_y, grid_theta
-
-def grid_to_world(grid_x, grid_y, grid_theta, metadata):
-    """
-    Convert grid coordinates to world coordinates.
-    
-    Args:
-        grid_x, grid_y, grid_theta: Grid coordinates
-        metadata: Map metadata
-        
-    Returns:
-        Tuple of (x, y, theta) in world coordinates
-    """
-    origin_x, origin_y, _ = metadata['origin']
-    resolution = metadata['resolution']
-    
-    x = origin_x + grid_x * resolution
-    y = origin_y + grid_y * resolution
-    theta = grid_theta  # Heading remains the same
-    
-    return x, y, theta
+    return g.reshape(nh, factor, nw, factor).max(axis=(1,3))

GEMstack/onboard/planning/map_utils.py

@@ -1,41 +1,20 @@
 import os
 import argparse
 import time
-import math
-import matplotlib.pyplot as plt
-import numpy as np
 import matplotlib
-# from collision import build_collision_lookup
-# from kinodynamic_rrt_star import OptimizedKinodynamicRRT
-# from map_utils import load_pgm_to_occupancy_grid
-# from visualization import visualize_path, animate_path  
+
 from .collision import build_collision_lookup
-from .map_utils import world_to_grid, grid_to_world
+from .map_utils import load_pgm_to_occupancy_grid
 from .visualization import visualize_path, animate_path
 from .kinodynamic_rrt_star import OptimizedKinodynamicRRT
-def optimized_kinodynamic_rrt_planning(start_world, goal_world, occupancy_grid, metadata = None,
-                                   safety_margin=10, vehicle_width=5, step_size=1.0,
-                                   turning_radius=3.0, max_iterations=100000, bidirectional=True):
+
+def optimized_kinodynamic_rrt_planning(start_world, goal_world, occupancy_grid, safety_margin=10, 
+                                       vehicle_width=20, vehicle_length=45.0, step_size=1.0, max_iterations=100000):
     """
-        parser.add_argument("--test", action="store_true", help="run unit tests and exit")
-    parser.add_argument("--vis", default=True, action="store_true", help="show visualizations for tests or planning result")
-    parser.add_argument("--animate", "-a", action="store_true", help="animate planned path")
-    parser.add_argument("--max-iter", type=int, default=20000, help="maximum RRT iterations")
-    parser.add_argument("--pad", type=int, default=100, help="crop padding (cells)")
-    parser.add_argument("--safety", type=int, default=2, help="safety margin (cells)")
-    parser.add_argument("--map-pgm", type=str, default="occupancy_grid_after_>0.pgm", help="path to PGM map file")
-    parser.add_argument("--map-yaml", type=str, default="rrt_occupancy_map.yaml", help="path to YAML metadata file")
-    parser.add_argument("--step-size", type=float, default=1.0, help="step size for path sampling")
-    parser.add_argument("--turning-radius", type=float, default=1.0, help="minimum turning radius")
-    parser.add_argument("--vehicle-width", type=float, default=1.5, help="vehicle width in meters")
-    parser.add_argument("--bidirectional", type=bool, default=False, help="Use bidirectional RRT*")
-    Optimized Kinodynamic RRT planning from start to goal
-    
     Args:
         start_world: (x, y, theta) start position in world coordinates
         goal_world: (x, y, theta) goal position in world coordinates
         occupancy_grid: Binary occupancy grid (1=obstacle, 0=free)
-        metadata: Map metadata
         safety_margin: Safety margin in grid cells
         vehicle_width: Vehicle width in meters
         step_size: Step size for path sampling
@@ -77,18 +56,21 @@ def optimized_kinodynamic_rrt_planning(start_world, goal_world, occupancy_grid,
     # Initialize planner with optimized parameters
     planner = OptimizedKinodynamicRRT(
         occupancy_grid=occupancy_grid,
-        collision_lookup=collision_lookup,
-        start_pose=start_grid,
-        goal_pose=goal_grid,
-        max_iter=max_iterations,
+        collision_lookup_table=collision_lookup,
+        start_pose_tuple=start_grid,
+        goal_pose_tuple=goal_grid,
+        max_iterations=max_iterations,
+        local_sampling_step_size=step_size,
+        vehicle_width=vehicle_width,
+        vehicle_length=vehicle_length
         # bidirectional=bidirectional
     )
 
     ## Testing Summoning RRT implementation
     # planner = BiRRT(start_grid, goal_grid, 1-occupancy_grid, [0,4384,0,4667])
     # grid_path = planner.search()
     t_rrt = time.time()
-    grid_path = planner.plan(visualize_final=True)
+    grid_path = planner.plan(visualize_planning_output=True)
     # anim = planner.animate_sampling()   
     t_rrt_final = time.time() - t_rrt
     print(f"RRT planning completed in {t_rrt_final:.2f}s")
@@ -103,51 +85,31 @@ def optimized_kinodynamic_rrt_planning(start_world, goal_world, occupancy_grid,
 
     return grid_path
 
+# Used for local testing
 def main():
     """Main function for running the planner."""
     parser = argparse.ArgumentParser(description="Optimized Kinodynamic RRT planner")
-    parser.add_argument("--test", action="store_true", help="run unit tests and exit")
     parser.add_argument("--vis", default=True, action="store_true", help="show visualizations for tests or planning result")
     parser.add_argument("--animate", "-a", action="store_true", help="animate planned path")
-    parser.add_argument("--max-iter", type=int, default=20000, help="maximum RRT iterations")
+    parser.add_argument("--max-iter", type=int, default=10000, help="maximum RRT iterations")
     parser.add_argument("--pad", type=int, default=100, help="crop padding (cells)")
     parser.add_argument("--safety", type=int, default=2, help="safety margin (cells)")
     parser.add_argument("--map-pgm", type=str, default="occupancy_grid_after_>0.pgm", help="path to PGM map file")
-    parser.add_argument("--map-yaml", type=str, default="rrt_occupancy_map.yaml", help="path to YAML metadata file")
-    parser.add_argument("--step-size", type=float, default=1.0, help="step size for path sampling")
-    parser.add_argument("--turning-radius", type=float, default=1.0, help="minimum turning radius")
-    parser.add_argument("--vehicle-width", type=float, default=1.5, help="vehicle width in meters")
-    parser.add_argument("--bidirectional", type=bool, default=False, help="Use bidirectional RRT*")
-
-    # parser = argparse.ArgumentParser(description="Optimized Kinodynamic RRT planner")
-    # parser.add_argument("--test", action="store_true", help="run unit tests and exit")
-    # parser.add_argument("--vis", action="store_true", help="show visualizations for tests or planning result")
-    # parser.add_argument("--animate", "-a", action="store_true", help="animate planned path")
-    # parser.add_argument("--max-iter", type=int, default=10000, help="maximum RRT iterations")
-    # parser.add_argument("--pad", type=int, default=100, help="crop padding (cells)")
-    # parser.add_argument("--safety", type=int, default=2, help="safety margin (cells)")
-    # parser.add_argument("--map-pgm", type=str, default="rrt_occupancy_map.pgm", help="path to PGM map file")
-    # parser.add_argument("--map-yaml", type=str, default="rrt_occupancy_map.yaml", help="path to YAML metadata file")
-    # parser.add_argument("--step-size", type=float, default=5.0, help="step size for path sampling")
-    # parser.add_argument("--turning-radius", type=float, default=100.0, help="minimum turning radius")
-    # parser.add_argument("--vehicle-width", type=float, default=12.0, help="vehicle width in meters")
-    # parser.add_argument("--bidirectional", type=bool, default=False, help="Use bidirectional RRT*")
+    parser.add_argument("--step-size", type=float, default=100.0, help="step size for path sampling")
+    parser.add_argument("--vehicle-width", type=float, default=20, help="vehicle width in meters")
+    parser.add_argument("--vehicle-length", type=float, default=45, help="vehicle length in meters")
 
     if "--vis" not in os.sys.argv and "--animate" not in os.sys.argv and "-a" not in os.sys.argv:
         matplotlib.use("Agg")
 
     args = parser.parse_args()
 
-    if args.test:
-        run_tests(show=args.vis)
-        return
-    
-    if not (os.path.exists(args.map_pgm) and os.path.exists(args.map_yaml)):
-        print(f"Map files not found: {args.map_pgm} and/or {args.map_yaml}")
-        print("Use --test for CI runs or provide map files with --map-pgm and --map-yaml")
+    if not (os.path.exists(args.map_pgm)):
+        print(f"Map files not found: {args.map_pgm}")
+        print("Use --test for CI runs or provide map files with --map-pgm")
         return
 
-    occupancy_grid, meta = load_pgm_to_occupancy_grid(args.map_pgm, args.map_yaml)
+    occupancy_grid = load_pgm_to_occupancy_grid(args.map_pgm)
 
     # start_w = (-200.0, -137.0, 0 * math.pi / 2)
     # goal_w = (-100.0, -137.0, 3*math.pi/2)
@@ -160,24 +122,22 @@ def main():
     # goal_w = (15.0, 11.0, 2*math.pi/2)
     # goal_w = (.0, 5.0, 1*math.pi/2)
 
-    visualize_path(occupancy_grid, [], meta, start_w, goal_w)
+    visualize_path(occupancy_grid, [], start_w, goal_w)
 
     path = optimized_kinodynamic_rrt_planning(
-        start_w, goal_w, occupancy_grid, meta,
+        start_w, goal_w, occupancy_grid,
         safety_margin=args.safety,
         vehicle_width=args.vehicle_width,
         step_size=args.step_size,
-        turning_radius=args.turning_radius,
         max_iterations=args.max_iter,
-        bidirectional=args.bidirectional
     )
 
     if path:
         print(f"Planned path with {len(path)} poses")
         if args.vis:
-            visualize_path(occupancy_grid, path, meta, start_w, goal_w)
+            visualize_path(occupancy_grid, path, start_w, goal_w)
         if args.animate:
-            animate_path(occupancy_grid, path, meta, pad_cells=args.pad)
+            animate_path(occupancy_grid, path, pad_cells=args.pad)
     else:
         print("Failed to find path")
 

GEMstack/onboard/planning/planner.py

@@ -1,24 +1,6 @@
 import math
-import numpy as np
 
-def normalize_angle(angle: float) -> float:
-    """Wrap angle to (‑π, π]."""
-    return (angle + math.pi) % (2 * math.pi) - math.pi
-
-def bresenham(x0, y0, x1, y1):
-    """Bresenham's line algorithm for grid traversal."""
-    x0, y0, x1, y1 = map(int, map(round, (x0, y0, x1, y1)))
-    dx, sx = abs(x1 - x0), (1 if x0 < x1 else -1)
-    dy, sy = -abs(y1 - y0), (1 if y0 < y1 else -1)
-    err = dx + dy
-    while True:
-        yield x0, y0
-        if x0 == x1 and y0 == y1:
-            break
-        e2 = 2 * err
-        if e2 >= dy:
-            err += dy
-            x0 += sx
-        if e2 <= dx:
-            err += dx
-            y0 += sy
+def normalize_angle(angle):
+    while angle > math.pi: angle -= 2.0 * math.pi
+    while angle < -math.pi: angle += 2.0 * math.pi
+    return angle

GEMstack/onboard/planning/utils.py

@@ -1,17 +1,15 @@
 import math
 import matplotlib.pyplot as plt
 import matplotlib.animation as animation
-# from map_utils import world_to_grid
-from .map_utils import world_to_grid
 import time
-def visualize_path(occupancy_grid, path, metadata, start_world, goal_world, show_headings=True):
+
+def visualize_path(occupancy_grid, path, start_world, goal_world, show_headings=True):
     """
     Visualize the planned path.
     
     Args:
         occupancy_grid: Binary occupancy grid (1=obstacle, 0=free)
         path: List of (x, y, theta) world coordinates
-        metadata: Map metadata
         start_world: (x, y, theta) start position in world coordinates
         goal_world: (x, y, theta) goal position in world coordinates
         show_headings: Whether to show heading arrows along the path
@@ -23,13 +21,9 @@ def visualize_path(occupancy_grid, path, metadata, start_world, goal_world, show
 
     # Extract path points
     if path:
-        grid_path = []
-        for world_pt in path:
-            grid_pt = world_to_grid(*world_pt, metadata)
-            grid_path.append(grid_pt)
 
-        xs = [p[0] for p in grid_path]
-        ys = [p[1] for p in grid_path]
+        xs = [p[0] for p in path]
+        ys = [p[1] for p in path]
 
         # Plot path
         plt.plot(xs, ys, 'g-', linewidth=2)
@@ -39,15 +33,15 @@ def visualize_path(occupancy_grid, path, metadata, start_world, goal_world, show
             arrow_interval = max(1, len(path) // 20)  # Show ~20 arrows along path
             arrow_length = 10
 
-            for i in range(0, len(grid_path), arrow_interval):
-                x, y, theta = grid_path[i]
+            for i in range(0, len(path), arrow_interval):
+                x, y, theta = path[i]
                 dx = arrow_length * math.cos(theta)
                 dy = arrow_length * math.sin(theta)
                 plt.arrow(x, y, dx, dy, head_width=3, head_length=6, fc='blue', ec='blue')
 
     # Show start and goal
-    start_grid = world_to_grid(*start_world, metadata)
-    goal_grid = world_to_grid(*goal_world, metadata)
+    start_grid = start_world
+    goal_grid = goal_world
 
     plt.scatter(start_grid[0], start_grid[1], color='green', s=100, marker='o', label='Start')
     plt.scatter(goal_grid[0], goal_grid[1], color='red', s=100, marker='x', label='Goal')
@@ -70,15 +64,14 @@ def visualize_path(occupancy_grid, path, metadata, start_world, goal_world, show
     plt.savefig(f"path_planning_result_{time.time()}.png")
     # plt.show()
 
-def animate_path(occupancy_grid, path, metadata, interval=60, pad_cells=20,
+def animate_path(occupancy_grid, path, interval=60, pad_cells=20,
                 save="ani.gif", vehicle_len=10):
     """
     Animate the drive and crop axes to the path region.
     
     Args:
         occupancy_grid: Binary occupancy grid (1=obstacle, 0=free)
         path: List of (x, y, theta) world coordinates
-        metadata: Map metadata
         interval: Animation interval in milliseconds
         pad_cells: Extra cells to pad around trajectory bbox
         save: If provided, save animation to this file
@@ -87,11 +80,8 @@ def animate_path(occupancy_grid, path, metadata, interval=60, pad_cells=20,
     Returns:
         Animation object
     """
-    def world_to_grid(x, y, th, meta):
-        ox, oy, _ = meta['origin']; res = meta['resolution']
-        return ((x-ox)/res, (y-oy)/res, th)
 
-    gpath = [world_to_grid(*pt, metadata) for pt in path]
+    gpath = [pt for pt in path]
     xs = [p[0] for p in gpath]; ys = [p[1] for p in gpath]
 
     xmin, xmax = min(xs)-pad_cells, max(xs)+pad_cells