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()
"""
****** 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)