Esempio n. 1
0
    def getPosEst(self):

        meanloc = (np.random.normal(loc=self.pos[0],
                                    scale=self.std / 2.,
                                    size=1)[0],
                   np.random.normal(loc=self.pos[1],
                                    scale=self.std / 2.,
                                    size=1)[0])

        out = Belief(source_stamp=rospy.Time.now(),
                     source_data="estimate",
                     data_type="location",
                     data_sub_type=self.name,
                     sender=self.name,
                     dim_names=[self.name + "/xpos", self.name + "/ypos"],
                     pers=(0.0, 0.0),
                     means=meanloc,
                     stds=[self.std, self.std])

        for x, y in zip(
                np.random.normal(loc=meanloc[0],
                                 scale=self.std,
                                 size=self.npoints),
                np.random.normal(loc=meanloc[1],
                                 scale=self.std,
                                 size=self.npoints)):
            out.points.append(Pt(point=(x, y)))

        return out
Esempio n. 2
0
    def goThatWay(self, moves):
        """
        Takes an array of directions to move in and selects a point in
        space in that direction.  The choice of point is fairly
        arbitrary.  We try to multiply the direction by a factor large
        enough that it won't be within the gnav.zeroDistance of the
        current location, unless the intent is really that we should
        not move.  (That's what the kGo factor is for.)
        """
        kGo = 2.0

        print "moves", moves

        meanMove =  [ np.mean(moves[:, 0]), np.mean(moves[:, 1]) ]
        sumsq = sum([d**2 for d in meanMove])**0.5

        # If the target point is close, don't normalize.  Otherwise, set
        # a goal in the general direction
        if sumsq > 1.0:
            meanMove = [ kGo * x/sumsq for x in meanMove ]

        self.goal = (self.pos.means[0] + meanMove[0],
                     self.pos.means[1] + meanMove[1], 0.0)

        print "We're at: (%.3f,%.3f)" % (self.pos.means[0], self.pos.means[1])
        print "headed to: (%.3f,%.3f)" % (self.goal[0], self.goal[1])

        # Eventually this should publish to a goal-setting topic, to be
        # read by some goal-setter over at gnav.
        self.goalPub.publish(Pt(point=self.goal))
Esempio n. 3
0
    def takeAdvice(self, req):
        """
        Takes a piece of advice from its neighbor, rates it, and
        returns the rating.  The advice is a distribution of points to
        which the other robot thinks this robot ought to go.  We
        compare that advice to our own intentions and return ratings
        indicating the degree of agreement between the two: 1.0 is
        complete agreement and 0.0 is complete disagreement.  We never
        return exactly 0.0 as a matter of policy.
        """
        if not req.question:
            return MRFQueryResponse(no_data=True)
        if not self.deltaConfig.any():
            return MRFQueryResponse(no_data=True)

        # Allocate our intention into bins.
        question = self.ptsToArray(req.question)

        # Establish bins so that there is one point in each outer bin.
        # (The Histogram class in perfesser is set up so the outer
        # bins will be empty, which is why we're not using it here.
        mins = [min(self.deltaConfig[:, 0]), min(self.deltaConfig[:, 1])]
        maxs = [max(self.deltaConfig[:, 0]), max(self.deltaConfig[:, 1])]

        bins = [ np.linspace(mn + 1.e-10, mx - 1.e-10, bn) for \
                     mn,mx,bin in zip(mins, maxs, self.nbins) ]
        hist = np.zeros(self.nbins)

        # Create a distribution of points that includes neighbor's
        # opinions except for the one asking the question.
        phi = self.deltaConfig[:, (0, 1)]

        messageProduct = [1.0] * phi.shape[0]
        for nb in self.neighbors:
            if (nb.name != req.asker) and (nb.lastReply.points):
                messageProduct = [ m * p.point[0] for m,p in \
                                   zip(messageProduct,
                                       nb.lastReply.points) ]

        phi = self.resample(phi, messageProduct)

        # Populate bins
        for delta in phi:
            hist[sum(delta[0] > bins[0])][sum(delta[1] > bins[1])] += 1.0

        n = sum(sum(hist))

        # Formulate a reply by binning the input data.
        m = MRFQueryResponse(no_data=False, source_stamp=req.source_stamp)
        for q in req.question:
            m.reply.append(Pt(point=( \
                        hist[sum(q.point[0] > bins[0])][sum(q.point[1] > bins[1])]/n, )))

        # print "entering service: ", self.name
        # print self.name, "'s opinion: ", self.deltaConfig
        # print "question: ",question
        # print "hist: ", hist
        # print "bins: ", bins
        # print m
        return m
Esempio n. 4
0
    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)
Esempio n. 5
0
 def list(self):
     """
     Compiles a list of robots and locations in the room.
     """
     out = Room(sourceName="listing")
     for rb in self.robots.values():
         out.names.append(rb.name)
         out.locations.append(Pt(point=(rb.x, rb.y)))
     return out
Esempio n. 6
0
    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)
Esempio n. 7
0
    def arrayToBelief(self, inArray):
        """
        Takes the numpy-style array in self.pointArray and packs it
        into an otherwise empty Belief object.  The object is
        returned, but should usually have its secondary data filled in
        before being published or used anywhere.
        """
        assert isinstance(inArray, np.ndarray)

        return Belief( points = [ Pt(point=row) for row in inArray ] )
Esempio n. 8
0
    def unwind(self):
        """Returns a Room2D object containing all the connections."""
        out = Room2D()
        for node in self.maps.values():
            rm = Room(sourceName=node.name)
            for friend in node.friends:
                rm.names.append(friend.name)
                rm.locations.append(Pt(point=friend.location))
            out.robots.append(rm)

        return out
Esempio n. 9
0
        def testUpdate(self):
            # Create a uniform distribution
            rospy.init_node('perfesser')

            pts = Belief()
            for i in range(1000):
                x = np.random.random()
                y = np.random.random()
                th = np.random.random()

                p = Pt()
                p.point = [x, y, th]
                pts.points.append(p)

            xmxflag = False ; ymxflag = False ; thmxflag = False
            xmnflag = False ; ymnflag = False ; thmnflag = False

            ntrials = 5 ; thres = 0.01
            for i in range(ntrials):
                self.g.update(pts)

                mx = map(max, self.g.pointArray.transpose())
                mn = map(min, self.g.pointArray.transpose())

                xmxflag = xmxflag or ((math.fabs(mx[0]) - 1.0) < thres)
                ymxflag = ymxflag or ((math.fabs(mx[0]) - 1.0) < thres)
                thmxflag = thmxflag or ((math.fabs(mx[0]) - 1.0) < thres)
                xmnflag = xmnflag or (math.fabs(mn[0]) < thres)
                ymnflag = ymnflag or (math.fabs(mn[0]) < thres)
                thmnflag = thmnflag or (math.fabs(mn[0]) < thres)
                self.setUp()

            self.assertTrue(self.pub.testObject)

            self.assertTrue(xmxflag)
            self.assertTrue(ymxflag)
            self.assertTrue(thmxflag)
            self.assertTrue(xmnflag)
            self.assertTrue(ymnflag)
            self.assertTrue(thmnflag)
Esempio n. 10
0
    def recordNeighbors(self, req):
        """
        At any time, the self.neighbors list should contain a Neighbor
        object for each robot whose position we can measure directly.

        We are receiving a list of robots and distances (signal
        strengths) via the /olsr/neighbors topic and accumulating.
        Use this as a callback to the /olsr/neighbors topic.  The
        result of calling this is an updated self.neighbors array.
        """
        ### This is a hack until the OLSR ROS node is working.  We just
        ### use a fixed list of all the robots and subtract our name.

        rm = Room(sourceName = self.name)
        rm.names = req
        rm.locations = [ Pt(point = (1.0, 0.0)), Pt(point = (1.0, 0.0)) ]

        rm.names.remove(self.name)

        self.neighbors = []
        for name in rm.names:
            self.neighbors.append(Neighbor(name = name))
Esempio n. 11
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
Esempio n. 12
0
    def nearest(self):
        """
        Returns a list of robots in the room, sorted from low xpos to high.
        """
        robotLocs = []
        for rb in self.robots:
            robotLocs.append([rb.x, rb.name])

        robotLocs.sort()

        r = Room(locations=[Pt(point=(loc, )) for loc, name in robotLocs],
                 names=[name for loc, name in robotLocs])

        return r
Esempio n. 13
0
    def getPosEst(self):
        out = Belief(source_stamp=rospy.Time.now(),
                     source_data="estimate",
                     data_type="location",
                     data_sub_type=self.name,
                     sender=self.name,
                     dim_names=[self.name + "/xpos"],
                     pers=(0.0, ),
                     means=[self.xpos],
                     stds=[self.std])

        for x in np.random.normal(loc=self.xpos,
                                  scale=self.std,
                                  size=self.npoints):
            out.points.append(Pt(point=(x, )))

        return out
Esempio n. 14
0
    def nearest(self):
        """
        Returns a list of robots in the room, accompanied by the names and
        locations of its neighbors.
        """
        out = Room2D()

        for rb in self.robots:
            out.names.append(rb.name)
            nr = Room()

            for rbt in self.robots:
                d = self.dist(rb.x, rb.y, rbt.x, rbt.y)
                if (d > 0.0) and (d < self.neighborDistance):
                    nr.names.append(rbt.name)
                    nr.locations.append(Pt(point=(rbt.x, rbt.y)))

            out.neighbors.append(nr)

        return out
Esempio n. 15
0
 def arrayToPts(self, array):
     return [ Pt(point = a) for a in array ]
Esempio n. 16
0
 def outPoints(self):
     """
     Returns the point array as a list of Pt objects.
     """
     return [ Pt(point = row) for row in self.pointArray ]
Esempio n. 17
0
    def doForceMove(self, belief):
        """
        Runs through the points in the input belief and calculates the
        'force' on each one.  Uses the result to come up with a magnitude
        and direction relative to the robot's position, then translates
        this into a Twist message, which is published.
        """
        # Update all the points in the belief with the model position
        # adjustment.  Then calculate the 'force' on each point.
        forceList = []
        for pt in belief.points:
            modelPt = self.modelPosition(pt.point)

            fX, fY = self.fl.sumForces(modelPt)

            # Convert to polar coordinates.
            fM = math.hypot(fX, fY)
            fD = math.atan2(fY, fX)

            # Convert to robot frame
            fDR = fD - modelPt[2]
            if math.fabs(fDR) > math.pi:
                fDR -= math.copysign(2 * math.pi, fDR)

            forceList.append(Pt(point = (fM, fDR)))

        # We have a long collection of forces, one for each point in the
        # belief.  We feed them to the guesser in order to get a decent
        # estimate of the mode and to use the guesser's binning functions
        # in making that estimate.
        self.guesser.newPoints(forceList)
        means = self.guesser.means()
        # This is the mean force direction, more or less.
        mfDirs = means[1]


        binmeans,binwidths = self.guesser.hist.binmeans()
        #print "self.position: ", self.position
        #print "%s, %s, %.3f" % (("(%.2f,%.2f)" % tuple(binmeans)),
        #                        ("(%.2f,%.2f)" % tuple(binwidths)),
        #                        mfDirs)
        #print self.guesser.hist.bins

        # How many bins should we use on the force angle when calculating a
        # mode?  The closer we are to going in the right direction, the tighter
        # the bins should get.  But there aren't enough points for too many
        # bins, so limit the number.
        nAngBins = min(11, 5 + 2 * (int(1.57 / math.fabs(mfDirs))))
        #print "::::",nAngBins, self.guesser.hist.numBins[1]

        # Decide whether or not to regenerate the bin boundaries.
        if (nAngBins != self.guesser.hist.numBins[1]) or \
                (math.fabs(binmeans[1] - mfDirs) > binwidths[1]/2.0):
            self.guesser.hist.rebin(self.guesser.pointArray, (3,nAngBins))
            #print "regenerating (angbins=%d)" % nAngBins
        else:
            self.guesser.hist.populate(self.guesser.pointArray)

        #print self.guesser.hist.str()

        # Choose the mode from the histogram and generate a decent force
        # value from its boundaries.
        fM,fDR = self.guesser.hist.mode()

        # Translate this into a Twist object, applying appropriate damping
        # and maxima.
        fDR = fDR % (2 * math.pi)
        fDR = fDR - (2 * math.pi) if fDR > math.pi else fDR

        print "fDR = ", fDR
        # Calculate some provisional speeds, later to be subject to damping.
        if math.fabs(fDR) > 0.2:
            # If we're pointing the wrong way, just rotate (linear = 0).
            provX = 0.0
        else:
            provX = min(self.maxSpeed, self.kSpeed * fM)

        provZ = math.fabs(self.kTurn * fDR)
        provZ = min(self.maxTurn, provZ)
        provZ = math.copysign(provZ, fDR)

        print "provX, provZ", provX, provZ

        # If the last location estimate had a lower variance than this one, 
        # ignore the new one.
        if provX > 0.0 and \
                (self.curstds < 0.5 * (belief.stds[0] + belief.stds[1])):
            print "out of here..."
            return

        self.curstds = 0.5 * (belief.stds[0] + belief.stds[1])
        
        # Apply damping and put it into the twist message.
        if provX:
            self.twist.linear.x += self.dampSpeed * (provX - self.twist.linear.x)
        else:
            self.twist.linear.x = provX

        self.twist.angular.z += self.dampTurn * (provZ - self.twist.angular.z)

        return