def _set_initial_localization_waypoint(self, *args):
"""Trigger localization to a waypoint."""
# Take the first argument as the localization waypoint.
if len(args) < 1:
# If no waypoint id is given as input, then return without initializing.
print("No waypoint specified to initialize to.")
return
destination_waypoint = graph_nav_util.find_unique_waypoint_id(
args[0][0], self._current_graph,
self._current_annotation_name_to_wp_id)
if not destination_waypoint:
# Failed to find the unique waypoint id.
return
robot_state = self._robot_state_client.get_robot_state()
current_odom_tform_body = get_odom_tform_body(
robot_state.kinematic_state.transforms_snapshot).to_proto()
# Create an initial localization to the specified waypoint as the identity.
localization = nav_pb2.Localization()
localization.waypoint_id = destination_waypoint
localization.waypoint_tform_body.rotation.w = 1.0
self._graph_nav_client.set_localization(
initial_guess_localization=localization,
# It's hard to get the pose perfect, search +/-20 deg and +/-20cm (0.2m).
max_distance=0.2,
max_yaw=20.0 * math.pi / 180.0,
fiducial_init=graph_nav_pb2.SetLocalizationRequest.
FIDUCIAL_INIT_NO_FIDUCIAL,
ko_tform_body=current_odom_tform_body)
def test_set_localization_exceptions(client, service, server):
make_call = lambda: client.set_localization(nav_pb2.Localization())
make_call()
service.lease_use_result = lease_pb2.LeaseUseResult(
status=lease_pb2.LeaseUseResult.STATUS_OLDER)
with pytest.raises(bosdyn.client.LeaseUseError):
make_call()
service.lease_use_result = lease_pb2.LeaseUseResult(
status=lease_pb2.LeaseUseResult.STATUS_OK)
make_call()
service.set_loc_resp.status = service.set_loc_resp.STATUS_ROBOT_IMPAIRED
with pytest.raises(bosdyn.client.graph_nav.RobotFaultedError):
make_call()
service.set_loc_resp.status = service.set_loc_resp.STATUS_UNKNOWN_WAYPOINT
with pytest.raises(bosdyn.client.graph_nav.UnknownMapInformationError):
make_call()
service.set_loc_resp.status = service.set_loc_resp.STATUS_ABORTED
with pytest.raises(bosdyn.client.graph_nav.RequestAbortedError):
make_call()
service.set_loc_resp.status = service.set_loc_resp.STATUS_FAILED
with pytest.raises(bosdyn.client.graph_nav.RequestFailedError):
make_call()
def _set_initial_localization_fiducial(self, *args):
"""Trigger localization when near a fiducial."""
robot_state = self._robot_state_client.get_robot_state()
current_odom_tform_body = get_odom_tform_body(
robot_state.kinematic_state.transforms_snapshot).to_proto()
# Create an empty instance for initial localization since we are asking it to localize
# based on the nearest fiducial.
localization = nav_pb2.Localization()
self._graph_nav_client.set_localization(initial_guess_localization=localization,
ko_tform_body=current_odom_tform_body)
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 main():
'''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)
# If the map directory is omitted, we assume that everything the user wants done is encoded in
# the mission itself.
parser.add_argument('--map_directory',
nargs='?',
help='Optional path to map directory')
parser.add_argument('--mission',
dest='mission_file',
help='Optional path to mission file')
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')
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')
args = parser.parse_args()
# Use the optional map_directory argument as a proxy for these other tasks we normally do.
do_localization = (args.map_directory is not None) and (not args.noloc)
do_map_load = args.map_directory is not None
fail_on_question = args.map_directory is not None
if not args.mission_file:
if not args.map_directory:
raise Exception(
'Must specify at least one of map_directory or --mission.')
args.mission_file = os.path.join(args.map_directory, 'missions',
'autogenerated')
print('[ REPLAYING MISSION {} : MAP {} : HOSTNAME {} ]'.format(
args.mission_file, args.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))
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Initialize clients
robot_state_client, command_client, mission_client, graph_nav_client = init_clients(
robot, body_lease, args.mission_file, args.map_directory,
do_map_load)
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."
# Ensure robot is powered on
assert ensure_power_on(robot), 'Robot power on failed.'
# Stand up
robot.logger.info('Commanding robot to stand...')
stand_command = RobotCommandBuilder.synchro_stand_command()
command_client.robot_command(stand_command)
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)
# 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)
else:
repeat_mission(robot, mission_client, lease_client,
args.duration, fail_on_question,
args.timeout)
finally:
# Ensure robot is powered off
lease_client.lease_wallet.advance()
robot.logger.info('Powering off...')
power_off_success = ensure_power_off(robot)
# Return lease
robot.logger.info('Returning lease...')
lease_client.return_lease(body_lease)
def main():
'''Replay stored mission'''
import argparse
body_lease = None
# Configure logging
bosdyn.client.util.setup_logging()
# Parse command-line arguments
parser = argparse.ArgumentParser()
bosdyn.client.util.add_common_arguments(parser)
# If the map directory is omitted, we assume that everything the user wants done is encoded in
# the mission itself.
parser.add_argument('map_directory',
nargs='?',
help='Optional path to map directory')
parser.add_argument('--mission',
dest='mission_file',
help='Optional path to mission file')
parser.add_argument('--localize',
action='store_true',
default=False,
help='Localize robot before running mission')
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 and localize but do not run robot')
args = parser.parse_args()
# Use the optional map_directory argument as a proxy for these other tasks we normally do.
do_map_load = args.map_directory is not None
do_robot_stand = args.map_directory is not None
do_localization = (args.map_directory is not None) or args.localize
fail_on_question = args.map_directory is not None
if not args.mission_file:
if not args.map_directory:
raise Exception(
'Must specify at least one of map_directory or --mission.')
args.mission_file = os.path.join(args.map_directory, 'missions',
'autogenerated')
print('[ REPLAYING MISSION {} : MAP {} : HOSTNAME {} ]'.format(
args.mission_file, args.map_directory, args.hostname))
# Initialize robot object
robot = init_robot(args.hostname, args.username, args.password,
args.app_token)
# 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))
try:
with bosdyn.client.lease.LeaseKeepAlive(lease_client):
# Initialize clients
robot_state_client, command_client, mission_client, graph_nav_client = init_clients(
robot, body_lease, args.mission_file, args.map_directory,
do_map_load)
if do_robot_stand:
# Ensure robot is powered on
assert ensure_power_on(robot), 'Robot power on failed.'
# Stand up
robot.logger.info('Commanding robot to stand...')
stand_command = RobotCommandBuilder.stand_command()
command_client.robot_command(stand_command)
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
try:
graph_nav_client.set_localization(
initial_guess_localization=localization)
except:
robot.logger.error('<<< INITIAL LOCALIZATION FAILED >>>')
localization_error = True
if not args.static_mode and not localization_error:
if args.duration == 0.0:
run_mission(robot,
mission_client,
lease_client,
fail_on_question,
mission_timeout=3)
else:
repeat_mission(robot, mission_client, lease_client,
args.duration, fail_on_question)
finally:
if do_robot_stand:
# Ensure robot is powered off
power_off_success = ensure_power_off(robot)
# Return lease
robot.logger.info('Returning lease...')
lease_client.return_lease(body_lease)
