def most_restrictive_travel_params(travel_params, vel_limit=None):
if travel_params is None:
travel_params = graph_nav_pb2.TravelParams()
else:
travel_params = copy.deepcopy(travel_params)
def take_limiting(mine, other, compare):
# This is basically a hack to deal with proto3's handling of unset POD.
# All float and integer fields that are unset in a message will have a value of 0.
if other == 0:
return mine
if mine == 0:
return other
if compare(mine, other):
return other
return mine
def take_velocity_limit(returned, other):
# Look at max_vel using >=, then min_vel using <=.
for min_max, comp in (('max_vel', operator.ge), ('min_vel', operator.le)):
# If the other doesn't even have this field, skip to the next one.
if not other.HasField(text(min_max)):
continue
lim_returned = getattr(returned, min_max)
lim_other = getattr(other, min_max)
lim_returned.linear.x = take_limiting(lim_returned.linear.x, lim_other.linear.x, comp)
lim_returned.linear.y = take_limiting(lim_returned.linear.y, lim_other.linear.y, comp)
lim_returned.angular = take_limiting(lim_returned.angular, lim_other.angular, comp)
if vel_limit is not None:
take_velocity_limit(travel_params.velocity_limit, vel_limit)
return travel_params
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 generate_travel_params(max_distance, max_yaw, velocity_limit=None):
""" Generate the API TravelParams for navigation requests.
Args:
max_distance: Distances (meters) threshold for when we've reached the final waypoint.
max_yaw: Angle (radians) threshold for when we've reached the final waypoint.
velocity_limit: SE2VelocityLimit protobuf message for the speed the robot should use.
Returns:
The API TravelParams protobuf message.
"""
travel_params = graph_nav_pb2.TravelParams(max_distance=max_distance, max_yaw=max_yaw)
if velocity_limit is not None:
travel_params.velocity_limit.CopyFrom(velocity_limit)
return travel_params
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_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