Esempio n. 1
0
    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)
Esempio n. 2
0
    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()