def mapUpdateCallback(self, msg):
rospy.loginfo("map update received")
# If the occupancy grids do not exist, create them
if self.occupancyGrid is None:
self.occupancyGrid = OccupancyGrid.fromMapUpdateMessage(msg)
self.deltaOccupancyGrid = OccupancyGrid.fromMapUpdateMessage(msg)
# Update the grids
self.occupancyGrid.updateGridFromVector(msg.occupancyGrid)
current_known_count = self.countKnownCells(self.occupancyGrid)
# print('UPDATE occupancy grid {}'.format(current_known_count))
now = rospy.get_rostime().secs
# rospy.loginfo("Elapsted time %i", now - self.startTime)
# For plotting
self.TimeAndCoverageArray += [(now - self.startTime,
current_known_count)]
# Update the frontiers
self.updateFrontiers()
# Flag there's something to show graphically
self.visualisationUpdateRequired = True
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 service')
# Wait for the
rospy.loginfo('Waiting for the request_map_update service')
rospy.wait_for_service('request_map_update')
rospy.loginfo('request_map_update service registered')
# Now pull the rest of the data from the mapping node
mapRequestService = rospy.ServiceProxy('request_map_update',
RequestMapUpdate)
mapUpdate = mapRequestService(False)
self.occupancyGrid = OccupancyGrid.fromMapUpdateMessage(
mapUpdate.initialMapUpdate)
#self.occupancyGrid.setScale(rospy.get_param('plan_scale', 5))
# Debug code
if False:
uniqueCellValues1 = []
for x in range(self.occupancyGrid.widthInCells):
for y in range(self.occupancyGrid.heightInCells):
if self.occupancyGrid.grid[x][y] not in uniqueCellValues1:
uniqueCellValues1.append(self.occupancyGrid.grid[x][y])
print '**************************************************************************************'
print str(uniqueCellValues1)
print '**************************************************************************************'
def mapUpdateCallback(self, msg):
# rospy.loginfo("map update received")
# If the occupancy grids do not exist, create them
if self.occupancyGrid is None:
self.occupancyGrid = OccupancyGrid.fromMapUpdateMessage(msg)
self.deltaOccupancyGrid = OccupancyGrid.fromMapUpdateMessage(msg)
# Update the grids
self.occupancyGrid.updateGridFromVector(msg.occupancyGrid)
self.deltaOccupancyGrid.updateGridFromVector(msg.deltaOccupancyGrid)
# Update the frontiers
self.updateFrontiers()
# Flag there's something to show graphically
self.visualisationUpdateRequired = True