Unit 13: Robotics & Autonomous Systems

# ==============================================================================
# DevAscent Academy - Unit 13: Robotics & Autonomous Systems
# Standalone Robot Path Planner with A* Search & Dynamic Obstacle Replanning
# Pure Python Implementation - Zero External Dependencies
# ==============================================================================

import math
import heapq

class GridMap:
    """Represents a 2D spatial occupancy grid with obstacle inflation."""
    def __init__(self, width, height):
        self.width = width
        self.height = height
        # 0 = Free Space, 1 = Static Obstacle, 2 = Dynamic Obstacle
        self.grid = [[0 for _ in range(width)] for _ in range(height)]

    def add_obstacle(self, x, y, obstacle_type=1):
        if 0 <= x < self.width and 0 <= y < self.height:
            self.grid[y][x] = obstacle_type

    def is_blocked(self, x, y):
        if not (0 <= x < self.width and 0 <= y < self.height):
            return True # Outside map boundaries
        return self.grid[y][x] != 0

    def inflate_obstacles(self, radius=1):
        """Minkowski sum inflation: expands obstacles by robot safety radius."""
        inflated = [row[:] for row in self.grid]
        for y in range(self.height):
            for x in range(self.width):
                if self.grid[y][x] == 1:
                    for dy in range(-radius, radius + 1):
                        for dx in range(-radius, radius + 1):
                            nx, ny = x + dx, y + dy
                            if 0 <= nx < self.width and 0 <= ny < self.height:
                                inflated[ny][nx] = 1
        self.grid = inflated


class AStarPlanner:
    """Computes optimal geometric path using A* search with Euclidean heuristic."""
    def __init__(self, grid_map):
        self.map = grid_map

    def euclidean_heuristic(self, p1, p2):
        """Admissible Euclidean straight-line distance heuristic."""
        return math.sqrt((p1[0] - p2[0])**2 + (p1[1] - p2[1])**2)

    def plan(self, start, goal):
        """Finds shortest collision-free coordinate path from start to goal."""
        if self.map.is_blocked(start[0], start[1]) or self.map.is_blocked(goal[0], goal[1]):
            print("[Planner Error] Start or Goal is located on an obstacle.")
            return None

        # Priority Queue entries: (estimated_total_cost, current_node)
        open_set = []
        heapq.heappush(open_set, (0.0, start))

        came_from = {}
        # Cost from start to current node along best path
        cost_to_reach = {start: 0.0}

        # 8-connected directional movements (orthogonal cost 1.0, diagonal cost 1.414)
        neighbors = [
            (1, 0, 1.0), (-1, 0, 1.0), (0, 1, 1.0), (0, -1, 1.0),
            (1, 1, 1.414), (-1, 1, 1.414), (1, -1, 1.414), (-1, -1, 1.414)
        ]

        while open_set:
            current_f, current = heapq.heappop(open_set)

            if current == goal:
                # Reconstruct path by traversing came_from backward
                path = []
                curr = goal
                while curr in came_from:
                    path.append(curr)
                    curr = came_from[curr]
                path.append(start)
                path.reverse()
                return path

            for dx, dy, step_cost in neighbors:
                neighbor = (current[0] + dx, current[1] + dy)

                if self.map.is_blocked(neighbor[0], neighbor[1]):
                    continue

                tentative_cost = cost_to_reach[current] + step_cost

                if neighbor not in cost_to_reach or tentative_cost < cost_to_reach[neighbor]:
                    came_from[neighbor] = current
                    cost_to_reach[neighbor] = tentative_cost
                    estimated_total = tentative_cost + self.euclidean_heuristic(neighbor, goal)
                    heapq.heappush(open_set, (estimated_total, neighbor))

        return None # No viable path found


class AutonomousMobileRobot:
    """Simulates an autonomous agent with sensory perception and replanning."""
    def __init__(self, start_pos, goal_pos, grid_map):
        self.current_pos = start_pos
        self.goal_pos = goal_pos
        self.map = grid_map
        self.planner = AStarPlanner(grid_map)
        self.trajectory_history = [start_pos]

    def execute_mission(self):
        print(f"\n================ STARTING ROBOTIC MISSION ================")
        print(f"Initial Location: {self.current_pos} -> Target Goal: {self.goal_pos}")

        path = self.planner.plan(self.current_pos, self.goal_pos)
        if not path:
            print("[Mission Aborted] No viable initial path exists.")
            return

        print(f"[Global Planner] Generated initial A* trajectory ({len(path)} waypoints).")

        step_idx = 0
        while self.current_pos != self.goal_pos:
            step_idx += 1
            next_waypoint = path[1]

            # SIMULATION EVENT: Sudden dynamic obstacle detected on planned path
            if step_idx == 4:
                print("\n[Perception Alert] 3D LiDAR detected dynamic moving obstacle at", next_waypoint)
                self.map.add_obstacle(next_waypoint[0], next_waypoint[1], obstacle_type=2)

                print("[Local Motion Controller] Waypoint blocked! Triggering dynamic A* replan...")
                path = self.planner.plan(self.current_pos, self.goal_pos)
                if not path:
                    print("[Mission Failed] Robot trapped by dynamic obstacles.")
                    return
                next_waypoint = path[1]
                print(f"[Replanner] New alternative trajectory established ({len(path)} waypoints).")

            # Advance robot to next waypoint
            self.current_pos = next_waypoint
            self.trajectory_history.append(self.current_pos)
            path.pop(0) # Advance path buffer
            print(f"Step {step_idx}: Robot navigated to pose {self.current_pos}")

        print("\n================ MISSION ACCOMPLISHED ================")
        print(f"Robot successfully reached goal {self.goal_pos} in {step_idx} steps.")
        print(f"Total Trajectory Length: {len(self.trajectory_history)} coordinates.")


# ==============================================================================
# DEMONSTRATION WORKFLOW
# ==============================================================================
if __name__ == "__main__":
    # Create 12x12 meter warehouse occupancy map
    warehouse_map = GridMap(width=12, height=12)

    # Insert static warehouse storage racks (vertical walls)
    for y in range(2, 9):
        warehouse_map.add_obstacle(x=5, y=y, obstacle_type=1)

    # Apply safety margin inflation
    warehouse_map.inflate_obstacles(radius=0)

    # Spawn AMR at loading bay (1, 1) with delivery goal at dock (10, 8)
    robot = AutonomousMobileRobot(start_pos=(1, 1), goal_pos=(10, 8), grid_map=warehouse_map)
    robot.execute_mission()