def smart_square(distance, ignore_sonar=False):
ignore = Obstacle.IS_VISION
if ignore_sonar:
ignore |= Obstacle.IS_SONAR
# Compute a square based on the current heading.
start_pose = swarmie.get_odom_location().get_pose()
start = Point()
start.x = start_pose.x
start.y = start_pose.y
print('Start point: ({:.2f}, {:.2f})'.format(start.x, start.y))
sq1 = Point()
sq1.x = start.x + distance * math.cos(start_pose.theta)
sq1.y = start.y + distance * math.sin(start_pose.theta)
sq2 = Point()
sq2.x = sq1.x + distance * math.cos(start_pose.theta + math.pi / 2)
sq2.y = sq1.y + distance * math.sin(start_pose.theta + math.pi / 2)
sq3 = Point()
sq3.x = sq2.x + distance * math.cos(start_pose.theta + math.pi)
sq3.y = sq2.y + distance * math.sin(start_pose.theta + math.pi)
swarmie.drive_to(sq1, ignore=ignore)
swarmie.drive_to(sq2, ignore=ignore)
swarmie.drive_to(sq3, ignore=ignore)
swarmie.drive_to(start, ignore=ignore)
swarmie.set_heading(start_pose.theta)
end_pose = swarmie.get_odom_location().get_pose()
print('Start point: ({:.2f}, {:.2f})'.format(end_pose.x, end_pose.y))
def main(**kwargs):
global planner, found_tag
found_tag = False
planner = Planner()
swarmie.fingers_open()
swarmie.wrist_middle()
locations_gone_to = 0
# Return to the best pile if possible, if nothing found there go to the
#next 2 piles
while swarmie.has_resource_pile_locations() and locations_gone_to < 3:
locations_gone_to += 1
cur_pose = swarmie.get_odom_location().get_pose()
cube_location = swarmie.get_best_resource_pile_location()
dist = math.sqrt((cube_location.x - cur_pose.x)**2 +
(cube_location.y - cur_pose.y)**2)
if dist > .5: # only bother if it was reasonably far away
return_to_last_exit_position(cube_location)
#if we are here then no cubes where found near the location
swarmie.remove_resource_pile_location(cube_location)
else: # must be right next to it
return_to_last_exit_position(cube_location, skip_drive_to=True)
swarmie.remove_resource_pile_location(cube_location)
random_walk(num_moves=30)
print("I'm homesick!")
search_exit(1)
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 setup_approach(save_loc=False):
"""Drive a little closer to the nearest block if it's far enough away."""
global claw_offset_distance
if swarmie.simulator_running():
extra_offset = 0.20
else:
extra_offset = 0.15
swarmie.fingers_open()
swarmie.set_wrist_angle(1.15)
rospy.sleep(1)
block = swarmie.get_nearest_block_location(targets_buffer_age=5.0)
if block is not None:
print("Making the setup approach.")
cur_loc = swarmie.get_odom_location().get_pose()
dist = math.hypot(cur_loc.x - block.x, cur_loc.y - block.y)
if dist > (claw_offset_distance + extra_offset):
swarmie.drive_to(block,
claw_offset=claw_offset_distance + extra_offset,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20,
**swarmie.speed_slow)
if save_loc:
swarmie.print_infoLog('Setting resource pile location.')
swarmie.add_resource_pile_location()
def main(**kwargs):
global planner, use_waypoints
has_block = False
if 'has_block' in kwargs :
has_block = kwargs['has_block']
# Whether to use waypoints from searching the map. Can be set to False if
# the map service fails.
use_waypoints = True
if not has_block:
swarmie.print_infoLog("I don't have a block. Not avoiding targets.")
planner = Planner()
swarmie.fingers_close() # make sure we keep a firm grip
swarmie.wrist_middle() # get block mostly out of camera view
home = get_best_home_location()
drive_result = drive_home(has_block, home)
if drive_result == MoveResult.OBSTACLE_HOME:
sys.exit(0)
elif drive_result == MoveResult.OBSTACLE_TAG:
sys.exit(GOHOME_FOUND_TAG)
# Look to the right and left before starting spiral search, which goes
# left:
ignore = Obstacle.PATH_IS_CLEAR
if has_block:
ignore |= Obstacle.TAG_TARGET
try:
cur_loc = swarmie.get_odom_location()
if not cur_loc.at_goal(home, 0.3):
print('Getting a little closer to home position.')
swarmie.drive_to(home, ignore=ignore, **swarmie.speed_fast)
planner.clear(2 * math.pi / 5, ignore=ignore, throw=True)
except HomeException:
face_home_tag()
sys.exit(0)
except TagException:
sys.exit(GOHOME_FOUND_TAG)
except ObstacleException:
pass # do spiral search
swarmie.print_infoLog(swarmie.rover_name + " Starting spiral search")
print('Starting spiral search with location')
try:
drive_result = spiral_search(has_block)
if drive_result == MoveResult.OBSTACLE_HOME:
sys.exit(0)
elif drive_result == MoveResult.OBSTACLE_TAG:
sys.exit(GOHOME_FOUND_TAG)
except PathException:
sys.exit(GOHOME_FAIL)
# didn't find anything
return GOHOME_FAIL
def drive_to(pose, use_waypoints):
planner.drive_to(pose,
tolerance=0.5,
tolerance_step=0.5,
avoid_targets=False,
avoid_home=True,
use_waypoints=use_waypoints,
**swarmie.speed_fast)
cur_loc = swarmie.get_odom_location()
if not cur_loc.at_goal(pose, 0.3):
print('Getting a little closer to last exit position.')
swarmie.drive_to(pose, throw=False)
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():
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 loc_in_home_frame(transform_timeout=1.0):
# type: (float) -> PoseStamped
"""Return the rover's current location in the home coordinate frame."""
cur_odom = swarmie.get_odom_location().Odometry
cur_loc = PoseStamped()
cur_loc.header = cur_odom.header
cur_loc.pose = cur_odom.pose.pose
swarmie.xform.waitForTransform(
'home',
cur_loc.header.frame_id,
cur_loc.header.stamp,
rospy.Duration(transform_timeout)
)
return swarmie.xform.transformPose('home', cur_loc)
def setup_approach():
"""Drive a little closer to the nearest tag if it's far enough away."""
global claw_offset_distance
if swarmie.simulator_running():
extra_offset = 0.20
else:
extra_offset = 0.15
block = swarmie.get_nearest_block_location(targets_buffer_age=5.0)
if block is not None:
print("Making the setup approach.")
cur_loc = swarmie.get_odom_location().get_pose()
dist = math.hypot(cur_loc.x - block.x, cur_loc.y - block.y)
if dist > (claw_offset_distance + extra_offset):
swarmie.drive_to(block,
claw_offset=claw_offset_distance + extra_offset,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=20,
**swarmie.speed_slow)
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 plant_walk(num_moves):
"""Do a plant walk `num_moves` times."""
for move in range(num_moves):
if rospy.is_shutdown():
water_plants_exit(-1)
rospy.sleep(1)
# get thirstiest plant also weigh in distance
plant_ids = [
plant_id for plant_id in range(len(swarmie.plants))
if swarmie.plants[plant_id]['pot_imp'] > moisture_msg.DRY_SOIL
or swarmie.plants[plant_id]['plant_imp'] > moisture_msg.DRY_PLANT
]
while not plant_ids:
rospy.loginfo("Waiting for plants to water")
rospy.sleep(5)
plant_ids = [
plant_id for plant_id in range(len(swarmie.plants)) if
swarmie.plants[plant_id]['pot_imp'] > moisture_msg.DRY_SOIL or
swarmie.plants[plant_id]['plant_imp'] > moisture_msg.DRY_PLANT
]
cur_pose = swarmie.get_odom_location().get_pose()
# get the closest plant
plant_id = min(plant_ids,
key=lambda k: math.sqrt(
(swarmie.plants[k]['point'].x - cur_pose.x)**2 +
(swarmie.plants[k]['point'].y - cur_pose.y)**2))
try:
drive_to_plant(plant_id)
rospy.loginfo("Watering plant #" + str(plant_id))
rospy.sleep(2)
swarmie.plants[plant_id]['pot_imp'] = 10
swarmie.plants[plant_id]['plant_imp'] = 10
swarmie.delete_light("plant_light_" + str(plant_id))
swarmie.plants[plant_id]['light'] = False
except rospy.ServiceException, e:
rospy.loginfo("plant_walk: ServiceException%s" % e)
except PathException:
rospy.loginfo("plant_walk: PathException")
def should_sweep(home_tag_yaw):
"""Given a rightmost home tag's orientation in the odometry frame, decide
whether the rover should turn right to sweep for more tags.
"""
rover_yaw = swarmie.get_odom_location().get_pose().theta
if swarmie.has_home_odom_location():
try:
loc_home = loc_in_home_frame()
except tf.Exception as e:
rospy.logwarn('Transform exception in should_sweep(): {}'.format(e))
loc_home = PoseStamped()
loc_home.pose.position.x = 0.0
loc_home.pose.position.y = 0.0
else:
loc_home = PoseStamped()
loc_home.pose.position.x = 0.0
loc_home.pose.position.y = 0.0
return (abs(angles.shortest_angular_distance(rover_yaw, home_tag_yaw))
> math.pi / 2
and (abs(loc_home.pose.position.x) < 0.6
or abs(loc_home.pose.position.y) < 0.6))
def drive_to_next_tag(rightmost_tag, prev_rightmost_tag):
"""Attempt to drive to the next rightmost home tag in view.
Args:
rightmost_tag: The current rightmost tag in view.
prev_rightmost_tag: The previous rightmost tag in view. Used as a
fallback if no home tags are currently in view.
Returns:
True if successful, False if we had to recover.
Raises:
HomeCornerException: From Swarmie.drive(), if we find a corner of home.
"""
# The rover drives repeatedly to the rightmost home tag in view. However,
# swarmie.drive_to() drives to a point such that the rover is on top of that
# point. If the rover did this here, it's likely no home tags would be in
# view anymore. The claw_offset argument allows the rover to stop short of
# the goal point by this distance in meters.
claw_offset = 0.3
# TODO: should sonar be ignored all the time?
ignore = (Obstacle.TAG_TARGET | Obstacle.TAG_HOME |
Obstacle.INSIDE_HOME | Obstacle.IS_SONAR)
if rightmost_tag is None:
if prev_rightmost_tag is not None:
rightmost_tag = prev_rightmost_tag
else:
return False
# TODO: pull helpers out of home_transform.py and into utils.py
# so you can use yaw_from_quaternion() here.
_r, _p, home_yaw = euler_from_quaternion(rightmost_tag.pose.orientation)
# Using set heading before driving is useful when the rover is
# mostly facing the corner to its left (which isn't the one we
# want to drive to). Given the home tag's orientation on the
# home ring, this gets the rover facing to the right, and keeps
# it that way. Otherwise, it's possible for the rover to be
# facing a corner to its left, and the simple "drive to the
# rightmost tag" logic here will fail; the rover won't move at
# all because its already where it should be according to
# drive_to().
if should_sweep(home_yaw):
swarmie.set_heading(home_yaw + math.pi / 3, ignore=ignore)
else:
cur_loc = swarmie.get_odom_location().get_pose()
dist = math.hypot(cur_loc.x - rightmost_tag.pose.position.x,
cur_loc.y - rightmost_tag.pose.position.y)
if abs(dist - claw_offset) < 0.07:
# If dist - claw_offset is small, drive_to() is unlikely to
# make the rover move at all. In this case it's worth
# recovering or raising an InsideHomeException
# from recover()
return False
swarmie.drive_to(rightmost_tag.pose.position,
claw_offset=claw_offset, ignore=ignore)
return True
