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")