""" rospy.loginfo('Waiting for static_map to become available.') rospy.wait_for_service('static_map') mapServer = rospy.ServiceProxy('static_map', GetMap) rospy.loginfo('Found static_map; requesting map data') # Query the map status response = mapServer() map = response.map rospy.loginfo('Got map data') # Allocate the occupancy grid and set the data from the array sent back by the map server occupancyGrid = OccupancyGrid(map.info.width, map.info.height, map.info.resolution) occupancyGrid.setScale(rospy.get_param('plan_scale', 5)) occupancyGrid.setFromDataArrayFromMapServer(map.data) occupancyGrid.expandObstaclesToAccountForCircularRobotOfRadius(0.2) """ ****** UP TO HERE """ start = rospy.get_param("start_pose") goal = rospy.get_param("goal_pose") # Create the planner. The first field is the title which will appear in the # graphics window, the second the occupancy grid used. planner = AStarOctilePlanner('A Star Octile Search', occupancyGrid) # This causes the planner to slow down and pause for things like key entries planner.setRunInteractively(True)
for y in xrange(4, 10): # set block cell positions here occupancyGrid.setCell(y, 17, 1) for y in xrange(0, 14): # set block cell positions here occupancyGrid.setCell(19, y, 1) occupancyGrid.setCell(17, 10, 0) occupancyGrid.setCell(2, 17, 0) occupancyGrid.setCell(8, 18, 0) occupancyGrid.setCell(8, 6, 0) occupancyGrid.setCell(14, 18, 0) occupancyGrid.setCell(17, 19, 0) occupancyGrid.setCell(18, 15, 1) start = (0, 0) goal = (20, 0) occupancyGrid.expandObstaclesToAccountForCircularRobotOfRadius( 0) # set cells size here for testing for Planner in Planners: print Planner.__name__ planner = Planner(Planner.__name__, occupancyGrid) # if Planner == Planners[-1]: # planner.setRunInteractively(True) # hold the drawing by the last case planner.setWindowHeightInPixels(400) goalReached = planner.search(start, goal) # print out infomation path = planner.extractPathToGoal() print '\n'
class PlannerControllerNode(object): def __init__(self): rospy.init_node('planner_controller', anonymous=True) self.waitForGoal = threading.Condition() self.waitForDriveCompleted = threading.Condition() self.goal = None pass def createOccupancyGridFromMapServer(self): # Get the map service rospy.loginfo('Waiting for static_map to become available.') rospy.wait_for_service('static_map') self.mapServer = rospy.ServiceProxy('static_map', GetMap) rospy.loginfo('Found static_map; requesting map data') # Query the map status response = self.mapServer() map = response.map rospy.loginfo('Got map data') # Allocate the occupancy grid and set the data from the array sent back by the map server self.occupancyGrid = OccupancyGrid(map.info.width, map.info.height, map.info.resolution) self.occupancyGrid.setScale(rospy.get_param('plan_scale', 5)) self.occupancyGrid.setFromDataArrayFromMapServer(map.data) self.occupancyGrid.expandObstaclesToAccountForCircularRobotOfRadius( 0.2) def createPlanner(self): self.planner = FIFOPlanner('FIFO', self.occupancyGrid) self.planner.setPauseTime(0) self.planner.windowHeightInPixels = rospy.get_param( 'maximum_window_height_in_pixels', 700) def createRobotController(self): self.robotController = Move2GoalController(self.occupancyGrid) def handleDriveToGoal(self, goal): # Report to the main loop that we have a new goal self.waitForGoal.acquire() self.goal = goal self.waitForGoal.notify() self.waitForGoal.release() # Wait until the robot has finished driving self.waitForDriveCompleted.acquire() self.waitForDriveCompleted.wait() self.waitForDriveCompleted.release() return GoalResponse(True) # Run the planner. Note that we do not take account of the robot orientation when planning. # The reason is simplicity; adding orientation means we have a full 3D planning problem. # As a result, the plan will not be as efficient as it could be. def driveToGoal(self, goal): # Get the current pose of the robot pose = self.robotController.getCurrentPose() start = (pose.x, pose.y) print "start = " + str(start) print "goal = " + str(goal) # Call the planner startCellCoords = self.occupancyGrid.getCellCoordinatesFromWorldCoordinates( start) goalCellCoords = self.occupancyGrid.getCellCoordinatesFromWorldCoordinates( (goal.x, goal.y)) print "startCellCoords = " + str(startCellCoords) print "goalCellCoords = " + str(goalCellCoords) # Exit if we need to if rospy.is_shutdown() is True: return False # Get the plan goalReached = self.planner.search(startCellCoords, goalCellCoords) # Exit if we need to if rospy.is_shutdown() is True: return False # If we can't reach the goal, give up and return if goalReached is False: rospy.logwarn("Could not reach the goal at (%d, %d); moving to next goal", \ goalCellCoords[0], goalCellCoords[1]) return False # Extract the path path = self.planner.extractPathToGoal() startTime = rospy.Time.now() # Now drive it self.robotController.drivePathToGoal(path, goal.theta, self.planner.getPlannerDrawer()) elapsed_time = rospy.Time.now() - startTime print("Elapsed time: " + elapsed_time) return True def run(self): # First set up the occupancy grid self.createOccupancyGridFromMapServer() # Create the planner self.createPlanner() # Set up the robot controller self.createRobotController() # Set up the wait for the service. Note that we can't directly # handle all the driving operations in the service # handler. The reason is that the planner can create a GUI, # and this MUST run in the main thread. The result is pretty # ugly logic and can lead to deadlocking. service = rospy.Service('drive_to_goal', Goal, self.handleDriveToGoal) print 'Spinning to service goal requests' while not rospy.is_shutdown(): # Wait for a new goal. Allow at most 0.1s, which gives # time to check if we are shutting down self.waitForGoal.acquire() self.waitForGoal.wait(0.1) self.waitForGoal.release() # If no goal has been allocated, cycle around if (self.goal is None): continue self.driveToGoal(self.goal) self.goal = None # Signal back to the service handler that we are done self.waitForDriveCompleted.acquire() self.waitForDriveCompleted.notify() self.waitForDriveCompleted.release()