def compute(self, env, robot, landmarks, visualize=False):
""" Returns a LandmarkScan taken from the given environment, robot, and
list of landmarks. """
scan = LandmarkScan()
if visualize:
pyglet.gl.glLineWidth(3)
for landmark in landmarks:
dx = landmark.body.position.x - robot.body.position.x
dy = landmark.body.position.y - robot.body.position.y
distance = sqrt(dx*dx + dy*dy)
angle = normalize_angle_pm_pi(atan2(dy, dx) - robot.body.angle)
if distance <= scan.MAX_RANGE and distance >= scan.MIN_RANGE and \
angle <= scan.MAX_ANGLE and angle >= scan.MIN_ANGLE:
scan.landmarks.append(DetectedLandmark(distance, angle))
if visualize:
x1 = robot.body.position.x
y1 = robot.body.position.y
x2 = landmark.body.position.x
y2 = landmark.body.position.y
pyglet.graphics.draw(2, pyglet.gl.GL_LINES,
('v2f', (x1, y1, x2, y2)),
('c3B', (200, 200, 255, 200, 200, 255)))
if visualize:
pyglet.gl.glLineWidth(1)
return scan
def compute(self, env, this_robot, others, visualize=False):
""" Returns a PuckScan taken from the given environment, this robot,
and list of other robots. """
scan = RobotScan()
if visualize:
pyglet.gl.glLineWidth(4)
for bot in others:
dx = bot.body.position.x - this_robot.body.position.x
dy = bot.body.position.y - this_robot.body.position.y
distance = sqrt(dx * dx + dy * dy)
angle = normalize_angle_pm_pi(
atan2(dy, dx) - this_robot.body.angle)
if distance <= scan.MAX_RANGE and distance >= scan.MIN_RANGE and \
angle <= scan.MAX_ANGLE and angle >= scan.MIN_ANGLE:
scan.robots.append(DetectedRobot(distance, angle))
if visualize:
x1 = this_robot.body.position.x
y1 = this_robot.body.position.y
x2 = bot.body.position.x
y2 = bot.body.position.y
pyglet.graphics.draw(2, pyglet.gl.GL_LINES,
('v2f', (x1, y1, x2, y2)),
('c3B', (0, 255, 255, 0, 255, 255)))
if visualize:
pyglet.gl.glLineWidth(1)
return scan
def compute(self, env, robot, pucks, visualize=False):
""" Returns a PuckScan taken from the given environment, robot, and
list of pucks. """
scan = PuckScan()
if visualize:
pyglet.gl.glLineWidth(3)
for puck in pucks:
dx = puck.body.position.x - robot.body.position.x
dy = puck.body.position.y - robot.body.position.y
distance = sqrt(dx*dx + dy*dy)
angle = normalize_angle_pm_pi(atan2(dy, dx) - robot.body.angle)
if distance <= scan.MAX_RANGE and distance >= scan.MIN_RANGE and \
angle <= scan.MAX_ANGLE and angle >= scan.MIN_ANGLE:
scan.pucks.append(DetectedPuck(distance, angle, puck.kind))
if visualize:
x1 = robot.body.position.x
y1 = robot.body.position.y
x2 = puck.body.position.x
y2 = puck.body.position.y
pyglet.graphics.draw(2, pyglet.gl.GL_LINES,
('v2f', (x1, y1, x2, y2)),
('c3B', (255, 255, 0, 255, 255, 0)))
if visualize:
pyglet.gl.glLineWidth(1)
return scan
def compute(self, env, this_robot, others, visualize=False):
""" Returns a PuckScan taken from the given environment, this robot,
and list of other robots. """
scan = RobotScan()
if visualize:
pyglet.gl.glLineWidth(4)
for bot in others:
dx = bot.body.position.x - this_robot.body.position.x
dy = bot.body.position.y - this_robot.body.position.y
distance = sqrt(dx*dx + dy*dy)
angle = normalize_angle_pm_pi(atan2(dy, dx) - this_robot.body.angle)
if distance <= scan.MAX_RANGE and distance >= scan.MIN_RANGE and \
angle <= scan.MAX_ANGLE and angle >= scan.MIN_ANGLE:
scan.robots.append(DetectedRobot(distance, angle))
if visualize:
x1 = this_robot.body.position.x
y1 = this_robot.body.position.y
x2 = bot.body.position.x
y2 = bot.body.position.y
pyglet.graphics.draw(2, pyglet.gl.GL_LINES,
('v2f', (x1, y1, x2, y2)),
('c3B', (0, 255, 255, 0, 255, 255)))
if visualize:
pyglet.gl.glLineWidth(1)
return scan
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_target(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = (rscan.ANGLE_MIN + target * (rscan.ANGLE_MAX - rscan.ANGLE_MIN) / float(rscan.NUMBER_POINTS))
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2*compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# Check for the presence of another robot in the centre of the image.
react_to_robot = False
react_to_robot_angle = None
for i in range(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi/4 and
rscan.ranges[i] < 2*this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) / float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = - self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle, (0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_target(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = (rscan.ANGLE_MIN + target *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) /
float(rscan.NUMBER_POINTS))
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2 * compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# Check for the presence of another robot in the centre of the image.
react_to_robot = False
react_to_robot_angle = None
for i in range(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi / 4
and rscan.ranges[i] < 2 * this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) /
float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = -self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle,
(0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_leftmost_pixel(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = self.index_to_angle(rscan, target)
# EXPERIMENT TO ACHIEVE A MINIMUM SIZE AGGREGATE.
lscan = sensor_suite.landmark_scan
closest_lmark_distance = float('inf')
for i in range(len(lscan.ranges)):
if (lscan.masks[i] & ANY_LANDMARK_MASK != 0
and lscan.ranges[i] < closest_lmark_distance):
closest_lmark_distance = lscan.ranges[i]
if closest_lmark_distance < self.circle_radius:
target_angle = target_angle + self.egress_angle
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2*compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# Check for the presence of another robot in the centre of the image.
react_to_robot = False
react_to_robot_angle = None
for i in range(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi/4 and
rscan.ranges[i] < 2*this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) / float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = - self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle, (0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
self.summed_angular_speed += twist.angular
#print self.summed_angular_speed
return twist
def outie_react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
scan = sensor_suite.range_scan
n = len(scan.ranges)
centre_angle = 0
lmark_distance, lmark_angle, lmark_mask = \
self.get_lmark_dist_angle_mask(sensor_suite)
# If no landmark is in view, then move towards the
if (lmark_mask == ARC_LANDMARK_MASK
and lmark_distance < self.outside_radius):
return self.home_to_angle(this_robot,
normalize_angle_pm_pi(lmark_angle + pi))
# If there is a blast landmark then clear the area.
if lmark_distance == None or lmark_mask == BLAST_LANDMARK_MASK:
return self.pushout_behaviour(this_robot, scan)
# If the closest landmark is an ARC and we are within the outer radius,
# then we will move away from it.
if (lmark_mask == ARC_LANDMARK_MASK
and lmark_distance < self.outside_radius):
return self.home_to_angle(this_robot,
normalize_angle_pm_pi(lmark_angle + pi))
# If we are within the outer radius, then we will steer away from pucks
# by altering our definition of the image centre.
# if lmark_distance < self.outside_radius:
# centre_angle = 0.3
# Relying on only a single forward-pointing ray causes distant pucks
# to be easily missed (they may be detected, but too sporadically to
# attract the robot). We accept as forward-pointing any sensor ray
# within 'front_angle_threshold' of zero. Correspondingly set the
# two predicates 'react_to_puck' and 'react_to_robot'.
react_to_puck = False
react_to_robot = False
# We will treat POLE landmarks as pucks. See if one is centred.
if lmark_mask == POLE_LANDMARK_MASK:
lscan = sensor_suite.landmark_scan
nl = len(lscan.ranges)
for i in range(nl):
if (fabs(lscan.angles[i] + centre_angle) <
2 * self.front_angle_threshold):
if (lscan.masks[i] & POLE_LANDMARK_MASK) != 0:
react_to_puck = True
for i in range(n):
if fabs(scan.angles[i] +
centre_angle) < self.front_angle_threshold:
if (scan.masks[i] & self.puck_mask) != 0:
react_to_puck = True
if scan.masks[i] == ROBOT_MASK:
react_to_robot = True
if react_to_robot:
react_to_puck = False
# Now react...
if react_to_puck:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
draw_line(this_robot, scan, 0, (255, 0, 255))
elif react_to_robot:
# Turn left and slow
twist.linear = self.linear_speed * self.slow_factor
twist.angular = self.angular_speed
if visualize: # Reacting to robot
draw_line(this_robot, scan, 0, (255, 0, 0))
else:
# Turn right
twist.linear = self.linear_speed
twist.angular = -self.angular_speed
if visualize:
draw_line(this_robot, scan, 0, (255, 0, 255))
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
lscan = sensor_suite.landmark_scan
span_angles = self.get_landmark_span_angles(lscan)
sector_widths = self.get_sector_widths(span_angles)
assert len(span_angles) == len(sector_widths)
# If any sector width is greater than the threshold then we will turn
# away from that sector. Otherwise, we will continue forwards. If
# there is more than one such "bad sector" then we choose the one with
# the smallest width.
n = len(sector_widths)
index_of_bad_sector = None
smallest_bad_width = float('inf')
for i in range(0, n):
if (sector_widths[i] > self.threshold
and sector_widths[i] < smallest_bad_width):
index_of_bad_sector = i
smallest_bad_width = sector_widths[i]
#print index_of_bad_sector
if index_of_bad_sector != None:
lmark_a_angle = self.careful_average_angles(
span_angles[(index_of_bad_sector + 1) % n])
lmark_b_angle = self.careful_average_angles(
span_angles[index_of_bad_sector])
#print "LANDMARK ANGLES (in degrees)"
#print "A (yellow): " + str(degrees(lmark_a_angle))
#print "B (white): " + str(degrees(lmark_b_angle))
# Choose direction to react based on the average of the
# corresponding landmark angles.
react_angle = (lmark_a_angle + get_smallest_angular_difference(
lmark_b_angle, lmark_a_angle) / 2.0)
twist.linear = 10.0
twist.angular = 5 * sign(normalize_angle_pm_pi(react_angle))
self.draw_line(this_robot, lscan, lmark_a_angle, (255, 255, 0))
self.draw_line(this_robot, lscan, lmark_b_angle, (255, 255, 255))
self.draw_line(this_robot, lscan, react_angle, (255, 0, 0))
else:
# Compute centroid of all pucks in the robot's ref. frame
cx = 0
cy = 0
n = len(rscan.ranges)
for i in range(n):
if rscan.masks[i] & self.acceptable_puck_mask != 0:
angle = self.index_to_angle(rscan, i)
if angle >= pi / 2 or angle < -pi / 2:
continue
#value = 1.0/rscan.ranges[i]
#value = 1.0/rscan.ranges[i]
value = 1.0
cx = cx + value * cos(angle)
cy = cy + value * sin(angle)
cx /= n
cy /= n
twist.linear = 10.0
twist.angular = 100.0 * cy
self.draw_line(this_robot, lscan, 0, (0, 255, 0))
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
lscan = sensor_suite.landmark_scan
span_angles = self.get_landmark_span_angles(lscan)
sector_widths = self.get_sector_widths(span_angles)
assert len(span_angles) == len(sector_widths)
# If any sector width is greater than the threshold then we will turn
# away from that sector. Otherwise, we will continue forwards. If
# there is more than one such "bad sector" then we choose the one with
# the smallest width.
n = len(sector_widths)
index_of_bad_sector = None
smallest_bad_width = float('inf')
for i in range(0, n):
if (sector_widths[i] > self.threshold
and sector_widths[i] < smallest_bad_width):
index_of_bad_sector = i
smallest_bad_width = sector_widths[i]
#print index_of_bad_sector
if index_of_bad_sector != None:
lmark_a_angle = self.careful_average_angles(span_angles[(index_of_bad_sector + 1) % n])
lmark_b_angle = self.careful_average_angles(span_angles[index_of_bad_sector])
#print "LANDMARK ANGLES (in degrees)"
#print "A (yellow): " + str(degrees(lmark_a_angle))
#print "B (white): " + str(degrees(lmark_b_angle))
# Choose direction to react based on the average of the
# corresponding landmark angles.
react_angle = (lmark_a_angle +
get_smallest_angular_difference(lmark_b_angle,
lmark_a_angle) / 2.0)
twist.linear = 10.0
twist.angular = 5 * sign(normalize_angle_pm_pi(react_angle))
self.draw_line(this_robot, lscan, lmark_a_angle, (255, 255, 0))
self.draw_line(this_robot, lscan, lmark_b_angle, (255, 255, 255))
self.draw_line(this_robot, lscan, react_angle, (255, 0, 0))
else:
# Compute centroid of all pucks in the robot's ref. frame
cx = 0
cy = 0
n = len(rscan.ranges)
for i in range(n):
if rscan.masks[i] & self.acceptable_puck_mask != 0:
angle = self.index_to_angle(rscan, i)
if angle >= pi/2 or angle < -pi/2:
continue
#value = 1.0/rscan.ranges[i]
#value = 1.0/rscan.ranges[i]
value = 1.0
cx = cx + value * cos(angle)
cy = cy + value * sin(angle)
cx /= n
cy /= n
twist.linear = 10.0
twist.angular = 100.0 * cy
self.draw_line(this_robot, lscan, 0, (0, 255, 0))
return twist
def outie_react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
scan = sensor_suite.range_scan
n = len(scan.ranges)
centre_angle = 0
lmark_distance, lmark_angle, lmark_mask = \
self.get_lmark_dist_angle_mask(sensor_suite)
# If no landmark is in view, then move towards the
if (lmark_mask == ARC_LANDMARK_MASK
and lmark_distance < self.outside_radius):
return self.home_to_angle(this_robot,
normalize_angle_pm_pi(lmark_angle + pi))
# If there is a blast landmark then clear the area.
if lmark_distance == None or lmark_mask == BLAST_LANDMARK_MASK:
return self.pushout_behaviour(this_robot, scan)
# If the closest landmark is an ARC and we are within the outer radius,
# then we will move away from it.
if (lmark_mask == ARC_LANDMARK_MASK
and lmark_distance < self.outside_radius):
return self.home_to_angle(this_robot,
normalize_angle_pm_pi(lmark_angle + pi))
# If we are within the outer radius, then we will steer away from pucks
# by altering our definition of the image centre.
# if lmark_distance < self.outside_radius:
# centre_angle = 0.3
# Relying on only a single forward-pointing ray causes distant pucks
# to be easily missed (they may be detected, but too sporadically to
# attract the robot). We accept as forward-pointing any sensor ray
# within 'front_angle_threshold' of zero. Correspondingly set the
# two predicates 'react_to_puck' and 'react_to_robot'.
react_to_puck = False
react_to_robot = False
# We will treat POLE landmarks as pucks. See if one is centred.
if lmark_mask == POLE_LANDMARK_MASK:
lscan = sensor_suite.landmark_scan
nl = len(lscan.ranges)
for i in range(nl):
if (fabs(lscan.angles[i] + centre_angle) <
2*self.front_angle_threshold):
if (lscan.masks[i] & POLE_LANDMARK_MASK) != 0:
react_to_puck = True
for i in range(n):
if fabs(scan.angles[i] + centre_angle) <self.front_angle_threshold:
if (scan.masks[i] & self.puck_mask) != 0:
react_to_puck = True
if scan.masks[i] == ROBOT_MASK:
react_to_robot = True
if react_to_robot:
react_to_puck = False
# Now react...
if react_to_puck:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
draw_line(this_robot, scan, 0, (255, 0, 255))
elif react_to_robot:
# Turn left and slow
twist.linear = self.linear_speed * self.slow_factor
twist.angular = self.angular_speed
if visualize: # Reacting to robot
draw_line(this_robot, scan, 0, (255, 0, 0))
else:
# Turn right
twist.linear = self.linear_speed
twist.angular = -self.angular_speed
if visualize:
draw_line(this_robot, scan, 0, (255, 0, 255))
return twist
def react(self, this_robot, sensor_suite, visualize=False):
twist = Twist()
rscan = sensor_suite.range_scan
n = len(rscan.ranges)
target = self.get_leftmost_pixel(rscan)
# Convert to an angle
if target == None:
target_angle = None
else:
target_angle = self.index_to_angle(rscan, target)
# EXPERIMENT TO ACHIEVE A MINIMUM SIZE AGGREGATE.
lscan = sensor_suite.landmark_scan
closest_lmark_distance = float('inf')
for i in range(len(lscan.ranges)):
if (lscan.masks[i] & ANY_LANDMARK_MASK != 0
and lscan.ranges[i] < closest_lmark_distance):
closest_lmark_distance = lscan.ranges[i]
if closest_lmark_distance < self.circle_radius:
target_angle = target_angle + self.egress_angle
if self.modulate and target_angle != None:
compass_angle = normalize_angle_pm_pi(this_robot.body.angle)
if cos(2 * compass_angle) < 0:
# Dig in to push the puck more
target_angle = target_angle - self.ingress_angle
else:
# Pull out to skirt the puck
target_angle = target_angle + self.egress_angle
# Check for the presence of another robot in the centre of the image.
react_to_robot = False
react_to_robot_angle = None
for i in range(0, n):
if (rscan.masks[i] == ROBOT_MASK and fabs(rscan.angles[i]) < pi / 4
and rscan.ranges[i] < 2 * this_robot.radius):
react_to_robot = True
react_to_robot_angle = (rscan.ANGLE_MIN + i *
(rscan.ANGLE_MAX - rscan.ANGLE_MIN) /
float(rscan.NUMBER_POINTS))
if react_to_robot:
# Turn left and slow
twist.linear = self.slow_factor * self.linear_speed
twist.angular = self.angular_speed
if visualize: # Reacting to robot
self.draw_line(this_robot, rscan, react_to_robot_angle,
(255, 0, 0))
elif (target_angle == None or target_angle <= 0):
# Turn right
twist.linear = self.linear_speed
twist.angular = -self.angular_speed
if visualize:
if target_angle != None:
self.draw_line(this_robot, rscan, target_angle,
(0, 255, 0))
else:
# Turn left
twist.linear = self.linear_speed
twist.angular = self.angular_speed
if visualize:
self.draw_line(this_robot, rscan, target_angle, (255, 0, 255))
self.summed_angular_speed += twist.angular
#print self.summed_angular_speed
return twist