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
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)
#! /usr/bin/env python
from comp0037_planner_controller.occupancy_grid import OccupancyGrid
from comp0037_planner_controller.greedy_planner import GREEDYPlanner
occupancyGrid = OccupancyGrid(21, 21, 0.5)
for y in xrange(0, 19):
occupancyGrid.setCell(11, y, 1)
start = (3, 18)
goal = (20, 0)
planner = GREEDYPlanner('Greedy Search', occupancyGrid);
planner.setRunInteractively(True)
planner.setWindowHeightInPixels(400)
goalReached = planner.search(start, goal)
path = planner.extractPathToGoal()
from comp0037_planner_controller.fifo_planner import FIFOPlanner
from comp0037_planner_controller.lifo_planner import LIFOPlanner
from comp0037_planner_controller.greedy_planner import GreedyPlanner
from comp0037_planner_controller.greedy_rewiring_planner import GreedyRewiringPlanner
from comp0037_planner_controller.astar_by_C_planner import AStarByCPlanner
from comp0037_planner_controller.astar_by_ED_planner import AStarByEDPlanner
from comp0037_planner_controller.astar_by_OD_planner import AStarByODPlanner
from comp0037_planner_controller.astar_by_MD_planner import AStarByMDPlanner
from comp0037_planner_controller.astar_by_SED_planner import AStarBySEDPlanner
# Planners = (FIFOPlanner, LIFOPlanner, GreedyPlanner, GreedyRewiringPlanner, DijkstraPlanner)
Planners = (FIFOPlanner, LIFOPlanner)
for mapindex in range(1, 4):
occupancyGrid = OccupancyGrid(21, 21, 0.5)
if mapindex == 0: #empty map
start = (2, 18)
goal = (20, 0)
elif mapindex == 1: #One Line Map
for y in xrange(0, 20): # set block cell positions here
occupancyGrid.setCell(11, y, 1)
start = (0, 0)
goal = (20, 0)
elif mapindex == 2:
for y in xrange(1, 20): # set block cell positions here
occupancyGrid.setCell(2, y, 1)
for y in xrange(0, 18): # set block cell positions here
#! /usr/bin/env python
from comp0037_planner_controller.occupancy_grid import OccupancyGrid
from comp0037_planner_controller.dijkstra_planner import DIJKSTRAPlanner
occupancyGrid = OccupancyGrid(21, 21, 0.5)
#for y in xrange(1, 19):
# occupancyGrid.setCell(11, y, 1)
start = (10, 10)
goal = (14, 10)
planner = DIJKSTRAPlanner('Dijkstra', occupancyGrid)
planner.setRunInteractively(True)
planner.setWindowHeightInPixels(400)
goalReached = planner.search(start, goal)
path = planner.extractPathToGoal()
#planner.extractPathEndingAtCoord((20,10))
#planner.extractPathEndingAtCoord((2,10))
#planner.extractPathEndingAtCoord((8,13))
"""
****** FOR FACTORY MAP ONLY
"""
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)
#! /usr/bin/env python
from comp0037_planner_controller.occupancy_grid import OccupancyGrid
from comp0037_planner_controller.fifo_planner import FIFOPlanner
# Create the occupancy grid
occupancyGrid = OccupancyGrid(21, 21, 0.5)
for y in xrange(1, 19):
occupancyGrid.setCell(11, y, 1)
# Start and goal cells
start = (3, 18)
goal = (20, 0)
# Create the planner on the original map
planner = FIFOPlanner('Depth First Search Original Occupancy Grid',
occupancyGrid)
planner.setWindowHeightInPixels(400)
planner.search(start, goal)
path = planner.extractPathToGoal()
# Now try it on our Minkowski sum map with radius 0.5 (1 cell)
occupancyGrid.expandObstaclesToAccountForCircularRobotOfRadius(0.5)
planner = FIFOPlanner('Depth First Search Robot Radius 0.5', occupancyGrid)
planner.setWindowHeightInPixels(400)
planner.search(start, goal)
path = planner.extractPathToGoal()
# Now try it on our Minkowski sum map with radius 0. Should be the same as the original
occupancyGrid.expandObstaclesToAccountForCircularRobotOfRadius(0)
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()
from comp0037_planner_controller.lifo_planner import LIFOPlanner
from comp0037_planner_controller.greedy_planner import GreedyPlanner
from comp0037_planner_controller.greedy_rewiring_planner import GreedyRewiringPlanner
from comp0037_planner_controller.astar_by_C_planner import AStarByCPlanner
from comp0037_planner_controller.astar_by_ED_planner import AStarByEDPlanner
from comp0037_planner_controller.astar_by_OD_planner import AStarByODPlanner
from comp0037_planner_controller.astar_by_MD_planner import AStarByMDPlanner
from comp0037_planner_controller.astar_by_ED_planner import AStarByEDPlanner
Planners = (FIFOPlanner, LIFOPlanner, GreedyPlanner, GreedyRewiringPlanner,
DijkstraPlanner, AStarByCPlanner, AStarByEDPlanner,
AStarByODPlanner, AStarByMDPlanner
) # append planners here for testing
occupancyGrid = OccupancyGrid(21, 21, 0.5)
for y in xrange(0, 20): # set block cell positions here
occupancyGrid.setCell(11, y, 1)
start = (2, 18)
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)
