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():
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 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 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