def __init__(self, name, neighborDist=2.0, npoints=70):
print "Starting.", name
self.name = name
self.npoints = npoints
self.neighborDist = neighborDist
# This is a list of the robots on our team, assumed to contain
# Neighbor() objects, as above.
self.neighbors = []
# This is a list of the robots within neighborDist of us.
self.neighborNames = []
# This is an array of points in configuration space indicating
# where we need to go next.
self.deltaConfig = np.array([[]])
self.twist = Twist()
self.maxSpeed = 0.5 # 0.25
self.maxTurn = 1.0 # .6
self.kSpeed = 15.0
self.kTurn = 13.0
# These are coefficients for the opinion formation. Tune and forget.
self.aw = 0.5
self.sp = 0.2 # Without this term, you get an exploding ring more often.
self.av = 0.6
self.se = 0.0
self.maxDist = 2.5
self.phi = Phi(self.name, self.aw, self.sp, self.av, self.se,
self.maxDist)
self.movePeriod = 5.0
self.nextMoveTime = rospy.Time.now().to_sec() + self.movePeriod
# Estimated variance in the position measurements.
self.std = 0.01
# 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)
rospy.sleep(1.0)
self.odomSub = rospy.Subscriber(self.name + "/odom",
Odometry,
self.trackOdom,
queue_size=1,
buff_size=250)
self.advice = rospy.Service(self.name + "/advice", MRFQuery,
self.takeAdvice)
self.busy = False
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 __init__(self, name, initpos=(0.0, 0.0, 0.0), npoints=10):
print "Starting.", name
self.name = name
self.pos = initpos
self.npoints = npoints
# This is a list of neighboring robots, assumed to contain
# Neighbor() objects, as above.
self.neighbors = []
self.velPub = rospy.Publisher(self.name + "/cmd_vel", Twist)
self.posPub = rospy.Publisher(self.name + "/position", Belief)
# This is an array of points in configuration space indicating
# where we need to go next.
self.deltaConfig = np.array([[]])
self.twist = Twist()
self.maxSpeed = 0.5 # 0.25
self.maxTurn = 1.0 # .6
self.kSpeed = 15.0
self.kTurn = 13.0
# These are coefficients for the opinion formation.
self.aw = 13.
self.sp = 0.1 # Without this term, you get an exploding ring more often.
self.av = 3.
self.se = 250.0
self.maxDist = 2.5
self.phi = Phi(self.name, self.aw, self.sp, self.av, self.se,
self.maxDist)
self.movePeriod = 5.0
self.nextMoveTime = rospy.Time.now().to_sec() + self.movePeriod
# When this is in the future, we are still dealing with a bump.
self.bumpFreeTime = 0.0
# Estimated variance in the position measurements.
self.std = 0.01
# 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)
rospy.sleep(1.0)
self.sensorSub = rospy.Subscriber(self.name + "/sensors", SensorPacket,
self.trackSensors)
self.odomSub = rospy.Subscriber(self.name + "/odom",
Odometry,
self.trackOdom,
queue_size=1,
buff_size=250)
self.advice = rospy.Service(self.name + "/advice", MRFQuery,
self.takeAdvice)
self.roomLoc = rospy.Subscriber("/room/locations", Room2D,
self.recordNeighbors)
self.busy = False
