Python OccupancyGrid.sample方法代码示例

# 需要导入模块: from nav_msgs.msg import OccupancyGrid [as 别名]
# 或者: from nav_msgs.msg.OccupancyGrid import sample [as 别名]
class BumpGuesser(object):
    def __init__(self, name, nPoints, nBins, wallError, announce=False):
        self.name = name
        self.nBins = nBins
        self.nPoints = nPoints
        # wallError is the probability that a bump is from a feature that
        # is on the map.  You can bump into moving objects, too.
        self.wallError = wallError
        self.announce = announce

        self.mapData = OccupancyGrid()
        self.guesser = Guesser(
            self.name,
            self.nPoints,
            (0.0, 0.0, 2 * math.pi),
            "location",
            HistogramData((self.nBins[0], self.nBins[1], 1)),
        )

        self.ready_to_publish = False

        self.stamp = rospy.Time.now()

    def feltBump(self):
        """
        Use this to signal that we've felt a bump and will shortly be contacted
        about using it for a guess.
        """
        self.stamp = rospy.Time.now()
        self.ready_to_publish = True

    def occupiedNeighbor(self, xi, yi):
        """
        Returns True if one of the immediate neighbor cells is
        occupied.  You shouldn't expect good results if you call this
        on an occupied cell.
        """

        xmax = self.mapData.og.info.width
        ymax = self.mapData.og.info.height

        if self.mapData.sampled:
            # Fails on an occupied cell
            assert self.mapData.mapArrayS[xi, yi] < 50
            for x in range(max(xi - 1, 0), min(xi + 1, xmax)):
                for y in range(max(yi - 1, 0), min(yi + 1, ymax)):
                    if self.mapData.mapArrayS[x, y] > 50:
                        return True
            return False
        else:
            # Fails on an occupied cell
            assert self.mapData.mapArray[xi, yi] < 50
            for x in range(max(xi - 1, 0), min(xi + 1, xmax)):
                for y in range(max(yi - 1, 0), min(yi + 1, ymax)):
                    if self.mapData.mapArray[x, y] > 50:
                        return True
            return False

    def addMap(self, newMapData):
        """ Receives a new map. """
        assert isinstance(newMapData, MapData)

        self.mapData = newMapData

    def handle_guess(self, newPointList):
        """
        Takes an input guess (a list of Point objects) and filters it
        against a map of points that seem probable because of a recent
        bump.  Then resamples per usual to return a list of equally
        probable points.
        """
        if not self.ready_to_publish:
            return False
        assert isinstance(newPointList, list)
        assert isinstance(newPointList[0], Point)

        # Find the limits of the input data.
        xmax = -1.0e6
        ymax = -1.0e6
        xmin = 1.0e6
        ymin = 1.0e6
        for pt in newPointList:
            xmax = max(xmax, pt.point[0])
            ymax = max(ymax, pt.point[1])
            xmin = min(xmin, pt.point[0])
            ymin = min(ymin, pt.point[1])

        # Shrink the map to accommodate the relevant area
        self.mapData.sample((xmin, ymin), (xmax, ymax))

        # Cruise through the map looking for empty cells next to occupied
        # ones.  These will be the likely cells when a bump is encountered.
        #
        # Because of the possibility of bumping an object that isn't on the
        # map, any empty map cell is possible.  Therefore, we run through
        # the map, packing the map data into a list of Point objects, since
        # that's what the perfesser wants for input.  While we're running
        # through, we keep a separate list of empty cells next to full ones.
        wallPointList = []
        emptyPointList = []
#.........这里部分代码省略.........