bug_algorithms.py

#Bug 1 and Bug 3 algorithms as State Machines
#The roBug tries to reach a clicked target. When it hits an obstacle,
#it follows the obstacle boundary until it can leave.

import math
from config_parameters import *
from geometry import distance, closest_point_on_segment

#State Definition:
#STATE_IDLE (0)            - No target set, doing nothing
#STATE_GOTO_GOAL (1)       - Moving directly toward the target
#STATE_FOLLOW_BOUNDARY (2)  - Following an obstacle wall (obstacle on left side)
#STATE_GOTO_LEAVE_POINT (3)      - Bug1 only: following wall back to the best leave point
#STATE_GOAL_REACHED (4)         - Got to the target
#STATE_GOAL_UNREACHABLE (5)     - Gave up

STATE_IDLE=0
STATE_GOTO_GOAL=1
STATE_FOLLOW_BOUNDARY=2
STATE_GOTO_LEAVE_POINT=3
STATE_GOAL_REACHED=4
STATE_GOAL_UNREACHABLE=5

#For Frontend to display
STATE_NAMES={
    0:"Idle",
    1:"Heading to Goal",
    2:"Following Boundary",
    3:"Going to Leave Point",
    4:"Goal Reached",
    5:"Unreachable"
}


class BugAlgorithms:

    def __init__(self, algorithm=1):
        self.algorithm=algorithm
        self.state=STATE_IDLE
        self.target_x=None
        self.target_y=None
        self._reset()

    def set_target(self, tx, ty):
        self.target_x=tx
        self.target_y=ty
        self.state=STATE_GOTO_GOAL
        self._reset()

    def _reset(self):
        self.hit_point=None
        self.hit_dist_to_goal=None
        self.best_leave=None
        self.best_leave_dist=None
        self.travel=0
        self.prev_pos=None
        self.min_travel_done=False
        self.prev_wall_angle=None  #angle where the wall was last seen
        self.max_turn=math.radians(MAX_TURN_DEG)

    def get_state_name(self):
        return STATE_NAMES.get(self.state,"?")

    #Singular move/tick, called once per frame
    def move(self, robot, scan, map):
        #Do nothing if idle, reached goal or can not reach goal
        if self.state in (STATE_IDLE, STATE_GOAL_REACHED, STATE_GOAL_UNREACHABLE):
            return

        #Some margin of error is acceptable (GOAL_REACHED_DIST),
        #if the goal has been reached finished state is entered, and method ends
        if robot.distance_to(self.target_x, self.target_y) < GOAL_REACHED_DIST:
            self.state=STATE_GOAL_REACHED
            return

        #Delegations to methods for 3 remaining non-error states

        #Going towards goal
        if self.state==STATE_GOTO_GOAL:
            self._goto_goal(robot, scan, map)

        #Following boundary/wall
        elif self.state==STATE_FOLLOW_BOUNDARY:
            self._follow_boundary(robot, scan, map)

        #Bug 1 only, going to optimal leave point
        elif self.state==STATE_GOTO_LEAVE_POINT:
            self._goto_leave(robot, scan, map)


    #head straight towards the set goal. But detects obstacles on the way to switch state.
    def _goto_goal(self, robot, scan, map):
        dx=self.target_x - robot.x
        dy=self.target_y - robot.y
        dist_to_goal=math.sqrt(dx*dx + dy*dy)
        robot.heading=math.atan2(dy, dx)

        #Scans in a cone directly ahead to detect potential obstacles, further objects
        #are more likely to be in the cone. But limits in detecting range limit
        #coned area.
        front=self._min_in_cone(scan, 0, 25)

        if front < OBSTACLE_DETECT_DIST:
            self.hit_point=(robot.x, robot.y)
            self.hit_dist_to_goal=dist_to_goal
            self.best_leave=(robot.x, robot.y)
            self.best_leave_dist=dist_to_goal
            self.travel=0
            self.prev_pos=(robot.x, robot.y)
            self.min_travel_done=False
            self.prev_wall_angle=None

            #Obstacle has been detected
            self.state=STATE_FOLLOW_BOUNDARY

            #Turn right so obstacle boundary is on the left
            #roBug follows boundaries on its left side, and immediately turns by 90 degrees
            #to achieve a (as much as possible) parallel to the boundary movement path
            robot.heading -= math.pi/2
        else:
            robot.move_toward(self.target_x, self.target_y, STEP_SIZE)


    def _follow_boundary(self, robot, scan, map):
        #Track distance traveled
        if self.prev_pos is not None:
            self.travel += distance(self.prev_pos, (robot.x, robot.y))
        self.prev_pos=(robot.x, robot.y)

        #Find the boundary on left side
        #If there is a last known angle, look there first to avoid tracking other objects.
        left_hit=self._find_wall_left(scan)

        front=self._min_in_cone(scan, 0, 20)
        step=STEP_SIZE

        if left_hit is not None:
            self.prev_wall_angle=left_hit[0]
            hit_angle=left_hit[0]
            hit_dist=left_hit[1]

            #Wall normal: points FROM the wall TOWARD the robot (away from surface)
            wall_direction=robot.heading + hit_angle
            normal_x=-math.cos(wall_direction)
            normal_y=-math.sin(wall_direction)

            #Tangent: normal rotated 90 deg CCW = direction along the wall (wall stays on left)
            tangent_x=-normal_y
            tangent_y=normal_x

            #Correction to maintain WALL_FOLLOW_DIST from the wall
            #Sign is negative because normal points AWAY from wall:
            #too far (err>0) -> pushes toward wall, too close (err<0) -> pushes away
            err=hit_dist - WALL_FOLLOW_DIST
            move_x=tangent_x - 0.5 * err * normal_x
            move_y=tangent_y - 0.5 * err * normal_y

            #If something is close ahead, intend to steer away more aggressively
            #May still be limited later if the attemtped turn is too sharp for distance to obstacle
            if front < WALL_FOLLOW_DIST * 1.2:
                urgency=1.0 - (front / (WALL_FOLLOW_DIST * 1.2))
                move_x += urgency * normal_x * 2.0
                move_y += urgency * normal_y * 2.0

            desired_heading=math.atan2(move_y, move_x)
        else:
            #Lost the wall - spin left and crawl forward until we find it again
            self.prev_wall_angle=None
            desired_heading=robot.heading + self.max_turn * 3
            step=STEP_SIZE * 0.15

        #Smooth out heading changes within 0 to 360 degrees
        # so the roBug does not jitter (turning full circles hundreds of degrees)
        heading_diff=desired_heading - robot.heading
        while heading_diff > math.pi: heading_diff -= 2*math.pi
        while heading_diff < -math.pi: heading_diff += 2*math.pi

        #Normally cap turning speed here, but allow sharper turns when close to a wall or wall is lost
        turn_limit=self.max_turn
        if front < WALL_FOLLOW_DIST * 0.5 or left_hit is None:
            turn_limit=self.max_turn * 3
        heading_diff=max(-turn_limit, min(turn_limit, heading_diff))
        new_heading=robot.heading + heading_diff

        #Coords to move in the new direction
        new_x=robot.x + math.cos(new_heading) * step
        new_y=robot.y + math.sin(new_heading) * step


        #Happy Path, safe to move
        if self._safe(new_x, new_y, map):
            robot.x=new_x
            robot.y=new_y
            robot.heading=new_heading
        else:
            #Blocked, turn right sharply and try a tiny step to unstick
            robot.heading -= self.max_turn * 2
            fallback_x=robot.x + math.cos(robot.heading) * STEP_SIZE * 0.1
            fallback_y=robot.y + math.sin(robot.heading) * STEP_SIZE * 0.1
            if self._safe(fallback_x, fallback_y, map):
                robot.x=fallback_x
                robot.y=fallback_y

        #Track which point along the boundary is closest to the goal (for Bug 1 leave point)
        dist_to_goal=robot.distance_to(self.target_x, self.target_y)
        if self.best_leave_dist is None or dist_to_goal < self.best_leave_dist:
            self.best_leave=(robot.x, robot.y)
            self.best_leave_dist=dist_to_goal

        #Dont check leave conditions until we have traveled a minimum distance
        #Otherwise the roBug might immediately leave after entering boundary follow
        if self.travel > MIN_BOUNDARY_TRAVEL:
            self.min_travel_done=True

        #Bug 1 and Bug 3 have different conditions for when to stop following
        if self.algorithm==1:
            self._check_bug1(robot)
        else:
            self._check_bug3(robot, scan, map)


    def _check_bug1(self, robot):
        #Bug 1: follow the ENTIRE boundary, then go to the best leave point
        if not self.min_travel_done: return
        if self.hit_point is None: return

        #Check if we are back near where we first hit the obstacle (full loop)
        if distance((robot.x,robot.y), self.hit_point) < THRESHOLD_RETURNED_INIT_HIT_POINT:
            if self.best_leave is not None:
                #If we happen to already be at the best leave point, skip straight to goal
                if distance((robot.x,robot.y), self.best_leave) < GOAL_REACHED_DIST * 2:
                    self.state=STATE_GOTO_GOAL
                    self.hit_point=None
                else:
                    #Otherwise follow the wall again until we reach the best leave point
                    self.state=STATE_GOTO_LEAVE_POINT
                    self.travel=0
                    self.min_travel_done=False
            else:
                self.state=STATE_GOAL_UNREACHABLE

        #Catch endless loops
        if self.travel > 600:
            self.state=STATE_GOAL_UNREACHABLE


    def _check_bug3(self, robot, scan, map):
        #Bug 3: leave as soon as the LIDAR shows the goal direction is clear
        if not self.min_travel_done: return

        dist_to_goal=robot.distance_to(self.target_x, self.target_y)

        #Catch endless loops
        if self.travel > 600:
            self.state=STATE_GOAL_UNREACHABLE
            return

        #Only allowed to leave if we are closer to the goal than when we first hit
        #Otherwise we havent rounded the obstacle enough and would just re-hit it
        if self.hit_dist_to_goal is not None and dist_to_goal >= self.hit_dist_to_goal:
            #But if we are back at the hit point after a lot of travel, give up
            if self.hit_point and distance((robot.x,robot.y),self.hit_point) < THRESHOLD_RETURNED_INIT_HIT_POINT:
                if self.travel > MIN_BOUNDARY_TRAVEL * 3:
                    self.state=STATE_GOAL_UNREACHABLE
            return

        #Compute the angle toward the goal relative to where the robot is currently facing
        dx=self.target_x - robot.x
        dy=self.target_y - robot.y
        goal_angle_world=math.atan2(dy, dx)
        goal_angle_relative=goal_angle_world - robot.heading
        while goal_angle_relative > math.pi: goal_angle_relative -= 2*math.pi
        while goal_angle_relative < -math.pi: goal_angle_relative += 2*math.pi

        #Can only check if the goal direction is actually within the LIDAR's field of view
        half_fov=math.radians(LIDAR_FOV / 2.0)
        if abs(goal_angle_relative) > half_fov - math.radians(5):
            return  #Goal is behind us, keep following

        #Check actual LIDAR readings in a narrow cone (+/- 4 deg) around the goal direction
        nearest_hit_toward_goal=self._min_in_cone(scan, math.degrees(goal_angle_relative), 4)

        #If there is nothing close in the goal direction, the current obstacle is no longer blocking
        #OBSTACLE_DETECT_DIST * 3 gives enough margin that we dont leave at a corner prematurely
        if nearest_hit_toward_goal > OBSTACLE_DETECT_DIST * 3:
            self.state=STATE_GOTO_GOAL
            self.hit_point=None


    def _goto_leave(self, robot, scan, map):
        #Bug 1 only: keep following the wall until we reach the best leave point
        if self.best_leave is None:
            self.state=STATE_GOTO_GOAL
            return
        if distance((robot.x,robot.y), self.best_leave) < GOAL_REACHED_DIST * 2:
            #Arrived at leave point, now head for the goal
            self.state=STATE_GOTO_GOAL
            self.hit_point=None
            return
        self._follow_boundary(robot, scan, map)


    def _find_wall_left(self, scan):
        #Searches LIDAR data for nearest hit on the left side (positive angles)
        #First checks near the last known wall angle to avoid jumping between obstacles
        #If nothing there, searches the full left range
        max_left_deg=LIDAR_FOV / 2.0 - 2
        max_detection_dist=WALL_FOLLOW_DIST * 6

        if self.prev_wall_angle is not None:
            prev_deg=math.degrees(self.prev_wall_angle)
            lo=max(10, prev_deg - 25)
            hi=min(max_left_deg, prev_deg + 25)
            result=self._nearest_in_range(scan, lo, hi, max_detection_dist)
            if result: return result

        return self._nearest_in_range(scan, 10, max_left_deg, max_detection_dist)


    def _nearest_in_range(self, scan, lo_deg, hi_deg, max_dist):
        #Finds the closest LIDAR hit within an angle range [lo_deg, hi_deg]
        closest_dist=LIDAR_MAX_RANGE
        closest_hit=None
        for angle, dist, hx, hy, hit in scan:
            if not hit: continue
            angle_deg=math.degrees(angle)
            if lo_deg < angle_deg < hi_deg and dist < closest_dist:
                closest_dist=dist
                closest_hit=(angle, dist, hx, hy)
        if closest_hit and closest_hit[1] < max_dist:
            return closest_hit
        return None


    def _min_in_cone(self, scan, center_deg, half_deg):
        #Returns the shortest LIDAR reading within a cone centered on center_deg
        #Used both for "is something ahead?" and "is the goal direction clear?"
        center_rad=math.radians(center_deg)
        half_rad=math.radians(half_deg)
        shortest=LIDAR_MAX_RANGE
        for angle, dist, hx, hy, hit in scan:
            if hit:
                diff=angle - center_rad
                while diff > math.pi: diff -= 2*math.pi
                while diff < -math.pi: diff += 2*math.pi
                if abs(diff) < half_rad and dist < shortest:
                    shortest=dist
        return shortest


    #Checks if a position is safe to move to (not inside or too close to any obstacle)
    def _safe(self, px, py, map, clearance=0.25):
        for seg in map.get_all_segments():
            d, cx, cy=closest_point_on_segment(px, py, seg[0], seg[1], seg[2], seg[3])
            if d < clearance: return False
        for c in map.circles:
            if math.sqrt((px - c[0])**2 + (py - c[1])**2) < c[2] + clearance:
                return False
        return True