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 power_on(self):
"""Enble the motor power if e-stop is enabled."""
try:
power.power_on(self._power_client)
return True, "Success"
except:
return False, "Error"
def test_power_on_failure():
mock_client = MockPowerClient()
timeout = 1.0
mock_client.feedback_fn = lambda: time.sleep(timeout / 2.0)
mock_client.response = power_pb2.STATUS_FAULTED
with pytest.raises(power.FaultedError, match=r".* Cannot power on due to a fault.*"):
power.power_on(mock_client, timeout_sec=timeout)
def test_power_on_timeout(feedback_fn):
mock_client = MockPowerClient()
mock_client.feedback_fn = feedback_fn
start = time.time()
timeout = 1.0
with pytest.raises(power.CommandTimedOutError):
power.power_on(mock_client, timeout_sec=timeout)
dt = time.time() - start
assert abs(dt - timeout) < 0.1
def enable_movement(self):
if self.__lease is None:
logging.info("Acquiring lease")
self.__lease = self.__lease_client.take()
self.__lease_keep_alive = LeaseKeepAlive(self.__lease_client)
if self.__estop_endpoint is None:
logging.info("Creating estop endpoint")
self.__estop_endpoint = estop.EstopEndpoint(
client=self.__estop_client,
name='mt-node-payload',
estop_timeout=9)
self.__estop_endpoint.force_simple_setup()
self.__estop_keepalive = EstopKeepAlive(self.__estop_endpoint)
try:
logging.info("Powering motors")
power.power_on(self.__power_client)
except BatteryMissingError:
logging.error("Battery missing")
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)
def test_power_on_success():
mock_client = MockPowerClient()
timeout = 1.0
mock_client.feedback_fn = lambda: time.sleep(timeout / 2.0)
mock_client.response = power_pb2.STATUS_SUCCESS
power.power_on(mock_client, timeout_sec=timeout)
