def getMapFromServer(self):
print "starting"
resp = self.mapServer()
print "got from server"
occupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution)
print "make grid"
occupancyGrid.setFromDataArrayFromMapServer(resp.map.data)
searchGrid = SearchGrid.fromOccupancyGrid(occupancyGrid)
print resp.map.data
gridDrawer = GridDrawer(searchGrid)
gridDrawer.update()
gridDrawer.waitForKeyPress()
pass
class PlannerControllerNode(object):
def __init__(self):
rospy.init_node('planner_controller', anonymous=True)
self.waitForGoal = threading.Condition()
self.waitForDriveCompleted = threading.Condition()
self.goal = None
self.goalReached = False
def createOccupancyGridFromMapServer(self):
# If we are using the ground truth map, get it directly from stdr. Otherwise,
# retrieve it from the mapper node
if rospy.get_param('use_reactive_planner_controller', False) is False:
rospy.loginfo('Using the ground truth map from stdr')
else:
rospy.loginfo('Getting map size information from stdr')
# 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))
# Copy data over if passive
if rospy.get_param('use_reactive_planner_controller', False) is True:
self.occupancyGrid.scaleEmptyMap()
else:
self.occupancyGrid.setFromDataArrayFromMapServer(map.data)
def mapUpdateCallback(self, msg):
# rospy.loginfo("map update received")
self.plannerController.handleMapUpdateMessage(msg)
def createPlanner(self):
if rospy.get_param('use_fifo_planner', False) is True:
self.planner = FIFOPlanner('FIFO', self.occupancyGrid)
else:
self.planner = AStarPlanner('AStar+Octile+Weight=1', self.occupancyGrid)
self.planner.setPauseTime(0)
self.planner.windowHeightInPixels = rospy.get_param('maximum_window_height_in_pixels', 700)
removeGoalCellFromPathIfOccupied = rospy.get_param('remove_goal_cell_from_path_if_occupied', False)
self.planner.setRemoveGoalCellFromPathIfOccupied(removeGoalCellFromPathIfOccupied)
def createRobotController(self):
self.robotController = Move2GoalController(self.occupancyGrid)
def createPlannerController(self):
if rospy.get_param('use_reactive_planner_controller', False) is True:
self.plannerController = ReactivePlannerController(self.occupancyGrid, self.planner, self.robotController)
else:
self.plannerController = PassivePlannerController(self.occupancyGrid, self.planner, self.robotController)
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(self.goalReached)
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 planner controller, which puts the two together
self.createPlannerController()
# 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 self 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.goalReached = self.plannerController.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()