Graph Nav

For clients to the graphnav service.

class bosdyn.client.graph_nav.GraphNavClient[source]

Bases: bosdyn.client.common.BaseClient

Client to the GraphNav service.

default_service_name = 'graph-nav-service'
service_type = 'bosdyn.api.graph_nav.GraphNavService'
update_from(other)[source]

Adopt key objects like processors, logger, and wallet from other.

set_localization(initial_guess_localization, ko_tform_body=None, max_distance=None, max_yaw=None, fiducial_init=2, use_fiducial_id=None, refine_fiducial_result_with_icp=False, do_ambiguity_check=False, **kwargs)[source]

Trigger a manual localization. Typically done to provide the initial localization.

Parameters
  • initial_guess_localization (nav_pb2.Localization) – Operator-supplied guess at localization.

  • ko_tform_body – Robot SE3Pose protobuf when the initial_guess was made.

  • max_distance – [optional] Margin of distance (meters) away from the initial guess.

  • max_yaw – [optional] Margin of angle (radians) away from the initial guess.

  • fiducial_init – Tells the initializer whether to use fiducials, and how to use them.

  • use_fiducial_id – If using FIDUCIAL_INIT_SPECIFIC, this is the specific fiducial ID to use for initialization.

  • refine_fiducial_result_with_icp – Boolean determining if ICP will run after a fiducial is used for an initial guess.

  • do_ambiguity_check – Boolean where if true, consider how nearby localizations appear.

Returns

The resulting localization after being triggered with a guess.

Raises
set_localization_async(initial_guess_localization, ko_tform_body=None, max_distance=None, max_yaw=None, fiducial_init=2, use_fiducial_id=None, refine_fiducial_result_with_icp=False, do_ambiguity_check=False, **kwargs)[source]

Async version of set_localization()

get_localization_state(request_live_point_cloud=False, request_live_images=False, request_live_terrain_maps=False, request_live_world_objects=False, request_live_robot_state=False, waypoint_id=None, **kwargs)[source]

Obtain current localization state of the robot.

Returns

The current localization protobuf for the robot.

Raises

RpcError – Problem communicating with the robot.

get_localization_state_async(request_live_point_cloud=False, request_live_images=False, request_live_terrain_maps=False, request_live_world_objects=False, request_live_robot_state=False, waypoint_id=None, **kwargs)[source]

Async version of get_localization_state().

navigate_route(route, cmd_duration, travel_params=None, leases=None, timesync_endpoint=None, **kwargs)[source]

Navigate the given route.

Parameters
  • route – Route protobuf of the route to follow.

  • travel_params – API TravelParams for the route.

  • cmd_duration – Number of seconds the command can run for.

  • leases – Leases to show ownership of necessary resources. Will use the client’s leases by default.

  • timesync_endpoint – Use this endpoint for timesync fields. Will use the client’s endpoint by default.

  • kwargs – Passed to underlying RPC. Example: timeout=5 to cancel the RPC after 5 seconds.

Returns

Command ID to use in feedback lookup.

Raises
navigate_route_async(route, cmd_duration, travel_params=None, leases=None, timesync_endpoint=None, **kwargs)[source]

Async version of navigate_route()

navigate_to(destination_waypoint_id, cmd_duration, route_params=None, travel_params=None, leases=None, timesync_endpoint=None, **kwargs)[source]

Navigate to a specific waypoint along a route chosen by the GraphNav service.

Parameters
  • destination_waypoint_id – Waypoint id string for where to go to.

  • cmd_duration – Number of seconds the command can run for.

  • route_params – API RouteGenParams for the route.

  • travel_params – API TravelParams for the route.

  • leases – Leases to show ownership of necessary resources. Will use the client’s leases by default.

  • timesync_endpoint – Use this endpoint for timesync fields. Will use the client’s endpoint by default.

Returns

Command ID to use in feedback lookup.

Return type

int

Raises
navigate_to_async(destination_waypoint_id, cmd_duration, route_params=None, travel_params=None, leases=None, timesync_endpoint=None, **kwargs)[source]

Async version of navigate_to().

navigation_feedback(command_id=0, **kwargs)[source]

Returns the feedback corresponding to the active route follow command.

Parameters

command_id (int) – If blank, will return current command status. If filled out, will attempt to return that command status

Returns

NavigationFeedbackResponse

Raises

RpcError – Problem communicating with the robot.

navigation_feedback_async(command_id=0, **kwargs)[source]

Async version of navigation_feedback().

clear_graph(lease=None, **kwargs)[source]

Clears the local graph structure. Also erases any snapshots currently in RAM.

Parameters

leases – Leases to show ownership of necessary resources. Will use the client’s leases by default.

Raises
clear_graph_async(lease=None, **kwargs)[source]

Async version of clear_graph().

upload_graph(lease=None, graph=None, **kwargs)[source]

Uploads a graph to the server and appends to the existing graph.

Parameters
  • leases – Leases to show ownership of necessary resources. Will use the client’s leases by default.

  • graph – Graph protobuf that represents the map with waypoints and edges.

Returns

The response, which includes waypoint and edge id’s sorted by whether it was cached.

Raises
upload_graph_async(lease=None, graph=None, **kwargs)[source]

Async version of upload_graph().

upload_waypoint_snapshot(waypoint_snapshot, lease=None, **kwargs)[source]

Uploads large waypoint snapshot as a stream for a particular waypoint.

Parameters
  • leases – Leases to show ownership of necessary resources. Will use the client’s leases by default.

  • waypoint_snapshot – WaypointSnapshot protobuf that will be stream uploaded to the robot.

Returns

The status of the upload request.

Raises
upload_edge_snapshot(edge_snapshot, lease=None, **kwargs)[source]

Uploads large edge snapshot as a stream for a particular edge.

Parameters
  • leases – Leases to show ownership of necessary resources. Will use the client’s leases by default.

  • edge_snapshot – EdgeSnapshot protobuf that will be stream uploaded to the robot.

Returns

The status of the upload request.

Raises
download_graph(**kwargs)[source]

Downloads the graph from the server.

Returns

The graph protobuf that represents the current map on the robot (with waypoints and edges).

Raises

RpcError – Problem communicating with the robot

download_graph_async(**kwargs)[source]

Async version of download_graph().

download_waypoint_snapshot(waypoint_snapshot_id, download_images=False, **kwargs)[source]

Download a specific waypoint snapshot with streaming from the server.

Parameters
  • waypoint_snapshot_id – WaypointSnapshot string ID for which snapshot to download from robot.

  • download_images – Boolean indicating whether or not to include images in the download.

Returns

The WaypointSnapshot protobuf from the robot’s current map.

Raises
download_edge_snapshot(edge_snapshot_id, **kwargs)[source]

Downloads a specific edge snapshot with streaming from the server.

Parameters

edge_snapshot_id – EdgeSnapshot string ID for which snapshot to download from robot.

Returns

The EdgeSnapshot protobuf from the robot’s current map.

Raises
write_graph_and_snapshots(directory)[source]

Download the graph and snapshots from robot to the specified directory.

static generate_travel_params(max_distance, max_yaw, velocity_limit=None)[source]

Generate the API TravelParams for navigation requests.

Parameters
  • 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.

static generate_route_params(waypoint_id_list)[source]

Generate the API RouteGenParams for navigation requests.

Parameters

waypoint_id_list – List of waypoint id strings in which a route should pass through.

Returns

The API RouteGenParams protobuf message.

static build_route(waypoint_id_list, edge_id_list)[source]

Generate the API RouteGenParams for navigation requests.

Parameters
  • waypoint_id_list – List of waypoint id strings in which a route should pass through. The ids should be ordered from [start waypoint –> destination waypoint].

  • edge_id_list – List of the edge_id’s which should be in the same ordering as the waypoint list.

Returns

The API Route protobuf message.

exception bosdyn.client.graph_nav.GraphNavServiceResponseError(response, error_message=None)[source]

Bases: bosdyn.client.exceptions.ResponseError

General class of errors for the GraphNav Recording Service.

exception bosdyn.client.graph_nav.RequestAbortedError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

Request was aborted by the system.

exception bosdyn.client.graph_nav.RequestFailedError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

Request failed to complete by the system.

exception bosdyn.client.graph_nav.RobotFaultedError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

Robot is experiencing a fault condition that prevents localization.

exception bosdyn.client.graph_nav.UnknownMapInformationError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

The given map information (waypoints,edges,routes) is unknown by the system.

exception bosdyn.client.graph_nav.TimeError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

Errors associated with timestamps and time sync.

exception bosdyn.client.graph_nav.CommandExpiredError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.TimeError

The command was received after its end time had already passed.

exception bosdyn.client.graph_nav.NoTimeSyncError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.TimeError

Client has not performed timesync with robot.

exception bosdyn.client.graph_nav.TooDistantError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.TimeError

The command was too far in the future.

exception bosdyn.client.graph_nav.RobotStateError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

Errors associated with the current state of the robot.

exception bosdyn.client.graph_nav.IsRecordingError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RobotStateError

Cannot navigate a route while recording a map.

exception bosdyn.client.graph_nav.RobotImpairedError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RobotStateError

Robot has a critical perception or behavior fault and cannot navigate.

exception bosdyn.client.graph_nav.RouteError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

Errors associated with the specified route.

exception bosdyn.client.graph_nav.ConstraintFaultError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteError

Route parameters contained a constraint fault.

exception bosdyn.client.graph_nav.InvalidEdgeError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteError

One or more edges do not connect to expected waypoints.

exception bosdyn.client.graph_nav.UnkownRouteElementsError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteError

One or more waypoints/edges are not in the map.

exception bosdyn.client.graph_nav.NoPathError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteError

There is no path to the specified waypoint.

exception bosdyn.client.graph_nav.UnknownWaypointError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteError

One or more waypoints are not in the map.

exception bosdyn.client.graph_nav.RouteNavigationError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.GraphNavServiceResponseError

Errors related to how the robot navigates the route.

exception bosdyn.client.graph_nav.FeatureDesertError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteNavigationError

Route contained too many waypoints with low-quality features.

exception bosdyn.client.graph_nav.RouteNotUpdatingError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteNavigationError

Graph nav was unable to update and follow the specified route.

exception bosdyn.client.graph_nav.RobotLostError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteNavigationError

Cannot issue a navigation request when the robot is already lost.

exception bosdyn.client.graph_nav.RobotNotLocalizedToRouteError(response, error_message=None)[source]

Bases: bosdyn.client.graph_nav.RouteNavigationError

The current localization doesn’t refer to any waypoint in the route (possibly uninitialized localization).