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 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
#! /usr/bin/env python
# Import the needed types.
from comp0037_reactive_planner_controller.occupancy_grid import OccupancyGrid
from comp0037_reactive_planner_controller.fifo_planner import FIFOPlanner
# Create the occupancy grid. Syntax is: number of cells in X, number of cells in Y,
# length of each cell in m
occupancyGrid = OccupancyGrid(21, 21, 0.5)
# The cells are indexed starting from 0.
# Set the state of the cells in the range [11,1]-[11,19] to be occupied.
# This corresponds to the "easy case" in the lectures
for y in xrange(1, 19):
occupancyGrid.setCell(11, y, 1)
# Start and goal cells
start = (3, 18)
goal = (20, 0)
# Create the planner. The first field is the title which will appear in the
# graphics window, the second the occupancy grid used.
planner = FIFOPlanner('Breadth First Search', occupancyGrid)
# This causes the planner to slow down and pause for things like key entries
planner.setRunInteractively(True)
# This specifies the height of the window drawn showing the occupancy grid. Everything
# should scale automatically to properly preserve the aspect ratio
planner.setWindowHeightInPixels(400)
def __init__(self):
# Get the ground truth map from stdr; we use this to figure out the dimensions of the map
rospy.init_node('mapper_node', anonymous=True)
self.mapServer = rospy.ServiceProxy('static_map', GetMap)
resp = self.mapServer()
self.entropyFile = open(
'/home/ros_user/cw2_catkin_ws/src/comp0037/comp0037_mapper/src/comp0037_mapper/entropy.txt',
'w')
# Drawing options
self.showOccupancyGrid = rospy.get_param('show_mapper_occupancy_grid',
True)
self.showDeltaOccupancyGrid = rospy.get_param(
'show_mapper_delta_occupancy_grid', True)
# Create the publisher for the regular map messages
self.mapUpdatePublisher = rospy.Publisher('updated_map',
MapUpdate,
queue_size=1)
# Get the map scale
mapScale = rospy.get_param('plan_scale', 5)
# Create the occupancy grid map. This is the "raw" one from the sensor.
self.occupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0.5)
self.occupancyGrid.setScale(mapScale)
self.occupancyGrid.scaleEmptyMap()
# Create the delta occupancy grid map. This stores the difference since the last time the map was sent out
self.deltaOccupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGrid.setScale(mapScale)
self.deltaOccupancyGrid.scaleEmptyMap()
windowHeight = rospy.get_param('maximum_window_height_in_pixels', 600)
if self.showOccupancyGrid is True:
self.occupancyGridDrawer = OccupancyGridDrawer('Mapper Node Occupancy Grid',\
self.occupancyGrid, windowHeight)
self.occupancyGridDrawer.open()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow = OccupancyGrid(
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGridForShow.setScale(mapScale)
self.deltaOccupancyGridForShow.scaleEmptyMap()
self.deltaOccupancyGridDrawer = OccupancyGridDrawer('Mapper Node Delta Occupancy Grid',\
self.deltaOccupancyGridForShow, windowHeight)
self.deltaOccupancyGridDrawer.open()
# Flag set to true if graphics can be updated
self.visualisationUpdateRequired = False
# Set up the lock to ensure thread safety
self.dataCopyLock = Lock()
self.noOdometryReceived = True
self.noTwistReceived = True
self.noLaserScanReceived = True
self.enableMapping = True
# Register the supported services
self.changeMapperStateService = rospy.Service('change_mapper_state',
ChangeMapperState,
self.mappingStateService)
self.requestMapUpdateService = rospy.Service(
'request_map_update', RequestMapUpdate,
self.requestMapUpdateService)
# Set up the subscribers. These track the robot position, speed and laser scans.
self.mostRecentOdometry = Odometry()
self.odometrySubscriber = rospy.Subscriber("robot0/odom",
Odometry,
self.odometryCallback,
queue_size=1)
self.mostRecentTwist = Twist()
self.twistSubscriber = rospy.Subscriber('/robot0/cmd_vel',
Twist,
self.twistCallback,
queue_size=1)
self.laserSubscriber = rospy.Subscriber("robot0/laser_0",
LaserScan,
self.laserScanCallback,
queue_size=1)
def __init__(self):
rospy.init_node('mapper_node', anonymous=True)
# Check if we use the ground truth map from stdr; if this is enabled, the mapper will ignore
# the laser scans
self.useGroundTruthMap = True
# Set up the lock to ensure thread safety
self.dataCopyLock = Lock()
self.noOdometryReceived = True
self.noTwistReceived = True
self.noLaserScanReceived = True
self.enableMapping = True
# List of active obstacles
self.activatedObstacles = []
self.disactivatedObstacles = []
self.activeObstacles = []
# Get the ground truth map from stdr; we use this to figure out the dimensions of the map
rospy.loginfo('waiting for the static_map service')
rospy.wait_for_service('static_map')
self.mapServer = rospy.ServiceProxy('static_map', GetMap)
resp = self.mapServer()
# Drawing options
self.showOccupancyGrid = rospy.get_param('show_mapper_occupancy_grid',
True)
self.showDeltaOccupancyGrid = rospy.get_param(
'show_mapper_delta_occupancy_grid', True)
# Flag set to true if graphics can be updated
self.visualisationUpdateRequired = False
# Create the publisher for the regular map messages
self.mapUpdatePublisher = rospy.Publisher('updated_map',
MapUpdate,
queue_size=1)
# Get the map scale
mapScale = rospy.get_param('plan_scale', 5)
# Create the occupancy grid
self.occupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0.5)
self.occupancyGrid.setScale(mapScale)
self.occupancyGrid.scaleEmptyMap()
# Create the delta occupancy grid map. This stores the difference since the last time the map was sent out
self.deltaOccupancyGrid = OccupancyGrid(resp.map.info.width,
resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGrid.setScale(mapScale)
self.deltaOccupancyGrid.scaleEmptyMap()
# If we are using ground truth, create a ground truth occupancy map (used for reference) and set its value to the
# ground truth map.
if self.useGroundTruthMap is True:
self.groundTruthOccupancyGrid = OccupancyGrid(
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution, 0.5)
self.groundTruthOccupancyGrid.setScale(mapScale)
self.groundTruthOccupancyGrid.setFromDataArrayFromMapServer(
resp.map.data)
self.updateOccupancyGridFromGroundTruthAndObstacles()
self.visualisationUpdateRequired = True
self.noLaserScanReceived = False
# Debug code
if False:
uniqueCellValues1 = []
for x in range(self.occupancyGrid.widthInCells):
for y in range(self.occupancyGrid.heightInCells):
if self.groundTruthOccupancyGrid.grid[x][
y] not in uniqueCellValues1:
uniqueCellValues1.append(self.occupancyGrid.grid[x][y])
uniqueCellValues = []
print str(len(resp.map.data))
for x in range(len(resp.map.data)):
g = resp.map.data[x]
if g not in uniqueCellValues:
uniqueCellValues.append(g)
print '**************************************************************************************'
print str(uniqueCellValues)
print str(uniqueCellValues1)
print '**************************************************************************************'
windowHeight = rospy.get_param('maximum_window_height_in_pixels', 600)
if self.showOccupancyGrid is True:
self.occupancyGridDrawer = OccupancyGridDrawer('Mapper Node Occupancy Grid',\
self.occupancyGrid, windowHeight)
self.occupancyGridDrawer.open()
if self.showDeltaOccupancyGrid is True:
self.deltaOccupancyGridForShow = OccupancyGrid(
resp.map.info.width, resp.map.info.height,
resp.map.info.resolution, 0)
self.deltaOccupancyGridForShow.setScale(mapScale)
self.deltaOccupancyGridForShow.scaleEmptyMap()
self.deltaOccupancyGridDrawer = OccupancyGridDrawer('Mapper Node Delta Occupancy Grid',\
self.deltaOccupancyGridForShow, windowHeight)
self.deltaOccupancyGridDrawer.open()
# Set up the subscribers. These track the robot position, speed, threshold and laser scans.
self.mostRecentOdometry = Odometry()
self.odometrySubscriber = rospy.Subscriber("robot0/odom",
Odometry,
self.odometryCallback,
queue_size=1)
self.mostRecentTwist = Twist()
self.twistSubscriber = rospy.Subscriber('/robot0/cmd_vel',
Twist,
self.twistCallback,
queue_size=1)
self.laserSubscriber = rospy.Subscriber("robot0/laser_0",
LaserScan,
self.laserScanCallback,
queue_size=1)
self.addObstacleToMapSubscriber = \
rospy.Subscriber("/add_obstacle_to_map", Int32, self.addObstacleToMapCallback, queue_size=1)
self.removeObstacleFromMapSubscriber = \
rospy.Subscriber("/remove_obstacle_from_map", Int32, self.removeObstacleFromMapCallback, queue_size=1)
# Register the supported services
self.changeMapperStateService = rospy.Service('change_mapper_state',
ChangeMapperState,
self.mappingStateService)
self.requestMapUpdateService = rospy.Service(
'request_map_update', RequestMapUpdate,
self.requestMapUpdateService)