if(wrapper.ss[1] > 5 and wrapper.ss[1] < 500): front_dist = (wrapper.ss[1]+17)/(GRID_SIZE*100) update = grid.update_grid((current_x,current_y),front_dist,kalfilter._X[2][0]) if(update): print 'Obstacle found - Updating Map' start = (current_x,current_y) start = grid.find_nearest_neighbor(start) arrows, cost = pathplan.a_star_search(grid,start,finish) star_path = pathplan.get_star_path(grid,cost,start,finish) waypoints = pathplan.set_waypoints(grid,star_path,start,finish) #pathplan._zero_turn = True pathplan.new_path = True pathplan._current_waypoint_number = 0 pathplan.update_current_waypoint(waypoints,x,y) pathplan.print_path(grid,star_path,width,height,start,finish) #kalfilter.plot_robot(grid.walls,waypoints) #Now that we know where we are we need to steer now pathplan.update_current_waypoint(waypoints,x,y) #Get the robot outputs vel, ang_vel = pathplan.get_outputs(x,y,theta) if(vel > .5 or ang_vel > math.pi/3): print 'Velocity too high!!' else: if(colorseg_debug): botcontrol.drive(0,0) else: botcontrol.drive(vel,ang_vel)
