예제 #1
0
    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
예제 #2
0
    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
예제 #3
0
    def checkBump(self, req):
        """
        Parses a SensorPacket for the bump and if it's there, fires
        off a message about it.  There are some provisions to make
        sure one bump doesn't trigger too large a number of responses.
        """
        if rospy.Time.now().to_sec() < self.waitTime:
            return

        if req.bumpLeft and req.bumpRight:
            direction = 0.0
        elif req.bumpLeft:
            direction = 1.0
        elif req.bumpRight:
            direction = -1.0
        else:
            return

        # Should consider using the Perfesser here for the means() and stds()
        bumpEst = Belief(
            source_stamp=self.curLocation.header.stamp,
            sender=self.name,
            source_data=self.curLocation.source_data,
            data_type=self.guesser.data_type,
            data_sub_type=self.guesser.data_sub_type,
            pers=self.guesser.periods,
        )
        bumpEst.points = []

        for p in self.curLocation.points:

            # Estimate the point on the robot's perimeter where the bump was
            # felt.
            psiCorr = p.point[2] + direction * 0.7
            # Calculate the opposite-facing direction.
            phiCorr = (psiCorr + math.pi) % (2 * math.pi)
            phiCorr -= (2 * math.pi) if psiCorr > math.pi else 0.0

            bumpEst.points.append(
                Pt(
                    point=(
                        p.point[0] + self.robotRadius * math.cos(psiCorr),
                        p.point[1] + self.robotRadius * math.sin(psiCorr),
                        phiCorr,
                    )
                )
            )

        self.guesser.newPoints(bumpEst.points)
        bumpEst.means = self.guesser.means()
        bumpEst.stds = self.guesser.stds()

        if self.bpub:
            bumpEst.header.stamp = rospy.Time.now()
            self.bpub.publish(bumpEst)

        self.waitTime = rospy.Time.now().to_sec() + 5.0
예제 #4
0
    def checkBump(self, req):
        """
        Parses a SensorPacket for the bump and if it's there, fires
        off a message about it.  There are some provisions to make
        sure one bump doesn't trigger too large a number of responses.
        """
        if rospy.Time.now().to_sec() < self.waitTime:
            return

        if req.bumpLeft and req.bumpRight:
            direction = 0.0
        elif req.bumpLeft:
            direction = 1.0
        elif req.bumpRight:
            direction = -1.0
        else:
            return

        # Should consider using the Perfesser here for the means() and stds()
        bumpEst = Belief(source_stamp=self.curLocation.header.stamp,
                         sender=self.name,
                         source_data=self.curLocation.source_data,
                         data_type=self.guesser.data_type,
                         data_sub_type=self.guesser.data_sub_type,
                         pers=self.guesser.periods)
        bumpEst.points = []

        for p in self.curLocation.points:

            # Estimate the point on the robot's perimeter where the bump was
            # felt.
            psiCorr = p.point[2] + direction * 0.7
            # Calculate the opposite-facing direction.
            phiCorr = (psiCorr + math.pi) % (2 * math.pi)
            phiCorr -= (2 * math.pi) if psiCorr > math.pi else 0.0

            bumpEst.points.append( \
                Pt(point=(p.point[0] + self.robotRadius * math.cos(psiCorr),
                          p.point[1] + self.robotRadius * math.sin(psiCorr),
                          phiCorr)))

        self.guesser.newPoints(bumpEst.points)
        bumpEst.means = self.guesser.means()
        bumpEst.stds = self.guesser.stds()

        if self.bpub:
            bumpEst.header.stamp = rospy.Time.now()
            self.bpub.publish(bumpEst)

        self.waitTime = rospy.Time.now().to_sec() + 5.0
예제 #5
0
    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
예제 #6
0
    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
예제 #7
0
    rospy.Subscriber('guesser/position/guessers', Announce, server.updateStack)

    rospy.sleep(5.0) # wait for the subscriber to be set up

    pts = Belief()
    pts.sender = "perfesser"
    pts.data_type = "location"
    pts.pers = (0.0, 0.0, 2 * math.pi)
    counter = 0

    while True:
        r = server.surveyGuess(guesser)

        if not r.no_data:
            pts.points = guesser.outPoints()
            pts.means = guesser.means()
            pts.stds = guesser.stds()
            pts.source_stamp = guesser.stamp
            pts.source_data = guesser.source_data
            pts.data_type = "location"
            pts.header.stamp = rospy.Time.now()
            counter += 1
            pub.publish(pts)

        rospy.sleep(wait_time)



    rospy.spin()