def storeGoal(self,msg): # goalX = tools.mapifyValue(msg.pose.position.x) -1 # goalY = tools.mapifyValue(msg.pose.position.y) goalX = tools.gmapifyValue(msg.pose.position.x) goalY = tools.gmapifyValue(msg.pose.position.y) goaltheta= tools.normalizeTheta((msg.pose.orientation.x,msg.pose.orientation.y,msg.pose.orientation.z,msg.pose.orientation.w)) self._goal_,self._current_ = self.map.storeGoal(goalX,goalY,goaltheta) try: path = self.map.getNextWaypoint() print path except RuntimeError,e: print e
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 _updateLocation(self): print "Updating location" try: self._map_list.waitForTransform('map', 'base_footprint', rospy.Time(0), rospy.Duration(0.5)) except tf.Exception: print "Waiting for transform failed" self.current_x = tools.gmapifyValue(0) self.current_y = tools.gmapifyValue(0) self.current_z = tools.gmapifyValue(0) return False (p,q) = self._map_list.lookupTransform("map","base_footprint",rospy.Time(0)) x,y,z = p self.current_theta = tools.normalizeTheta(q) self.current_x = tools.gmapifyValue(x) self.current_y = tools.gmapifyValue(y) self.current_z = tools.gmapifyValue(z) self._currentSet = True print "The values are: ",self.current_x, self.current_y # self.current_x = tools.gmapifyValue(self._newX) # self.current_y = tools.gmapifyValue(self._newY) self._currentSet = True # print "The values are: ",self.current_x, self.current_y return True