def _make_localize_node(self):
"""Make localization node."""
loc = nodes_pb2.Node()
loc.name = "localize robot"
impl = nodes_pb2.BosdynGraphNavLocalize()
loc.impl.Pack(impl)
return loc
def _make_goto_node(self, waypoint_id):
""" Create a leaf node that will go to the waypoint. """
ret = nodes_pb2.Node()
ret.name = "goto %s" % waypoint_id
impl = nodes_pb2.BosdynNavigateTo()
impl.destination_waypoint_id = waypoint_id
ret.impl.Pack(impl)
return ret
def _make_localize_node(self):
"""Make localization node."""
loc = nodes_pb2.Node()
loc.name = "localize robot"
impl = nodes_pb2.BosdynGraphNavLocalize()
impl.localization_request.fiducial_init = graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NEAREST
loc.impl.Pack(impl)
return loc
def _make_localize_node(self, waypoint_id):
"""Make localization node."""
loc = nodes_pb2.Node()
loc.name = "localize robot"
impl = nodes_pb2.BosdynGraphNavLocalize()
impl.localization_request.fiducial_init = graph_nav_pb2.SetLocalizationRequest.FIDUCIAL_INIT_NEAREST_AT_TARGET
impl.localization_request.initial_guess.waypoint_id = waypoint_id
loc.impl.Pack(impl)
return loc
def _make_go_route_node(self, prev_waypoint_id, waypoint_id):
""" Create a leaf node that will go to the waypoint along a route. """
ret = nodes_pb2.Node()
ret.name = "go route %s -> %s" % (prev_waypoint_id, waypoint_id)
vel_limit = NAV_VELOCITY_LIMITS
if prev_waypoint_id not in self._all_graph_wps_in_order:
raise RuntimeError(
f'{prev_waypoint_id} (FROM) not in {self._all_graph_wps_in_order}'
)
if waypoint_id not in self._all_graph_wps_in_order:
raise RuntimeError(
f'{waypoint_id} (TO) not in {self._all_graph_wps_in_order}')
prev_wp_idx = self._all_graph_wps_in_order.index(prev_waypoint_id)
wp_idx = self._all_graph_wps_in_order.index(waypoint_id)
impl = nodes_pb2.BosdynNavigateRoute()
backwards = False
if wp_idx >= prev_wp_idx:
impl.route.waypoint_id[:] = self._all_graph_wps_in_order[
prev_wp_idx:wp_idx + 1]
else:
backwards = True
# Switch the indices and reverse the output order.
impl.route.waypoint_id[:] = reversed(
self._all_graph_wps_in_order[wp_idx:prev_wp_idx + 1])
edge_ids = [e.id for e in self._graph.edges]
for i in range(len(impl.route.waypoint_id) - 1):
eid = map_pb2.Edge.Id()
# Because we only record one (non-branching) chain, and we only create routes from
# older to newer waypoints, we can just follow the time-sorted list of all waypoints.
if backwards:
from_i = i + 1
to_i = i
else:
from_i = i
to_i = i + 1
eid.from_waypoint = impl.route.waypoint_id[from_i]
eid.to_waypoint = impl.route.waypoint_id[to_i]
impl.route.edge_id.extend([eid])
if eid not in edge_ids:
raise RuntimeError(
f'Was map recorded in linear chain? {eid} not in graph')
if any(self._desert_flag[wp_id] for wp_id in impl.route.waypoint_id):
tolerance = graph_nav_pb2.TravelParams.TOLERANCE_IGNORE_POOR_FEATURE_QUALITY
else:
tolerance = graph_nav_pb2.TravelParams.TOLERANCE_DEFAULT
travel_params = graph_nav_pb2.TravelParams(
velocity_limit=vel_limit, feature_quality_tolerance=tolerance)
impl.travel_params.CopyFrom(travel_params)
ret.impl.Pack(impl)
return ret
def upload_mission(robot, client, filename, lease):
"""Upload the mission to the robot"""
# Load the mission from disk
robot.logger.info('Loading mission from ' + filename)
with open(filename, 'rb') as mission_file:
data = mission_file.read()
mission_proto = nodes_pb2.Node()
mission_proto.ParseFromString(data)
# Upload the mission to the robot
robot.logger.info('Uploading the mission to the robot...')
client.load_mission(mission_proto, leases=[lease])
robot.logger.info('Uploaded mission to robot.')
def _make_goto_node(self, waypoint_id):
""" Create a leaf node that will go to the waypoint. """
ret = nodes_pb2.Node()
ret.name = "goto %s" % waypoint_id
vel_limit = NAV_VELOCITY_LIMITS
# We don't actually know which path we will plan. It could have many feature deserts.
# So, let's just allow feature deserts.
tolerance = graph_nav_pb2.TravelParams.TOLERANCE_IGNORE_POOR_FEATURE_QUALITY
travel_params = graph_nav_pb2.TravelParams(
velocity_limit=vel_limit, feature_quality_tolerance=tolerance)
impl = nodes_pb2.BosdynNavigateTo(travel_params=travel_params)
impl.destination_waypoint_id = waypoint_id
ret.impl.Pack(impl)
return ret
def _make_mission(self):
""" Create a mission that visits each waypoint on stored path."""
# Create a Sequence that visits all the waypoints.
sequence = nodes_pb2.Sequence()
sequence.children.add().CopyFrom(self._make_localize_node())
for waypoint_id in self._waypoint_path:
if waypoint_id == 'LOCALIZE':
sequence.children.add().CopyFrom(self._make_localize_node())
else:
sequence.children.add().CopyFrom(
self._make_goto_node(waypoint_id))
# Return a Node with the Sequence.
ret = nodes_pb2.Node()
ret.name = "visit %d waypoints" % len(self._waypoint_path)
ret.impl.Pack(sequence)
return ret
def _make_goto_node(self, waypoint_id):
""" Create a leaf node that will go to the waypoint. """
ret = nodes_pb2.Node()
ret.name = "goto %s" % waypoint_id
vel_limit = NAV_VELOCITY_LIMITS
if self._desert_flag[waypoint_id]:
tolerance = graph_nav_pb2.TravelParams.TOLERANCE_IGNORE_POOR_FEATURE_QUALITY
else:
tolerance = graph_nav_pb2.TravelParams.TOLERANCE_DEFAULT
travel_params = graph_nav_pb2.TravelParams(
velocity_limit=vel_limit, feature_quality_tolerance=tolerance)
impl = nodes_pb2.BosdynNavigateTo(travel_params=travel_params)
impl.destination_waypoint_id = waypoint_id
ret.impl.Pack(impl)
return ret
def _make_mission(self):
""" Create a mission that visits each waypoint on stored path."""
self.add_message("Mission: " + str(self._waypoint_commands))
# Create a Sequence that visits all the waypoints.
sequence = nodes_pb2.Sequence()
prev_waypoint_command = None
first_waypoint = True
for waypoint_command in self._waypoint_commands:
if waypoint_command == 'LOCALIZE':
if prev_waypoint_command is None:
raise RuntimeError(f'No prev waypoint; LOCALIZE')
sequence.children.add().CopyFrom(
self._make_localize_node(prev_waypoint_command))
elif waypoint_command == 'INITIALIZE':
# Initialize to any fiducial.
sequence.children.add().CopyFrom(self._make_initialize_node())
else:
if first_waypoint:
# Go to the beginning of the mission using any path. May be a no-op.
sequence.children.add().CopyFrom(
self._make_goto_node(waypoint_command))
else:
if prev_waypoint_command is None:
raise RuntimeError(
f'No prev waypoint; route to {waypoint_command}')
sequence.children.add().CopyFrom(
self._make_go_route_node(prev_waypoint_command,
waypoint_command))
prev_waypoint_command = waypoint_command
first_waypoint = False
# Return a Node with the Sequence.
ret = nodes_pb2.Node()
ret.name = "Visit %d goals" % len(self._waypoint_commands)
ret.impl.Pack(sequence)
return ret
def proto_from_tuple(tup):
"""Return a bosdyn.api.mission Node from a tuple. EXPERIMENTAL.
Example:
::
('do-A-then-B', bosdyn.api.mission.nodes_pb2.Sequence(always_restart=True),
[
('A', foo.bar.Command(...), []),
('B', foo.bar.Command(...), []),
]
)
would make a Sequence named 'do-A-then-B' that always restarted, executing some Command
named 'A' followed by the Command named 'B'. NOTE: The "List of children tuples" will only
work if the parent node has 'child' or 'children' attributes. See tests/test_util.py for a
longer example.
Args:
tup: (Name of node, Instantiated implementation of node protobuf, List of children tuples)
"""
node = nodes_pb2.Node()
name_or_dict, inner_proto, children = tup
if isinstance(name_or_dict, dict):
node.name = name_or_dict.get('name', '')
node.reference_id = name_or_dict.get('reference_id', '')
node.node_reference = name_or_dict.get('node_reference', '')
if 'user_data' in name_or_dict:
node.user_data.CopyFrom(name_or_dict['user_data'])
for name, pb_type in six.iteritems(name_or_dict.get('parameters', {})):
parameter = util_pb2.VariableDeclaration(name=name, type=pb_type)
node.parameters.add().CopyFrom(parameter)
for key, value in six.iteritems(
name_or_dict.get('parameter_values', {})):
parameter_value = util_pb2.KeyValue(key=key)
if isinstance(value, six.string_types) and value[0] == '$':
parameter_value.value.parameter.name = value[1:]
# Leave type unspecified, since we can't look it up easily yet.
else:
parameter_value.value.constant.CopyFrom(
python_var_to_value(value))
node.parameter_values.add().CopyFrom(parameter_value)
for key, value in six.iteritems(name_or_dict.get('overrides', {})):
# Rather than keep this matching the compiler functionality, just omit the check.
# It's not even a CompileError anyway.
#if key not in inner_proto.DESCRIPTOR.fields_by_name:
# raise CompileError('Override specified for "{}" but no such field in "{}"'.format(
# key, inner_proto.DESCRIPTOR.full_name))
override = util_pb2.KeyValue(key=key)
override.value.parameter.name = value
# We do need the final field for setting the type accurately...
#override.value.parameter.type = field_desc_to_pb_type(inner_proto.DESCRIPTOR.fields_by_name[key])
node.overrides.add().CopyFrom(override)
else:
node.name = name_or_dict
if node.node_reference:
return node
num_children = len(children)
inner_type = inner_proto.DESCRIPTOR.name
# Do some sanity checking on the children.
if hasattr(inner_proto, 'children'):
if num_children == 0:
raise Error('Proto "{}" of type "{}" has no children!'.format(
node.name, inner_type))
for child_tup in children:
child_node = proto_from_tuple(child_tup)
inner_proto.children.add().CopyFrom(child_node)
elif hasattr(inner_proto, 'child'):
if isinstance(inner_proto,
nodes_pb2.ForDuration) and num_children == 2:
inner_proto.child.CopyFrom(proto_from_tuple(children[0]))
inner_proto.timeout_child.CopyFrom(proto_from_tuple(children[1]))
elif num_children == 1:
inner_proto.child.CopyFrom(proto_from_tuple(children[0]))
else:
raise Error('Proto "{}" of type "{}" has {} children!'.format(
node.name, inner_type, num_children))
elif num_children != 0:
raise Error(
'Proto "{}" of type "{}" was given {} children, but I do not know how to add'
' them!'.format(node.name, inner_type, num_children))
node.impl.Pack(inner_proto)
return node
from __future__ import unicode_literals
from builtins import str as text
import copy
import operator
import re
import six
import google.protobuf.message
import google.protobuf.text_format
from bosdyn.api.mission import mission_pb2, nodes_pb2, util_pb2
from bosdyn.api.docking import docking_pb2
from bosdyn.api.graph_nav import graph_nav_pb2
from bosdyn.mission import constants
# This is a special dummy message we'll use for TYPE_MESSAGE parameters.
DUMMY_MESSAGE = nodes_pb2.Node(name='dummy-message-for-parameterization')
class Error(Exception):
pass
class InvalidConversion(Error):
"""Could not convert the provided value to the destination type."""
def __init__(self, original_value, destination_typename):
self.original_value = original_value
self.destination_typename = destination_typename
def __str__(self):
return 'Could not convert "{}" to type "{}"'.format(
self.original_value, self.destination_typename)
help=
'Specify the user-string input to Tick. Set to the node name in Autowalk missions.'
)
options = parser.parse_args()
# Build our mission!
# Start with the thing we want to do, which is to make a RemoteGrpc call...
mission_call = nodes_pb2.RemoteGrpc()
# ... to the service whose name matches the name remote_mission_service is using ...
mission_call.service_name = remote_mission_service.SERVICE_NAME_IN_DIRECTORY
# ... and if we want to provide a lease, ask for the one resource we know about ...
if options.add_resources:
mission_call.lease_resources[:] = options.add_resources.split(',')
# ... and registered to "localhost" (i.e. the robot).
mission_call.host = 'localhost'
# Optionally add an input set by the RemoteGrpc node.
if options.user_string:
name = 'user-string'
value = util_pb2.Value(constant=util_pb2.ConstantValue(
string_value=options.user_string))
mission_call.inputs.add().CopyFrom(
util_pb2.KeyValue(key=name, value=value))
# That will be the implementation of our mission.
mission = nodes_pb2.Node()
mission.impl.Pack(mission_call)
with open(options.output_file, 'wb') as output:
output.write(mission.SerializeToString())
# Add one of the valid responses.
option = prompt.options.add()
option.text = 'Fail'
option.answer_code = -1
# Now, build the Condition node that checks for a SUCCESS response:
# 0 == prompt_answer_code
condition = nodes_pb2.Condition()
condition.operation = nodes_pb2.Condition.COMPARE_EQ
condition.lhs.const.int_value = 0
# During execution, this node will read the value of "prompt_answer_code" out of the blackboard.
condition.rhs.var.name = prompt.source
# We expect it to be an integer.
condition.rhs.var.type = util_pb2.VariableDeclaration.TYPE_INT
# Pack the Condition implementation into a Node message.
condition_node = nodes_pb2.Node(name='Check prompt is success')
condition_node.impl.Pack(condition)
# And set the node as the child of the Prompt, giving it access to it's "source" variable in
# the blackboard.
prompt.child.CopyFrom(condition_node)
# Finally, pack the Prompt itself into a Node message.
prompt_node = nodes_pb2.Node(name='Ask the question')
prompt_node.impl.Pack(prompt)
# Write out the mission.
with open(options.output_file, 'wb') as output:
output.write(prompt_node.SerializeToString())
