def exit_home():
"""After we drop of a cube, back up out of home and turn away from home."""
# TODO: Is this simple function call reliable enough during congested rounds?
# it's very bad if the rover don't make it fully back out of the home ring.
swarmie.drive(-.5, ignore=Obstacle.IS_VISION | Obstacle.IS_SONAR)
swarmie.turn(-8 * math.pi / 9,
ignore=Obstacle.IS_VISION | Obstacle.IS_SONAR)
def recover():
"""Recover from difficult situations:
- No home tags are in view anymore
- The tag to drive to is too close (we might be inside of home)
Raises:
InsideHomeException: if we're stuck inside of home.
"""
# TODO: should sonar be ignored when recovering?
# TODO: is simply backing up a little bit a reliable recovery move?
ignore = (Obstacle.TAG_TARGET | Obstacle.TAG_HOME |
Obstacle.INSIDE_HOME | Obstacle.IS_SONAR)
check_inside_home()
if swarmie.has_home_odom_location():
home_odom = swarmie.get_home_odom_location()
loc = swarmie.get_odom_location().get_pose()
angle = angles.shortest_angular_distance(loc.theta,
math.atan2(home_odom.y - loc.y,
home_odom.x - loc.x))
# If the rover is facing away from home's center, turn toward it
if abs(angle) > math.pi / 3:
swarmie.turn(angle, ignore=ignore)
return
swarmie.drive(-.1, ignore=ignore)
def main():
rospy.sleep(5)
swarmie.pump_backward(True)
rospy.sleep(5)
swarmie.pump_backward(False)
prev_heading = swarmie.get_odom_location().get_pose().theta
for _ in range(0, 3):
swarmie.drive(-.4, ignore=Obstacle.IS_SONAR)
swarmie.turn(-math.pi / 2, ignore=Obstacle.IS_SONAR)
swarmie.drive(.2, ignore=Obstacle.IS_SONAR)
swarmie.set_heading(prev_heading, ignore=Obstacle.IS_SONAR)
swarmie.drive(.4, ignore=Obstacle.IS_SONAR)
swarmie.pump_forward(True)
rospy.sleep(5)
swarmie.pump_forward(False)
swarmie.drive(-.4, ignore=Obstacle.IS_SONAR)
def dumb_square(distance):
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.turn(math.pi / 2, ignore=Obstacle.IS_VISION)
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.turn(math.pi / 2, ignore=Obstacle.IS_VISION)
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.turn(math.pi / 2, ignore=Obstacle.IS_VISION)
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.turn(math.pi / 2, ignore=Obstacle.IS_VISION)
def main(**kwargs):
remove_from_queue = rospy.ServiceProxy('start_queue/remove', QueueRemove)
# During a normal startup the rover will be facing the center and close to
# the nest. But there's no guarantee where we will be if mobility crashes
# and is forced to restart. This checks to see if we know home's location.
if swarmie.has_home_odom_location():
rospy.logwarn("Init started, but home's location is already known. " +
"Returning normally.")
return 0
# Assume the starting position is facing the center.
# This should be valid by contest rules.
#
# Drive closer until we can see the center.
try:
# Ignore cubes if they're put in the way. It's more important to continue
# this behavior and find a corner of home than it is to stop for a cube.
swarmie.drive(1, ignore=Obstacle.TAG_TARGET | Obstacle.IS_SONAR)
except AbortException:
raise
except DriveException:
# This could happen if we bump into another rover.
# Let's just call it good.
pass
try:
find_home_corner()
except PathException as e:
# It isn't ideal if we can't find a home corner, but it's worth
# continuing to turn around and let the rover begin searching.
swarmie.print_infoLog('<font color="red">{}</font>'.format(e.status))
rospy.logwarn(e.status)
swarmie.turn(-2 * math.pi / 3,
ignore=Obstacle.IS_VISION | Obstacle.IS_SONAR)
remove_from_queue(
QueueRemoveRequest(rover_name=swarmie.rover_name, notify_others=True))
return 0
def turnaround(ignore=Obstacle.IS_SONAR | Obstacle.VISION_SAFE):
swarmie.turn(random.gauss(math.pi / 2, math.pi / 4),
ignore=ignore,
**swarmie.speed_fast)
def run(self):
status_msgs = []
try:
while True:
print(textwrap.dedent(TeleopSwarmie.msg))
print(self.obstacle_msg())
print(self.params_msg())
for status in status_msgs:
print(status)
status_msgs = []
key = self.get_key()
try:
if key in self.drive_bindings.keys():
dist = self.params['drive_dist']
if self.drive_bindings[key] < 0:
dist = -dist
swarmie.drive(dist, ignore=self.ignore_obst)
elif key in self.turn_bindings.keys():
theta = self.params['turn_theta']
if self.turn_bindings[key] < 0:
theta = -theta
swarmie.turn(theta, ignore=self.ignore_obst)
elif key in self.claw_bindings.keys():
self.claw_bindings[key]()
elif key in self.param_bindings.keys():
self.params[self.param_bindings[key][0]]\
*= self.param_bindings[key][1]
if self.param_bindings[key][0] == 'drive_speed':
self.param_client.update_configuration(
{'DRIVE_SPEED': self.params['drive_speed']})
elif self.param_bindings[key][0] == 'reverse_speed':
self.param_client.update_configuration({
'REVERSE_SPEED':
self.params['reverse_speed']
})
elif self.param_bindings[key][0] == 'turn_speed':
self.param_client.update_configuration(
{'TURN_SPEED': self.params['turn_speed']})
# Update params once now to make sure no params were
# set to invalid values.
server_config = self.param_client.get_configuration()
self.update_params(server_config)
elif key in self.obst_bindings.keys():
if self.obst_bindings[key][1] == 0:
self.ignore_obst = 0
else:
if (self.obst_bindings[key][1] & self.ignore_obst
== self.obst_bindings[key][1]):
self.ignore_obst ^= self.obst_bindings[key][1]
else:
self.ignore_obst |= self.obst_bindings[key][1]
else:
if (key == '\x03'):
break
except TagException as e:
status_msgs.append('\033[91m*****I saw a tag!*****\033[0m')
except HomeException as e:
status_msgs.append('\033[91m*****I saw Home!*****\033[0m')
except HomeCornerException as e:
status_msgs.append(
'\033[91m*****I saw a corner of Home!*****\033[0m')
except InsideHomeException as e:
status_msgs.append(
'\033[91m*****I might be inside of Home!*****\033[0m')
except ObstacleException as e:
status_msgs.append(
"\033[91m*****There's an obstacle in front of " +
"me*****\033[0m")
except (PathException, AbortException) as e:
status_msgs.append('\033[91m*****Exception:*****\033[0m')
for exception in traceback.format_exception_only(
type(e), e):
status_msgs.append(exception)
except Exception as e:
print('Something went wrong:')
for exception in traceback.format_exception_only(type(e), e):
print(exception)
traceback.print_exc()
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, self.settings)
def escape_home(detections):
rospy.logwarn('Getting out of the home ring!!')
angle, dist = get_angle_and_dist_to_escape_home(detections)
swarmie.turn(angle, ignore=Obstacle.IS_SONAR | Obstacle.IS_VISION)
swarmie.drive(dist, ignore=Obstacle.IS_SONAR | Obstacle.IS_VISION)