Exemplo n.º 1
0
class MapGuesser(object):
    def __init__(self, name, nPoints, nBins, announce=False):
        self.name = name
        self.nBins = nBins
        self.nPoints = nPoints
        self.announce = announce

        self.mapData = MapData(np.array([[0.0,0.0]]), (0.0,0.0), (1.0,1.0))
        self.guesser = Guesser(self.name, (0.0, 0.0, 2*math.pi),
                               "location", 
                               (self.nBins[0],self.nBins[1],1))


    def addMap(self, newMapData):
        assert isinstance(newMapData, MapData)

        self.mapData = newMapData

        # Pack the map data into a list of Point objects, since that's
        # what the perfesser wants for input.  This will make the grid
        # into a histogram with which to filter other input points.
        self.mapPointList = []
        for xi in range(self.mapData.mapArray.shape[0]):
            for yi in range(self.mapData.mapArray.shape[1]):
                if self.mapData.mapArray[xi,yi] < 0.5:
                    p = Pt()
                    p.point = (self.mapData.orig[0] + xi * self.mapData.ints[0],
                               self.mapData.orig[1] + yi * self.mapData.ints[1],
                               0.0)
                    self.mapPointList.append(p)

    def handle_guess(self, greq):
        """
        Takes an input guess (a list of Point objects) and throws out
        the points that seem impossible.  Then resamples to return a list
        of equally probable points.  Most of this functionality is provided
        via the perfesser's guesser methods.
        """
        # Check input types
        assert isinstance(greq, GuessRequest)
        assert isinstance(greq.inPoints, list)
        assert isinstance(greq.inPoints[0], Pt)

        self.guesser.newPoints(self.mapPointList)

        pts = Belief()
        pts.points = greq.inPoints
        self.guesser.update(pts)

        # Return
        gresp = GuessResponse(sender = self.name,
                              source_stamp = rospy.Time.now(),
                              source_data = "",
                              outPoints = self.guesser.outPoints(),
                              no_data = False)

        return gresp
Exemplo n.º 2
0
class MapGuesser(object):
    def __init__(self, name, nPoints, nBins, announce=False):
        self.name = name
        self.nBins = nBins
        self.nPoints = nPoints
        self.announce = announce

        self.mapData = MapData(np.array([[0.0, 0.0]]), (0.0, 0.0), (1.0, 1.0))
        self.guesser = Guesser(self.name, (0.0, 0.0, 2 * math.pi), "location",
                               (self.nBins[0], self.nBins[1], 1))

    def addMap(self, newMapData):
        assert isinstance(newMapData, MapData)

        self.mapData = newMapData

        # Pack the map data into a list of Point objects, since that's
        # what the perfesser wants for input.  This will make the grid
        # into a histogram with which to filter other input points.
        self.mapPointList = []
        for xi in range(self.mapData.mapArray.shape[0]):
            for yi in range(self.mapData.mapArray.shape[1]):
                if self.mapData.mapArray[xi, yi] < 0.5:
                    p = Pt()
                    p.point = (self.mapData.orig[0] +
                               xi * self.mapData.ints[0],
                               self.mapData.orig[1] +
                               yi * self.mapData.ints[1], 0.0)
                    self.mapPointList.append(p)

    def handle_guess(self, greq):
        """
        Takes an input guess (a list of Point objects) and throws out
        the points that seem impossible.  Then resamples to return a list
        of equally probable points.  Most of this functionality is provided
        via the perfesser's guesser methods.
        """
        # Check input types
        assert isinstance(greq, GuessRequest)
        assert isinstance(greq.inPoints, list)
        assert isinstance(greq.inPoints[0], Pt)

        self.guesser.newPoints(self.mapPointList)

        pts = Belief()
        pts.points = greq.inPoints
        self.guesser.update(pts)

        # Return
        gresp = GuessResponse(sender=self.name,
                              source_stamp=rospy.Time.now(),
                              source_data="",
                              outPoints=self.guesser.outPoints(),
                              no_data=False)

        return gresp
Exemplo n.º 3
0
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 = []
        for xi in range(self.mapData.ogS.info.width):
            for yi in range(self.mapData.ogS.info.height):
                if self.mapData.mapArrayS[xi,yi] < 50:
                    p = Point()
                    p.point = self.mapData.transform((xi, yi))
                    emptyPointList.append(p)
                    if (self.occupiedNeighbor(xi, yi)):
                        newP = Point()
                        newP.point = (p.point[0] + np.random.normal(0.0, self.mapData.ogS.info.resolution/3.0),
                                      p.point[1] + np.random.normal(0.0, self.mapData.ogS.info.resolution/3.0),
                                      p.point[2])

                        wallPointList.append(newP)

        # Using the wallError, sample the two lists together to get a roughly
        # correct distribution of points to feed to the perfesser.
        self.mapPointList = []
        for i in range(self.nPoints):
            if i < self.wallError * self.nPoints:
                self.mapPointList.append(random.choice(wallPointList))
            else:
                self.mapPointList.append(random.choice(emptyPointList))

        self.guesser.newPoints(self.mapPointList)

        pts = Belief()
        pts.points = newPointList
        self.guesser.update(pts)

        self.pointList = self.guesser.outPoints()
        self.ready_to_publish = False
        return True
Exemplo n.º 4
0
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 = []
        for xi in range(self.mapData.ogS.info.width):
            for yi in range(self.mapData.ogS.info.height):
                if self.mapData.mapArrayS[xi, yi] < 50:
                    p = Point()
                    p.point = self.mapData.transform((xi, yi))
                    emptyPointList.append(p)
                    if self.occupiedNeighbor(xi, yi):
                        newP = Point()
                        newP.point = (
                            p.point[0] + np.random.normal(0.0, self.mapData.ogS.info.resolution / 3.0),
                            p.point[1] + np.random.normal(0.0, self.mapData.ogS.info.resolution / 3.0),
                            p.point[2],
                        )

                        wallPointList.append(newP)

        # Using the wallError, sample the two lists together to get a roughly
        # correct distribution of points to feed to the perfesser.
        self.mapPointList = []
        for i in range(self.nPoints):
            if i < self.wallError * self.nPoints:
                self.mapPointList.append(random.choice(wallPointList))
            else:
                self.mapPointList.append(random.choice(emptyPointList))

        self.guesser.newPoints(self.mapPointList)

        pts = Belief()
        pts.points = newPointList
        self.guesser.update(pts)

        self.pointList = self.guesser.outPoints()
        self.ready_to_publish = False
        return True