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