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
def __init__(self, name, pos=(0.0, 0.0, 0.0)): self.name = name # Use this to substitute for measuring the neighbor's position. self.lsub = rospy.Subscriber("/" + self.name + "/guesser/position/guess", Belief, self.getPos, queue_size = 1, buff_size = 200) # Use this to hold a measurement of the neighboring robot's # position. self.location = Belief(data_type = "location", data_sub_type = self.name, pers = (0.0, 0.0, 2*math.pi)) # Use this to hold the last question sent to the neighboring # robot. This 'question' would have been a suggestion about # appropriate directions for that neighboring robot to move # in. self.lastQuestion = Belief(data_type = "destination", data_sub_type = self.name, pers = (0.0, 0.0, 2*math.pi)) # This is the reply to the question stored above, a collection # of ratings of each of the points in the question above. # Each point is a floating-point value x: 0 < x < 1. self.lastReply = Belief(data_type = "rating", data_sub_type = "from " + self.name, pers = (0.0, 0.0, 2*math.pi)) # This is the service by which we send the lastQuestion and # receive the lastReply. Note the absolute service name. self.advise = rospy.ServiceProxy("/" + self.name + "/cover/advice", MRFQuery, persistent=True)
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
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
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
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
def __init__(self, name, pos=(0.0, 0.0)): self.name = name # This is a tentative position, used in the demo to figure out # which robots we can simulate "seeing." self.pos = pos self.lsub = rospy.Subscriber(self.name + "/position", Belief, self.getPos, queue_size=1, buff_size=200) # Use this to hold our measurement of the neighboring robot's # position. self.location = Belief(data_type="location", data_sub_type=self.name, pers=(0.0, 0.0)) # Use this to hold the last question sent to the neighboring # robot. This 'question' would have been a suggestion about # appropriate targets for that neighboring robot to move # towards. self.lastQuestion = Belief(data_type="destination", data_sub_type=self.name, pers=(0.0, 0.0)) # This is the reply to the question stored above, a collection # of ratings of each of the points in the question above. # Each point is a floating-point value x: 0 < x < 1. self.lastReply = Belief(data_type="rating", data_sub_type="from " + self.name, pers=(0.0, 0.0)) # This is the service by which we send the lastQuestion and # receive the lastReply. self.advise = rospy.ServiceProxy(self.name + "/advice", MRFQuery) # This is the latest suggestion sent to us by this neighbor. self.lastSuggestion = Belief(data_type="suggestion", data_sub_type=self.name, pers=(0.0, 0.0)) # The reply we made to the suggestion above. self.rating = Belief(data_type="rating", data_sub_type="from host", pers=(0.0, 0.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 ] )
def belief(self): """ Returns the belief embodied by the current state of the Guesser. """ return Belief( source_stamp = self.stamp, source_data = self.source_data, data_type = self.data_type, data_sub_type = self.data_sub_type, sender = self.name, means = self.means(), stds = self.stds(), pers = self.periods, dim_names = self.dim_names, points = self.outPoints())
def __init__(self, name="bump_mapper", pub=False, radius=0.25): self.name = name self.bpub = pub self.robotRadius = radius self.waitTime = 0.0 self.curLocation = Belief() self.curVelocity = Twist() self.velTimestamp = rospy.Time.now().to_sec() self.guesser = Guesser(name=self.name, periods=(0.0, 0.0, 2 * math.pi), data_type="location", num_bins=(1, 1, 1), data_sub_type="obstacle")
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
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)
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
def __init__(self): self.output = Belief()
# experiments, in the coordinate system we've defined for the # entire floor of the building. guesser.uniform(npoints, [[28.0, 35.0], [35.0, 40.0], [-math.pi, math.pi]]) # If you're doing simulation experiments, or if there is some # other reason you want to give the guesser a head start, you can # use a normal distribution instead. # guesser.normal([[0.0,0.01], [0.0, 0.01], [0.0, 0.01]]) server = PerfesserServer(rospy.ServiceProxy, debug=True) 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"
def __init__(self, name, npoints = 7, away = 1.3, space = 0.6, avoid = 0.5, seek = 4.0, maxDist = 2.0, velPub = False, goalPub = False): print "Starting:" , name self.name = name self.pos = Belief(data_type = "position", data_sub_type = self.name, pers = (0.0, 0.0, 2*math.pi)) self.npoints = npoints # These are coefficients for the opinion formation. # Away controls the desire to move apart. self.away = away # Without this space term, you get an exploding ring more often with # nothing in the middle. self.space = space # Controls the predilection to avoid previously-encountered obstacles. self.avoid = avoid # Controls interest in new territory. self.seek = seek # Poorly named -- the potential function in the phi.away() method # attempts to put this distance between each node. self.maxDist = maxDist # This is a list of neighboring robots, assumed to contain # Neighbor() objects, as defined above. self.neighbors = [] # This is an array of points in configuration space indicating # where we need to go next. self.deltaConfig = np.array([[]]) self.twist = Twist() #** These are moved to gnav self.maxSpeed = 0.25 self.maxTurn = 0.35 self.kSpeed = 1.0 self.kTurn = 1.0 self.phi = Phi(self.name, self.away, self.space, self.avoid, self.seek, self.maxDist) self.movePeriod = 3.0 self.nextMoveTime = rospy.Time.now().to_sec() + self.movePeriod # When this is in the future, we are still dealing with a bump. Don't # register other obstacles within a couple of seconds. self.bumpFreeTime = 0.0 # Estimated variance in the position measurements. This is important # in the resampling of the guesses. self.std = 0.1 # When rating a neighbor's opinion about our desired position, # use this many bins. Shouldn't be a radically different # order of magnitude from npoints. self.nbins = (5,5) self.velPub = velPub self.goalPub = goalPub try: # assert velPub assert goalPub except: print "must specify a goal publisher" self.busy = False
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