def _updateLocation(self): try: self._map_list.waitForTransform('map', 'base_footprint', rospy.Time.now(), rospy.Duration(0.5)) except tf.Exception: print "TF library chunkin out broski" return (p,q) = self._map_list.lookupTransform("map","base_footprint",rospy.Time.now()) self.current_x,self.current_y, self.current_z = p self.current_theta = tools.normalizeTheta(q) self.current_x = tools.mapifyValue(self.current_x) - 1 self.current_y = tools.mapifyValue(self.current_y)
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()