def toggle_power(self, should_power_on): """Power the robot on/off dependent on the current power state.""" is_powered_on = self.check_is_powered_on() if not is_powered_on and should_power_on: # Power on the robot up before navigating when it is in a powered-off state. power_on(self._power_client) motors_on = False while not motors_on: future = self._robot_state_client.get_robot_state_async() state_response = future.result( timeout=10 ) # 10 second timeout for waiting for the state response. if state_response.power_state.motor_power_state == robot_state_pb2.PowerState.STATE_ON: motors_on = True else: # Motors are not yet fully powered on. time.sleep(.25) elif is_powered_on and not should_power_on: # Safe power off (robot will sit then power down) when it is in a # powered-on state. safe_power_off(self._robot_command_client, self._robot_state_client) else: # Return the current power state without change. return is_powered_on # Update the locally stored power state. self.check_is_powered_on() return self._powered_on
def test_safe_power_off_success(): mock_command_client = MockRobotCommandClient() mock_state_client = MockRobotStateClient() mock_state_client.power_state = robot_state_pb2.PowerState.STATE_OFF timeout = 1.0 mock_command_client.feedback_fn = lambda: time.sleep(timeout / 2.0) mock_command_client.response = power_pb2.STATUS_SUCCESS power.safe_power_off(mock_command_client, mock_state_client, timeout)
def test_safe_power_off_timeout(feedback_fn): mock_command_client = MockRobotCommandClient() mock_state_client = MockRobotStateClient() mock_state_client.feedback_fn = feedback_fn start = time.time() timeout = 1.0 with pytest.raises(power.CommandTimedOutError): power.safe_power_off(mock_command_client, mock_state_client, timeout) dt = time.time() - start assert abs(dt - timeout) < 0.1
def main(raw_args=None): """Replay stored mission""" body_lease = None # Configure logging bosdyn.client.util.setup_logging() # Parse command-line arguments parser = argparse.ArgumentParser() bosdyn.client.util.add_common_arguments(parser) parser.add_argument('--timeout', type=float, default=3.0, dest='timeout', help='Mission client timeout (s).') parser.add_argument('--noloc', action='store_true', default=False, dest='noloc', help='Skip initial localization') parser.add_argument( '--disable_alternate_route_finding', action='store_true', default=False, dest='disable_alternate_route_finding', help='Disable creating alternate-route-finding graph structure') parser.add_argument( '--disable_directed_exploration', action='store_true', default=False, dest='disable_directed_exploration', help='Disable directed exploration for skipped blocked branches') group = parser.add_mutually_exclusive_group() group.add_argument('--time', type=float, default=0.0, dest='duration', help='Time to repeat mission (sec)') group.add_argument('--static', action='store_true', default=False, dest='static_mode', help='Stand, but do not run robot') # Subparser for mission type subparsers = parser.add_subparsers(dest='mission_type', help='Mission type') subparsers.required = True # Subparser for simple mission simple_parser = subparsers.add_parser('simple', help='Simple mission (non-Autowalk)') simple_parser.add_argument('simple_mission_file', help='Mission file for non-Autowalk mission.') # Subparser for Autowalk mission autowalk_parser = subparsers.add_parser( 'autowalk', help='Autowalk mission using graph_nav') autowalk_parser.add_argument( 'map_directory', help= 'Directory containing graph_nav map and default Autowalk mission file.' ) autowalk_parser.add_argument( '--autowalk_mission', dest='autowalk_mission_file', help='Optional alternate Autowalk mission file.') args = parser.parse_args(raw_args) if args.mission_type == 'simple': do_map_load = False fail_on_question = False do_localization = False mission_file = args.simple_mission_file map_directory = None print('[ REPLAYING SIMPLE MISSION {} : HOSTNAME {} ]'.format( mission_file, args.hostname)) else: do_map_load = True fail_on_question = True if args.noloc: do_localization = False else: do_localization = True map_directory = args.map_directory if args.autowalk_mission_file: mission_file = args.autowalk_mission_file else: mission_file = map_directory + '/missions/autogenerated' print( '[ REPLAYING AUTOWALK MISSION {} : MAP DIRECTORY {} : HOSTNAME {} ]' .format(mission_file, map_directory, args.hostname)) # Initialize robot object robot = init_robot(args.hostname, args.username, args.password) # Acquire robot lease robot.logger.info('Acquiring lease...') lease_client = robot.ensure_client( bosdyn.client.lease.LeaseClient.default_service_name) body_lease = lease_client.acquire() if body_lease is None: raise Exception('Lease not acquired.') robot.logger.info('Lease acquired: %s', str(body_lease)) # Initialize power client robot.logger.info('Starting power client...') power_client = robot.ensure_client(PowerClient.default_service_name) # Initialize other clients robot_state_client, command_client, mission_client, graph_nav_client = init_clients( robot, body_lease, mission_file, map_directory, do_map_load, args.disable_alternate_route_finding) try: with bosdyn.client.lease.LeaseKeepAlive(lease_client): assert not robot.is_estopped(), "Robot is estopped. " \ "Please use an external E-Stop client, " \ "such as the estop SDK example, to configure E-Stop." # Turn on power power_on(power_client) # Stand up and wait for the perception system to stabilize robot.logger.info('Commanding robot to stand...') blocking_stand(command_client, timeout_sec=20) countdown(5) robot.logger.info('Robot standing.') # Localize robot localization_error = False if do_localization: graph = graph_nav_client.download_graph() robot.logger.info('Localizing robot...') robot_state = robot_state_client.get_robot_state() localization = nav_pb2.Localization() # Attempt to localize using any visible fiducial graph_nav_client.set_localization( initial_guess_localization=localization, ko_tform_body=None, max_distance=None, max_yaw=None, fiducial_init=graph_nav_pb2.SetLocalizationRequest. FIDUCIAL_INIT_NEAREST) # Run mission if not args.static_mode and not localization_error: if args.duration == 0.0: run_mission(robot, mission_client, lease_client, fail_on_question, args.timeout, args.disable_directed_exploration) else: repeat_mission(robot, mission_client, lease_client, args.duration, fail_on_question, args.timeout, args.disable_directed_exploration) finally: # Turn off power lease_client.lease_wallet.advance() robot.logger.info('Powering off...') safe_power_off(command_client, robot_state_client) # Return lease robot.logger.info('Returning lease...') lease_client.return_lease(body_lease)