def main():
swarmie.start(node_name='task')
if swarmie.simulator_running():
swarmie.plants_init()
taskman = Task()
while not rospy.is_shutdown():
taskman.run_next()
def main(use_swarmie=False):
rovername = rospy.get_namespace().strip('/')
swarmie.start(node_name='teleop')
if use_swarmie:
teleop = TeleopSwarmie(rovername)
else:
teleop = TeleopKey(rovername)
teleop.run()
for _ in range(10):
try:
prev_rightmost_tag = rightmost_tag
rightmost_tag = find_rightmost_home_tag()
if not drive_to_next_tag(rightmost_tag, prev_rightmost_tag):
recover()
fail_count += 1
if fail_count >= max_fail_count:
raise PathException(('Unable to find a home corner, ' +
'no home tags are in view.'))
except HomeCornerException:
# success!!
return True
check_inside_home()
raise PathException('Unable to find a home corner, I gave up.')
def main():
if find_home_corner():
print('Success!! We found a corner of home!!!')
if __name__ == '__main__':
# For testing this as a standalone script.
swarmie.start(node_name='find_corner')
main()
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
if __name__ == '__main__':
swarmie.start(node_name='pickup')
sys.exit(main())
global quiet
quiet = not quiet
if quiet :
print ('Topic echo is OFF')
else:
print ('Topic echo is ON')
if __name__ == '__main__' :
global quiet
quiet = False
namespace = rospy.get_namespace()
rover = namespace.strip('/')
swarmie.start(tf_rover_name=rover, node_name='rdb')
if swarmie.simulator_running():
swarmie.plants_init()
print ('Connected.')
rospy.Subscriber('status', String, lambda msg : logHandler('/status:', msg))
print ("Subscribed to", rospy.resolve_name('status'))
rospy.Subscriber('/infoLog', String, lambda msg : logHandler('/infoLog:', msg))
print ("Subscribed to /infoLog")
rospy.Subscriber('state_machine', String, lambda msg : logHandler('/state_machine:', msg))
print ("Subscribed to", rospy.resolve_name('state_machine'))
print ('\033[31;1mTopic data will be displayed. Call toggle_output() to toggle.\033[0m')
readline.parse_and_bind("tab: complete")
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():
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter)
parser.add_argument('distance',
type=float,
help='The length (m) of each side of the square.')
parser.add_argument(
'--ignore-sonar',
action='store_true',
help='Whether the rover should ignore sonar obstacles on its journey.')
args = parser.parse_args()
smart_square(args.distance, args.ignore_sonar)
if __name__ == '__main__':
swarmie.start(node_name='square')
main()
#! /usr/bin/env python
import math
import rospy
from mobility.swarmie import swarmie
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)
if __name__ == '__main__':
swarmie.start(node_name='plant_demo')
main()
block = swarmie.get_nearest_block_location(targets_buffer_age=0.5)
if block is not None:
nearest.poses = [Pose(position=block)]
nearest_block_pub.publish(nearest)
blocks_pub.publish(blocks)
blocks_filtered_pub.publish(blocks_filtered)
def main():
blocks_pub = rospy.Publisher('targets/blocks', PoseArray, queue_size=10)
blocks_filtered_pub = rospy.Publisher('targets/blocks/filtered',
PoseArray,
queue_size=10)
nearest_block_pub = rospy.Publisher('targets/nearest_block',
PoseArray,
queue_size=10)
rate = rospy.Rate(1)
while not rospy.is_shutdown():
target_tags = swarmie.get_targets_buffer(
age=0.5, id=0) # type: List[AprilTagDetection]
publish(blocks_pub, blocks_filtered_pub, nearest_block_pub,
target_tags)
rate.sleep()
if __name__ == '__main__':
swarmie.start(node_name='targets_block_location')
main()
(math.sqrt(home_detection.pose.position.x ** 2
+ home_detection.pose.position.y **2)
+ OVERSHOOT_DIST)
if not see_home_tag:
# doesn't make sense to turn or drive if no home tags were seen
return 0, 0
return result['angle'], result['dist']
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)
def main(**kwargs):
"""Get out of home!"""
obstacles = swarmie.get_obstacle_condition()
if obstacles & Obstacle.INSIDE_HOME != Obstacle.INSIDE_HOME:
return
escape_home(swarmie.get_latest_targets(id=256))
if __name__ == '__main__':
swarmie.start(node_name='escape_home')
sys.exit(main())
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
if __name__ == '__main__':
swarmie.start(node_name='plant approach')
sys.exit(main())
# 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
if __name__ == '__main__':
swarmie.start(node_name='init')
sys.exit(main())
claw_offset=0.5,
ignore=Obstacle.VISION_SAFE | Obstacle.IS_SONAR,
timeout=3,
**swarmie.speed_slow) #get a bit closer
except ObstacleException:
pass
def water_plants_exit(code):
sys.exit(code)
def main(**kwargs):
global planner, use_waypoints
use_waypoints = True
try:
planner
except NameError:
from mobility.planner import Planner
planner = Planner(use_rviz_nav_goal=True)
if swarmie.simulator_running() and not swarmie.plants:
swarmie.plants_init()
plant_walk(num_moves=10)
print("I'm homesick!")
water_plants_exit(1)
if __name__ == '__main__':
swarmie.start(node_name='watering plants')
sys.exit(main())
# 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 main(**kwargs):
"""Do refill behavior."""
# TODO: What should find_home_corner() do when it's called with no home tags
# in view? Does this happen very often?
use_approx_home = True
try:
if find_home_corner():
use_approx_home = False
except PathException as e:
swarmie.print_infoLog('<font color="red">{}</font>'.format(e.status))
rospy.logwarn(e.status)
refill(use_approx_home)
exit_home()
return 0
if __name__ == '__main__':
swarmie.start(node_name='refill')
sys.exit(main())
def main() :
swarmie.start(node_name='task')
taskman = Task()
while not rospy.is_shutdown():
taskman.run_next()
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
if __name__ == '__main__' :
parser = argparse.ArgumentParser(
formatter_class=argparse.ArgumentDefaultsHelpFormatter
)
parser.add_argument(
'--has-block',
action='store_true',
help=('whether the rover currently has a block, and should ' +
'accordingly either avoid cubes or stop for them')
)
args = parser.parse_args()
swarmie.start(node_name='gohome')
sys.exit(main(has_block=args.has_block))
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)
if __name__ == '__main__':
swarmie.start(node_name='search')
sys.exit(main())
#! /usr/bin/env python
"""Calibrate the IMU using the 2D method, if necessary."""
from __future__ import print_function
import sys
import rospy
from mobility.swarmie import swarmie
def main(**kwargs):
# Wait until the extended calibration file has been loaded in IMU node.
while not swarmie.imu_is_finished_validating():
pass
if not swarmie.simulator_running():
# TODO: would it make sense to calibrate the simulated IMU as well?
swarmie.start_gyro_bias_calibration()
rospy.sleep(3)
swarmie.store_imu_calibration()
return 0
if __name__ == '__main__':
swarmie.start(node_name='calibrate_imu')
sys.exit(main())
