def storeCostmap(self, msg, gMap=True):
print "\n\nSTORING THE COSTMAP\n\n"
try:
self._map_tfListener.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(0.5))
except tf.Exception:
print "TF library chunkin out broski"
return
self._mapLL = tools.lMaptoLLMap(msg.data,
msg.info.height,
msg.info.width)
self._mapL = msg.data
# self._max_w = 37#msg.info.width
# self._max_h = 37#msg.info.height
self._max_w = msg.info.width
self._max_h = msg.info.height
self._pose = msg.info.origin.position
try: ## Try to get the transform to the location of the robot.
(p,q) = self._map_tfListener.lookupTransform("map","base_footprint",rospy.Time(0))
except tf.Exception:
print "Python can't read your future, calm down"
return
self._reducedMap = self._mapLL
self._reduce_w = self._max_w
self._reduce_h = self._max_h
# print "Reduced map is: "
# print self._reducedMap
if gMap: ## IF this is being run on the gMap
self._ox,self._oy,_ = p ## Get the position of the robot from the map to base footprint transform
self._ox = tools.gmapifyValue(self._ox + self._pose.x) ## create a '/map' grid value from the sum of the robot location and the offset to the x min of the costmap
self._oy = tools.gmapifyValue(self._oy + self._pose.y) ## create a '/map' grid value from the sum of the robot location and the offset to the y min of the costmap
self._reducedMap = self._mapLL
self._reduce_w = self._max_w
self._reduce_h = self._max_h
else: ## IF this is being run in the simulator
## Set the offsets in the map space
self._ox,self._oy,_ = p ## Get the position of the robot from the map to base footprint transform
self._ox = tools.mapifyValue(self._ox + self._pose.x + 1) ## create a '/map' grid value from the sum of the robot location and the offset to the x min of the costmap
self._oy = tools.mapifyValue(self._oy + self._pose.y) ## create a '/map' grid value from the sum of the robot location and the offset to the y min of the costmap
## Reduce the resolutino on this map:
self._reducedMap, self._reduce_w, self._reduce_h = self._shrinkTwo()
def _updateMap(self,msg):
print "_updateMap has been called"
# print msg
self._map = tools.lMaptoLLMap(msg.data,
msg.info.height,
msg.info.width)
self._max_h = msg.info.height
self._max_w = msg.info.width
self._pose = msg.info.origin
#
# self._newX = msg.x
# self._newY = msg.y
self._mapSet = True
self._updateLocation()
def _updateMap(self, msg):
"""
This is the callback for when the '/map' topic publishes changes.
The function is smart enough to know when it is the first vs. when the map is updated.
:param msg:
:return: None
"""
self._map = tools.lMaptoLLMap(msg.data,
msg.info.height,
msg.info.width)
self._height=msg.info.height
self._width=msg.info.width
self._pose=msg.info.origin
print "The real map's h and w", self._height, self._width
if self._startup is True:
self._repaint_map(baseMap=True)
self._startup = True
