# -*- coding: utf-8 -*- """ Created on Sat Oct 24 14:54:47 2020 @author: hbartos whogans1 """ #our program solves the maze using a right wall follow. #The robots goal is to spot a purple ball. Once the ball is found, #the robot will print a map representing the path it has taken through the maze, and celebrate import sys, os import numpy as np from ursim import RoboSimApp, ctrl import maze from matplotlib import colors import matplotlib.pyplot as plt DEG = np.pi/180 def solve_maze(m, x0, y0, dir0, x1, y1): maze_commands= 'wallcheck1' return maze_commands ###################################################################### class MazeController(ctrl.Controller): def __init__(self, commands): self.commands = commands ################################################## def initialize(self, time, odom_pose): self.command_index = 0 if self.commands: init_state = self.commands else: init_state = 'done' #self.movenum stores the current move that the robot is on, ex: 1st 2nd 3rd etc self.movenum=1 #self.map is a 2d array, where each square represents a square of the maze. If a square is zero, it has not been visited. #if a square stores a number, that number is the move number at which the robot most recently visited that square. self.map= np.full((20,20 ), 0) self.map[5][5]=1 self.reset_state(time, odom_pose, init_state, True,False) ################################################## def reset_state(self, time, odom_pose, state, reset_time,turncompleted): #Each wallcheck state includes a turn which may or may not be called. #to keep track of whether that robot has completed its turn, we use a variable called self.turny self.turny=turncompleted self.state = state self.start_pose = odom_pose if reset_time: self.start_time = time self.labeled_state = state ################################################## def points_at_angles(self, scan, desired_angles, cutoff=3*DEG): if scan is None or scan.angles is None or scan.ranges is None: return None # get indices of valid ranges ok_idx = np.nonzero(~np.isnan(scan.ranges))[0] if not len(ok_idx): return None valid_angles = scan.angles[ok_idx] valid_ranges = scan.ranges[ok_idx] points = [] for angle in desired_angles: angle_err = np.abs(angle - valid_angles) min_idx = angle_err.argmin() min_err = angle_err[min_idx] if min_err > cutoff: return None true_angle = valid_angles[min_idx] point = np.array([ np.cos(true_angle), np.sin(true_angle) ]) * valid_ranges[min_idx] points.append(point) return np.array(points) ################################################## def turn(self, turn_angle): command = ctrl.ControllerOutput(forward_vel=0, angular_vel=0.75*turn_angle+0.05*np.sign(turn_angle)) is_finished = abs(turn_angle) < 0.002 return command, is_finished ################################################## def drive(self, dist): command = ctrl.ControllerOutput(forward_vel=dist + 0.005*np.sign(dist), angular_vel=0) is_finished = abs(dist) < 0.01 return command, is_finished ################################################## def straighten(self, scan): points = self.points_at_angles(scan, [2*DEG, -2*DEG]) if points is None: return None, False else: delta = points[1] - points[0] turn_angle = np.arctan2(delta[0], -delta[1]) return self.turn(turn_angle) ################################################## def nudge(self, scan): points = self.points_at_angles(scan, [0]) if points is None: return None, False else: dist_to_wall = points[0][0] rounded_dist = np.floor(dist_to_wall) + 0.5 dist_to_drive = dist_to_wall - rounded_dist return self.drive(dist_to_drive) #calculates distance to wall def walle(self, scan): points = self.points_at_angles(scan, [0]) if points is None: return -1 else: dist_to_wall = points[0][0] return dist_to_wall #this method checks to see the balls location in the robot frame, if the ball is visible def ball(self,camera_data): dball = camera_data.detections['purple_ball'] if len(dball): blob = dball[0] return blob return -1 #this method displays the squares which the robot has travelled. # Visited squares appear in various shades of blue.After the first 14 visited squares, all visited squares are in thistle #display prints the array which shows the number of moves, as well as two graphs to show the path the robot took #one path draws a line to show the path of the robot, the other shades squares to show where the robot was def display(self): print('here is your map! Each number represents the most recent move number at which a square was visited') #matrix plot print(np.matrix(self.map)) #first plot is color coded from blue to black, with blue being the earliest move and black being the latest fig, ax = plt.subplots(1,1, tight_layout=True) county=0 gradient=[(1,1,1)] for i in range(self.movenum): county=county+(1/self.movenum) gradient.append((0.0,0.0,1-county)) cmap=colors.ListedColormap(gradient) count=1 bounds=[-1,.9] for i in range(self.movenum): count=count+1 bounds.append(.9+count) norm=colors.BoundaryNorm(bounds,cmap.N) ax.imshow(self.map,cmap=cmap,norm=norm) #second plot is a line which shows the path the robot traveled xy = np.full((self.movenum, 2), 0) for i in range(19): for j in range (19): if self.map[i][j] != 0: order = self.map[i][j] xy[order-1][0] = i xy[order-1][1] = j fig, ax = plt.subplots() for start, stop in zip(xy[:-1], xy[1:]): x, y = zip(start, stop) ax.plot(x, y, color=plt.cm.gist_ncar(np.random.random())) plt.show() return True ################################################## #the update method looks for a right-side wall and follows that wall using several states ################################################## def update(self, time, dt, robot_state, camera_data): #first, if the ball is seen, the robot locates the ball # and orients itself in front of it. Once it reaches the ball, it displays the traversal map of the maze, and celebrates scan = camera_data.scan cmd_vel=ctrl.ControllerOutput(0,0) if self.state == 'done': return None dist_to_ball=-1 blob=self.ball(camera_data) if(blob!=-1): dist_to_ball= ((blob.xyz_mean[1])**2+ (blob.xyz_mean[0])**2)**.5 #print(self.walle(scan)) if (dist_to_ball<.5 and dist_to_ball >0): if (self.state != 'display_stat'): if (self.state != 'celebration'): self.reset_state(time,robot_state.odom_pose, 'display_stat',False,False) else: self.reset_state(time,robot_state.odom_pose, 'celebration',False,False) if(self.state=='forward'): if(dist_to_ball>=.5 and (dist_to_ball<1)): self.reset_state(time,robot_state.odom_pose, 'findball',False,False) if((1<dist_to_ball) and (1.4<self.walle(scan)<1.6)): self.reset_state(time,robot_state.odom_pose, 'findball',False,False) T_world_from_init = self.start_pose T_world_from_cur = robot_state.odom_pose T_init_from_cur = T_world_from_init.inverse() * T_world_from_cur #findball turns towards the ball once the ball has been sighted, then moves straight at the ball if(self.state=='findball'): angle = np.arctan2(blob.xyz_mean[1], blob.xyz_mean[0]) desired_angle = angle turn_angle = desired_angle - T_init_from_cur.angle cmd_vel, done = self.turn(turn_angle) if abs(turn_angle) <= 2*DEG: self.reset_state(time, robot_state.odom_pose, 'forward', False, True) #Each wallcheck state includes a turn which may or may not be called. #to keep track of whether that robot has completed its turn, we use a variable called self.turny #wallcheck1 turns right to check for a right wall if (self.state == 'wallcheck1'): #print('wallcheck1') #turns right if (self.turny==False): desired_angle = -np.pi / 2 turn_angle = desired_angle - T_init_from_cur.angle cmd_vel, done = self.turn(turn_angle) if abs(turn_angle) <= 2*DEG: self.reset_state(time, robot_state.odom_pose, 'wallcheck1', False, True) #if the robot has completed its turn, striaghten, and enter state wallcheck2 if(self.turny== True): cmd_vel, done1 = self.straighten(scan) if(done1): self.turnstarted=False self.reset_state(time, robot_state.odom_pose, 'wallcheck2', False, False) #wallcheck2 moves straight forward if there is no wall in front of the robot, on what formerly was the right side # if there is a wall in front of the robot, turn back to face forwards if self.state == 'wallcheck2': #print('wallcheck2') #if there is no wall in front of the robot, go forward if (self.turny==False): #if(self.walle(scan)==-1): # self.reset_state(time, robot_state.odom_pose, 'forward', False, False) print(self.walle(scan)) if ((self.walle(scan)>=1) and (self.turnstarted==False)): self.reset_state(time, robot_state.odom_pose, 'forward', False, False) elif (self.walle(scan)<1): self.turnstarted=True #if there is a wall, turn left to face the direction which was forward during wallcheck1 if(self.turnstarted==True): desired_angle = np.pi / 2 turn_angle = desired_angle - T_init_from_cur.angle cmd_vel, done = self.turn(turn_angle) if abs(turn_angle) <= 2*DEG: self.reset_state(time, robot_state.odom_pose, 'wallcheck2', False, True) #when done turning, enter wallcheck3 if(self.turny==True): cmd_vel, done1 = self.straighten(scan) if(done1): self.turnstarted=False self.reset_state(time, robot_state.odom_pose, 'wallcheck3', False, False) #In the wallcheck3 state, the robot has turned so that is facing its original forward direction # if there is no wall in front of it, the robot should go forwards. Otherwise, the robot must turn left, and re-enter wallcheck1 if self.state== 'wallcheck3': #print('wallcheck3') #if there is no wall, go forward if(self.turny==False): #if(self.walle(scan)== -1): #self.reset_state(time, robot_state.odom_pose, 'forward', False, False) if ((self.walle(scan)>1) and (self.turnstarted==False)): self.reset_state(time, robot_state.odom_pose, 'forward', False, False) #otherwise, turn left elif (self.walle(scan)<1): self.turnstarted=True if(self.turnstarted==True): desired_angle = np.pi / 2 turn_angle = desired_angle - T_init_from_cur.angle cmd_vel, done = self.turn(turn_angle) if abs(turn_angle) <= 2*DEG: self.reset_state(time, robot_state.odom_pose, 'wallcheck3', False, True) #when done turning left, enter wallcheck1 if(self.turny==True): cmd_vel, done1 = self.straighten(scan) if(done1): self.reset_state(time, robot_state.odom_pose, 'wallcheck1', False,False) #correctly aligns the robot to the center of a square elif self.state == 'nudge': cmd_vel, done = self.nudge(scan) if (done): self.reset_state(time, robot_state.odom_pose, 'wallcheck1', False,False) #forward goes forward, saves its location, and transitions to nudge elif self.state == 'forward': dist_to_drive = 0.9 - np.sqrt(T_init_from_cur.position[0]**2 + T_init_from_cur.position[1]**2) cmd_vel, done = self.drive(dist_to_drive) #every time the robot finishes a move forward, it records its updated location in self.map if done: T=robot_state.odom_pose posy=T.position self.movenum=self.movenum + 1 x=int(np.round(posy[0]))+5 y=int(np.round(posy[1]))+5 self.map[x][y]=self.movenum self.reset_state(time, robot_state.odom_pose, 'nudge', False,False) #when the robot has successfully completed the maze and found the ball, it celebrates by spinning in circles elif self.state=='celebration': cmd_vel=ctrl.ControllerOutput(0,10) done=False #displays the map of the robots visited locations while solving the maze elif self.state=='display_stat': if self.display(): done=True self.reset_state(time, robot_state.odom_pose, 'display_stat', False,False) """elif self.state=='display_stat2': if self.other_plot(): done=True self.reset_state(time, robot_state.odom_pose, 'display_stat2', False,False) """ return cmd_vel ###################################################################### def parse_command_line(): try: command_type = sys.argv[1] mazefile = sys.argv[2] x0 = int(sys.argv[3]) y0 = int(sys.argv[4]) dir0 = maze.DIR_STRING_LOOKUP[sys.argv[5]] if not os.path.exists(mazefile): mazefile = os.path.join(os.path.dirname(__file__), 'maps', mazefile) print('mazefile:', mazefile) assert command_type in ('practice', 'solve') if command_type == 'practice': commands = sys.argv[6:] else: x1 = int(sys.argv[6]) y1 = int(sys.argv[7]) except: name = os.path.basename(sys.argv[0]) print(('usage: python {} practice MAZEFILE X0 Y0 DIRECTION CMD1 [CMD2 ...]\n' ' or: python {} solve MAZEFILE X0 Y0 DIRECTION X1 Y1').format(name, name)) print() raise m = maze.Maze(mazefile) assert m.is_solvable() assert x0 >= 0 and x0 < m.width() assert y0 >= 0 and y0 < m.height() if command_type == 'solve': assert x1 >= 0 and x1 < m.width() assert y1 >= 0 and y1 < m.height() commands = solve_maze(m, x0, y0, dir0, x1, y1) else: x1 = None y1 = None return m, x0, y0, dir0, x1, y1, commands ###################################################################### def make_wall(sim, x0, y0, x1, y1): p0 = np.array([x0, y0], dtype=float) p1 = np.array([x1, y1], dtype=float) pmid = 0.5*(p0 + p1) diff = p1 - p0 dist = np.linalg.norm(diff) offset = 0.5*diff - 0.01*diff/dist angle = (np.random.random()*2-1)*1*DEG c = np.cos(angle) s = np.sin(angle) R = np.array([ [c, -s], [s, c]]) offset = np.dot(R, offset) pmid += 1 p0 = pmid - offset p1 = pmid + offset sim.add_wall(p0, p1) ###################################################################### def setup_maze(sim, m, x0, y0, dir0, x1, y1): w = m.width() h = m.height() sim.set_dims(w+2, h+2) # do outer walls for x in range(w): make_wall(sim, x, 0, x+1, 0) make_wall(sim, x, h, x+1, h) for y in range(h): make_wall(sim, 0, y, 0, y+1) make_wall(sim, w, y, w, y+1) for x in range(w): for y in range(h): if x+1 < w and not m.can_move(x, y, maze.DIR_RIGHT): make_wall(sim, x+1, y, x+1, y+1) if y+1 < h and not m.can_move(x, y, maze.DIR_UP): make_wall(sim, x, y+1, x+1, y+1) init_pos = np.array([x0 + 1.5, y0 + 1.5]) init_angle = maze.DIR_HEADINGS[dir0] angle_offset = (np.random.random())*5*DEG + 5*DEG if np.random.randint(2): angle_offset = -angle_offset init_pos += (np.random.random(2)*2-1)*0.10 init_angle += angle_offset if x1 is not None and y1 is not None: theta = np.linspace(90, (360+90), 11)*DEG ct = np.cos(theta) st = np.sin(theta) r = np.where(np.arange(len(theta)) % 2, 0.125, 0.3) points = np.array([ r*ct, r*st ]).transpose() points += [x1 + 1.5, y1 + 1.5] sim.add_ball([x1+1.5,y1+1.5]) for i in range(len(points)-1): sim.add_tape_strip(points[i:i+2], 'blue') sim.initialize_robot(init_pos, init_angle) ###################################################################### def main(): m, x0, y0, dir0, x1, y1, commands = parse_command_line() controller = MazeController(commands) app = RoboSimApp(controller) setup_maze(app.sim, m, x0, y0, dir0, x1, y1) app.run() if __name__ == '__main__': main()