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():
swarmie.start(node_name='task')
if swarmie.simulator_running():
swarmie.plants_init()
taskman = Task()
while not rospy.is_shutdown():
taskman.run_next()
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
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)
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 refill(approx_home=False):
"""Drive in and refill the tank.
Args:
approx_home: Whether we should use the approximate home location as the
goal point to drive toward.
"""
if approx_home:
rospy.logwarn("Performing refill using approximate home location.")
home_odom = swarmie.get_home_odom_location(approx=approx_home)
if swarmie.simulator_running():
claw_offset = 0.5
else:
claw_offset = 0.6
swarmie.drive_to(home_odom,
claw_offset=claw_offset,
ignore=Obstacle.IS_VISION | Obstacle.IS_SONAR)
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 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
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")