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 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():
global claw_offset_distance
print("Missed, trying to recover.")
try:
swarmie.drive(-0.15,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20)
# Wait a moment to detect tags before possible backing up further
rospy.sleep(0.25)
block = swarmie.get_nearest_block_location(targets_buffer_age=1.0)
if block is not None:
pass
else:
swarmie.drive(-0.15,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20)
except (AbortException, InsideHomeException):
raise
except DriveException as e:
print("Oh no, we have an exception!", e)
pass
def approach(save_loc=False):
global claw_offset_distance
print("Attempting a pickup.")
setup_approach(save_loc)
block = swarmie.get_nearest_block_location(targets_buffer_age=0.5)
if block is not None:
# claw_offset should be a positive distance of how short drive_to needs to be.
swarmie.drive_to(block,
claw_offset=claw_offset_distance,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20,
**swarmie.speed_slow)
# Grab - minimal pickup with sim_check.
if swarmie.simulator_running():
finger_close_angle = 0
else:
finger_close_angle = 0.2
swarmie.set_finger_angle(finger_close_angle) #close
rospy.sleep(1)
swarmie.wrist_up()
rospy.sleep(.5)
# did we succesuflly grab a block?
if swarmie.has_block():
swarmie.wrist_middle()
swarmie.drive(-0.3,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20)
return True
else:
swarmie.set_wrist_angle(0.55)
rospy.sleep(1)
swarmie.fingers_open()
# Wait a moment for a block to fall out of claw
rospy.sleep(0.25)
else:
print("No legal blocks detected.")
swarmie.wrist_up()
sys.exit(1)
# otherwise reset claw and return Falase
swarmie.wrist_up()
return False
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 wander():
try:
rospy.loginfo("Wandering...")
swarmie.drive(random.gauss(2.5, 1), **swarmie.speed_fast)
prev_heading = swarmie.get_odom_location().get_pose().theta
rospy.loginfo("Circling...")
swarmie.circle()
swarmie.set_heading(prev_heading + random.gauss(0, math.pi / 6),
**swarmie.speed_fast)
except HomeException:
print("I saw home!")
# TODO: We used to set the home odom location here, while we had
# the chance. If finding home during gohome becomes difficult,
# it may be useful to have home_transform publish a less
# accurate, but easier to update, home position estimate.
turnaround()
except ObstacleException:
print("I saw an obstacle!")
turnaround(ignore=Obstacle.IS_SONAR | Obstacle.VISION_HOME)
def abs_square(distance):
start_pose = swarmie.get_odom_location().get_pose()
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.set_heading(start_pose.theta + math.pi / 2, ignore=-1)
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.set_heading(start_pose.theta + math.pi, ignore=-1)
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.set_heading(start_pose.theta + (3 * math.pi) / 2, ignore=-1)
swarmie.drive(distance, ignore=Obstacle.IS_VISION)
swarmie.set_heading(start_pose.theta, ignore=-1)
def main(**kwargs):
global claw_offset_distance
claw_offset_distance = 0.23
if swarmie.simulator_running():
claw_offset_distance = 0.16
for i in range(3):
try:
if approach(save_loc=bool(i == 0)):
print("Got it!")
sys.exit(0)
recover()
except TimeoutException:
rospy.logwarn(
('Timeout exception during pickup. This rover may be ' +
'physically deadlocked with an obstacle or another rover.'))
swarmie.drive(random.uniform(-0.1, -0.2),
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20)
print("Giving up after too many attempts.")
return 1
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 approach(save_loc=False):
global claw_offset_distance
print("Attempting a pickup.")
setup_approach(save_loc)
block = swarmie.get_nearest_block_location(targets_buffer_age=0.5)
if block is not None:
# claw_offset should be a positive distance of how short drive_to needs to be.
swarmie.drive_to(block,
claw_offset=claw_offset_distance,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20,
**swarmie.speed_slow)
swarmie.drive(-0.3,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20)
return True
else:
print("No plant detected.")
sys.exit(1)
return False
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)
