-
Notifications
You must be signed in to change notification settings - Fork 551
/
graph_nav.proto
1472 lines (1273 loc) · 67.8 KB
/
graph_nav.proto
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
// Copyright (c) 2023 Boston Dynamics, Inc. All rights reserved.
//
// Downloading, reproducing, distributing or otherwise using the SDK Software
// is subject to the terms and conditions of the Boston Dynamics Software
// Development Kit License (20191101-BDSDK-SL).
syntax = "proto3";
package bosdyn.api.graph_nav;
option java_outer_classname = "GraphNavProto";
import "bosdyn/api/basic_command.proto";
import "bosdyn/api/data_chunk.proto";
import "bosdyn/api/geometry.proto";
import "bosdyn/api/gps/gps.proto";
import "bosdyn/api/graph_nav/gps.proto";
import "bosdyn/api/graph_nav/lost_detection.proto";
import "bosdyn/api/graph_nav/nav.proto";
import "bosdyn/api/graph_nav/map.proto";
import "bosdyn/api/graph_nav/area_callback.proto";
import "bosdyn/api/header.proto";
import "bosdyn/api/lease.proto";
import "bosdyn/api/license.proto";
import "bosdyn/api/robot_state.proto";
import "bosdyn/api/service_fault.proto";
import "google/protobuf/wrappers.proto";
import "google/protobuf/duration.proto";
import "google/protobuf/timestamp.proto";
message VisualRefinementOptions {
// Whether to return a STATUS_VISUAL_ALIGNMENT_FAILED if refine_with_visual_features fails.
bool verify_refinement_quality = 1;
}
// The SetLocalization request is used to initialize or reset the localization of GraphNav
// to a map. A localization consists of a waypoint ID, and a pose of the robot relative to that
// waypoint. GraphNav uses the localization to decide how to navigate through a map. The
// SetLocalizationRequest contains parameters to help find a correct localization. For example,
// AprilTags (fiducials) may be used to set the localization, or the caller can provide an explicit
// guess of the localization.
// Once the SetLocalizationRequest completes, the current localization to the map
// will be modified, and can be retrieved using a GetLocalizationStateRequest.
message SetLocalizationRequest {
// Common request header.
RequestHeader header = 1;
// Operator-supplied guess at localization. In this localization message, only waypoint_id and
// waypoint_tform_body will be used. All other fields are ignored. If the message is not
// provided, the initial guess will be assumed to be the identity transform to whatever waypoint
// was requested for methods that require it. Use initial_guess.waypoint_id to restrict the
// SetLocalizationRequest to specific waypoints.
Localization initial_guess = 3;
// Robot pose when the initial_guess was made.
// This overcomes the race that occurs when the client is trying to initialize a moving robot.
// GraphNav will use its local ko_tform_body and this ko_tform_body to update the initial
// localization guess, if necessary.
SE3Pose ko_tform_body = 4;
// The max distance [meters] is how far away the robot is allowed to localize from the position
// supplied in the initial guess. If not specified, the offset is used directly. Otherwise it
// searches a neighborhood of the given size.
double max_distance = 5;
// The max yaw [radians] is how different the localized yaw is allowed to be from the supplied
// yaw in the initial guess. If not specified, the offset is used directly. Otherwise it
// searches a neighborhood of the given size.
double max_yaw = 6;
enum FiducialInit {
FIDUCIAL_INIT_UNKNOWN = 0; // It is a programming error to use this one.
FIDUCIAL_INIT_NO_FIDUCIAL = 1; // Ignore fiducials during initialization.
FIDUCIAL_INIT_NEAREST = 2; // Localize to the nearest fiducial in any waypoint.
FIDUCIAL_INIT_NEAREST_AT_TARGET =
3; // Localize to nearest fiducial at the target waypoint (from initial_guess).
FIDUCIAL_INIT_SPECIFIC = 4; // Localize to the given fiducial at the target waypoint (from
// initial_guess) if it exists, or any waypoint otherwise.
}
// Tells the initializer whether to use fiducials, and how to use them.
FiducialInit fiducial_init = 7;
// If using FIDUCIAL_INIT_SPECIFIC, this is the specific fiducial ID to use for initialization.
// If no detection of this fiducial exists, the service will return STATUS_NO_MATCHING_FIDUCIAL.
// If detections exist, but are low quality, STATUS_FIDUCIAL_TOO_FAR_AWAY, FIDUCIAL_TOO_OLD, or
// FIDUCIAL_POSE_UNCERTAIN will be returned.
int32 use_fiducial_id = 8;
// If true, consider how nearby localizations appear (like turned 180).
bool do_ambiguity_check = 10;
// If using FIDUCIAL_INIT_SPECIFIC and this is true, the initializer will only consider
// fiducial detections from the target waypoint (from initial_guess). Otherwise, if the
// target waypoint does not contain a good measurement of the desired fiducial, nearby waypoints
// may be used to infer the robot's location.
bool restrict_fiducial_detections_to_target_waypoint = 11;
oneof refinement {
// If true, and we are using fiducials during initialization, will run ICP after the
// fiducial was used for an initial guess.
bool refine_fiducial_result_with_icp = 9;
// Improve localization based on images from body cameras
VisualRefinementOptions refine_with_visual_features = 12;
}
}
// Info on whether the robot's current sensor setup is compatible with the recorded data
// in the map.
message SensorCompatibilityStatus {
// If true, the loaded map has LIDAR data in it.
bool map_has_lidar_data = 1;
// If true, the robot is currently configured to use LIDAR data.
bool robot_configured_for_lidar = 2;
}
// The SetLocalization response message contains the resulting localization to the map.
message SetLocalizationResponse {
// Common response header.
ResponseHeader header = 1;
// Result of using the lease.
LeaseUseResult lease_use_result = 2;
enum Status {
// The status is unknown/unset.
STATUS_UNKNOWN = 0;
// Localization success.
STATUS_OK = 1;
// Robot is experiencing a condition that prevents localization.
STATUS_ROBOT_IMPAIRED = 2;
// The given waypoint is unknown by the system.
// This could be due to a client error, or because the graph was changed out from under the
// client.
STATUS_UNKNOWN_WAYPOINT = 3;
// Localization was aborted, likely because of a new request.
STATUS_ABORTED = 4;
// Failed to localize for some other reason; see the error_report for details.
// This is often because the initial guess was incorrect.
STATUS_FAILED = 5;
// Failed to localize because the fiducial requested by 'use_fiducial_id' was too far away
// from the robot.
STATUS_FIDUCIAL_TOO_FAR_AWAY = 6;
// Failed to localize because the fiducial requested by 'use_fiducial_id' had a detection
// time that was too far in the past.
STATUS_FIDUCIAL_TOO_OLD = 7;
// Failed to localize because the fiducial requested by 'use_fiducial_id' did not exist in
// the map at the required location.
STATUS_NO_MATCHING_FIDUCIAL = 8;
// Failed to localize because the fiducial requested by 'use_fiducial_id' had an unreliable
// pose estimation, either in the current detection of that fiducial, or in detections that
// were saved in the map. Note that when using FIDUCIAL_INIT_SPECIFIC, fiducial detections
// at the target waypoint will be used so long as they are not uncertain -- otherwise,
// detections at adjacent waypoints may be used. If there exists no uncertain detection of
// the fiducial near the target waypoint in the map, the service returns this status.
STATUS_FIDUCIAL_POSE_UNCERTAIN = 9;
// The localization could not be set, because the map was recorded using a different sensor
// setup than the robot currently has onboard. See SensorStatus for more details.
STATUS_INCOMPATIBLE_SENSORS = 10;
// Visual feature based alignment failed or the pose solution was considered unreliable.
STATUS_VISUAL_ALIGNMENT_FAILED = 11;
// Failed to localize because there is no map loaded.
STATUS_NO_MAP_LOADED = 12;
}
// Return status for the request.
Status status = 3;
// If set, describes the reason the status is not OK.
string error_report = 4;
// Result of localization.
Localization localization = 5;
message SuspectedAmbiguity {
// Example of a potentially ambiguous localization near the
// result of the initialization.
SE3Pose alternate_robot_tform_waypoint = 1;
}
// Alternative information if the localization is ambiguous.
SuspectedAmbiguity suspected_ambiguity = 7;
// If the status is ROBOT_IMPAIRED, this is why the robot is impaired.
RobotImpairedState impaired_state = 8;
// This status determines whether the robot has compatible sensors for the
// map that was recorded. Note that if sensors aren't working, STATUS_IMPAIRED
// will be returned, rather than STATUS_INCOMPATIBLE_SENSORS.
SensorCompatibilityStatus sensor_status = 9;
enum QualityCheckResult {
// Unset. Note that the quality check is only performed if the overall Status
// enum returns STATUS_SUCCESS, and will be unset otherwise.
QUALITY_CHECK_UNKNOWN = 0;
// The quality check passed.
QUALITY_CHECK_SUCCESS = 1;
// After applying the localization, a poor point cloud match to the map was detected.
// This can happen if, for example, the map has changed, or the starting location
// of the robot is now very different than it was at recording time.
QUALITY_CHECK_POOR_POINT_CLOUD_MATCH = 2;
// After applying the localization, Graph Nav checked the localization, and found that
// the robot's gravity vector does not align with the map's. This can happen if a fiducial
// being used to align to the map was detected wrongly during recording, or if the robot's
// IMU is miscalibrated. It can also occur when the initial guess passed in to the
// SetLocalization RPC is in the incorrect reference frame.
QUALITY_CHECK_POOR_GRAVITY_ALIGNMENT = 3;
// There wasn't enough data to make a determination about quality.
QUALITY_CHECK_SKIPPED = 4;
// The prior passed in is too different from the expected height of the robot e.w.r.t the
// waypoint.
QUALITY_CHECK_BAD_HEIGHT = 5;
}
// Graph Nav will check the quality of the resulting localization and report the status
// here. Note that to preserve backwards compatability with 3.2 and earlier, a poor quality
// check does not result in this RPC failing.
QualityCheckResult quality_check_result = 10;
}
message RouteGenParams {
}
// Parameters describing how to travel along a route.
message TravelParams {
oneof cartesian_distance_parameters {
// Threshold for the maximum distance [meters] that defines a circular region for when we
// have reached the final waypoint.
double max_distance = 1;
// Oriented 2D bounding box [meters & radians] that defines an rectangular region for when
// we have reached the final waypoint.
bosdyn.api.OrientedBox2 box_region = 15;
}
// Threshold for the maximum yaw [radians] that defines when we have reached
// the final waypoint (ignored if ignore_final_yaw is set to true).
double max_yaw = 2;
// Speed the robot should use.
// Omit to let the robot choose.
SE2VelocityLimit velocity_limit = 3;
// If true, the robot will only try to achieve
// the final translation of the route. Otherwise,
// it will attempt to achieve the yaw as well.
bool ignore_final_yaw = 4;
// Max distance from the recorded edge that the robot is allowed to travel when avoiding
// obstacles or optimized its path. This is half of the full width of the corridor the robot may
// walk within. If this value is not set, the robot will choose a default corridor width.
double max_corridor_distance = 18;
// Indicates whether robot will navigate through areas with poor quality features
enum FeatureQualityTolerance {
TOLERANCE_UNKNOWN = 0; // Unknown value
TOLERANCE_DEFAULT =
1; // Navigate through default number of waypoints with poor quality features
TOLERANCE_IGNORE_POOR_FEATURE_QUALITY =
2; // Navigate through unlimited number of waypoints with poor quality features
}
FeatureQualityTolerance feature_quality_tolerance = 5;
// Disable directed exploration to skip blocked portions of route
bool disable_directed_exploration = 6;
// Disable alternate-route-finding; overrides the per-edge setting in the map.
bool disable_alternate_route_finding = 8;
// Path following mode
bosdyn.api.graph_nav.Edge.Annotations.PathFollowingMode path_following_mode = 9;
// Time to wait for blocked path to clear (seconds)
google.protobuf.Duration blocked_path_wait_time = 10;
// Ground clutter avoidance mode.
bosdyn.api.graph_nav.Edge.Annotations.GroundClutterAvoidanceMode ground_clutter_mode = 11;
// The strictness at which the lost detector will declare the robot lost during this command.
// Note that this level of strictness will continue to apply even if the robot is teleoperated
// by the user. Changing the level of strictness also resets the lost detector. So if the robot
// is lost, issuing a new command with a lower level of strictness will cause the robot to
// continue as if it were not lost. Issuing a command with LOST_DETECTOR_STRICTNESS_UNKNOWN
// results in no change.
LostDetectorStrictness lost_detector_strictness = 14;
// Determines which local path planner to use.
enum PathPlannerMode {
// Unknown value.
PLANNER_MODE_UNKNOWN = 0;
// Use default path planner. Currently this is the short range planner.
PLANNER_MODE_DEFAULT = 1;
// Use short range planner.
PLANNER_MODE_SHORT_RANGE = 2;
// Use long range planner. This is an experimental feature that helps the robot navigate
// around large obstacles that were not present during mission recording.
// NOTE: Alternate route-finding waypoints are disabled when using the long range planner.
PLANNER_MODE_LONG_RANGE = 3;
};
PathPlannerMode planner_mode = 17;
}
message ModifyNavigationResponse {
ResponseHeader header = 1;
// Results of using the various leases.
repeated LeaseUseResult lease_use_results = 2;
enum Status {
STATUS_UNKNOWN = 0;
STATUS_OK = 1; // Modify request was accepted.
STATUS_UNRECOGNIZED_COMMAND = 2; // The command ID wasn't the ID of the last command.
}
// Status code specific to the ModifyNavigationResponse.
Status status = 3;
}
// The NavigateToRequest can be used to command GraphNav to drive the robot to a specific waypoint.
// GraphNav will plan a path through the map which most efficiently gets the robot to the specified
// goal waypoint. Parameters are provided which influence how GraphNav will generate and follow the
// path. This RPC returns immediately after the request is processed. It does not block until
// GraphNav completes the path to the goal waypoint. The user is expected to periodically check the
// status of the NavigateTo command using the NavigationFeedbackRequest RPC.
message NavigateToRequest {
// Common request header.
RequestHeader header = 1;
// The Leases to show ownership of the robot and the graph.
repeated Lease leases = 2;
// ID of the waypoint to go to.
string destination_waypoint_id = 3;
// Preferences on how to pick the route.
RouteGenParams route_params = 4;
// Parameters that define how to traverse and end the route.
TravelParams travel_params = 5;
// The timestamp (in robot time) that the navigation command is valid until.
google.protobuf.Timestamp end_time = 6;
// Identifier provided by the time sync service to verify time sync between robot and client.
string clock_identifier = 7;
// If provided, graph_nav will move the robot to an SE2 pose relative to the waypoint.
// Note that the robot will treat this as a simple goto request. It will first arrive at the
// destination waypoint, and then travel in a straight line from the destination waypoint to the
// offset goal, attempting to avoid obstacles along the way.
SE2Pose destination_waypoint_tform_body_goal = 8;
// Unique identifier for the command. If 0, this is a new command, otherwise it is a
// continuation of an existing command. If this is a continuation of an existing command, all
// parameters will be ignored, and the old parameters will be preserved.
uint32 command_id = 9;
// Defines robot behavior when route is blocked. Defaults to reroute when route is blocked.
RouteFollowingParams.RouteBlockedBehavior route_blocked_behavior = 10;
}
// Response to a NavigateToRequest. This is returned immediately after the request is processed. A
// command_id is provided to specify the ID that the user may use to poll the system for feedback on
// the NavigateTo command.
message NavigateToResponse {
// Common response header.
ResponseHeader header = 1;
// Results of using the various leases.
repeated LeaseUseResult lease_use_results = 2;
enum Status {
// An unknown / unexpected error occurred.
STATUS_UNKNOWN = 0;
// Request was accepted.
STATUS_OK = 1;
// [Time error] Client has not done timesync with robot.
STATUS_NO_TIMESYNC = 2;
// [Time error] The command was received after its end time had already passed.
STATUS_EXPIRED = 3;
// [Time error]The command end time was too far in the future.
STATUS_TOO_DISTANT = 4;
// [Robot State Error] Cannot navigate a route if the robot has a critical
// perception fault, or behavior fault, or LIDAR not working.
STATUS_ROBOT_IMPAIRED = 5;
// [Robot State Error] Cannot navigate a route while recording a map.
STATUS_RECORDING = 6;
// [Route Error] One or more of the waypoints specified weren't in the map.
STATUS_UNKNOWN_WAYPOINT = 7;
// [Route Error] There is no path to the specified waypoint.
STATUS_NO_PATH = 8;
// [Route Error] Route contained too many waypoints with low-quality features.
STATUS_FEATURE_DESERT = 10;
// [Route Error] Happens when you try to issue a navigate to while the robot is lost.
STATUS_LOST = 11;
// [Route Error] Happens when the current localization doesn't refer to any waypoint in the
// map (possibly uninitialized localization).
STATUS_NOT_LOCALIZED_TO_MAP = 13;
// [Wrestling error] Happens when graph nav refuses to follow the route you specified.
STATUS_COULD_NOT_UPDATE_ROUTE = 12;
// [Route Error] Happens when you try to issue a navigate to while the robot is stuck.
// Navigate to a different waypoint, or clear the route and try again.
STATUS_STUCK = 14;
// [Request Error] Happens when you try to continue a command that was either expired, or
// had an unrecognized id.
STATUS_UNRECOGNIZED_COMMAND = 15;
// [Route Error] Happens when you try to navigate along a route and a needed callback is no
// longer available.
STATUS_AREA_CALLBACK_ERROR = 16;
}
// Return status for the request.
Status status = 3;
// If the status is ROBOT_IMPAIRED, this is why the robot is impaired.
RobotImpairedState impaired_state = 6;
// Unique identifier for the command, If 0, command was not accepted.
uint32 command_id = 4;
// On a relevant error status code, these fields contain the waypoint/edge IDs that caused the
// error.
repeated string error_waypoint_ids = 5;
// Errors about Area Callbacks in the map.
AreaCallbackServiceError area_callback_error = 10;
}
// These parameters are specific to how the robot follows a specified route in NavigateRoute.
message RouteFollowingParams {
// For each enum in this message, if UNKNOWN is passed in, we default to one of the values
// (indicated in the comments). Passing UNKNOWN is not considered a programming error.
// This setting applies when a new NavigateRoute command is issued (different route or
// final-waypoint-offset), and command_id indicates a new command.
enum StartRouteBehavior {
// The mode is unset.
START_UNKNOWN = 0;
// The robot will find the shortest path to the start of the route, possibly using
// edges that are not in the route. After going to the start, the robot will follow the
// route.
START_GOTO_START = 1;
// The robot will find the shortest path to any point on the route, and go to the point
// that gives that shortest path. Then, the robot will follow the rest of the route from
// that point.
// If multiple points on the route are similarly close to the robot, the robot will
// prefer the earliest on the route.
// This is the default.
START_GOTO_ROUTE = 2;
// The robot will fail the command with status STATUS_NOT_LOCALIZED_TO_ROUTE.
START_FAIL_WHEN_NOT_ON_ROUTE = 3;
}
// This setting applies when a NavigateRoute command is issued with the same route and
// final-waypoint-offset. It is not necessary that command_id indicate the same command.
// The expected waypoint is the last waypoint that GraphNav was autonomously navigating to.
enum ResumeBehavior {
// The mode is unset.
RESUME_UNKNOWN = 0;
// The robot will find the shortest path to any point on the route after the
// furthest-along traversed edge, and go to the point that gives that shortest path.
// Then, the robot will follow the rest of the route from that point.
// This is the default.
RESUME_RETURN_TO_UNFINISHED_ROUTE = 1;
// The robot will fail the command with status STATUS_NOT_LOCALIZED_TO_ROUTE.
RESUME_FAIL_WHEN_NOT_ON_ROUTE = 2;
}
// This setting applies when the robot discovers that the route is blocked.
enum RouteBlockedBehavior {
// The mode is unset.
ROUTE_BLOCKED_UNKNOWN = 0;
// For NavigateToRequests, the robot will find the shortest path to the destination
// that avoids the blockage.
// For NavigateRouteRequests, the robot will find the shortest path to any point
// after the furthest-along blockage, and after the furthest-along traversed edge,
// and go to the point that gives that shortest path. Then, the robot will follow
// the rest of the route from that point. If multiple points on the route are
// similarly close to the robot, the robot will prefer the earliest on the route.
// This is the default.
ROUTE_BLOCKED_REROUTE = 1;
// The robot will fail the command with status STATUS_STUCK;
ROUTE_BLOCKED_FAIL = 2;
}
StartRouteBehavior new_cmd_behavior = 1;
ResumeBehavior existing_cmd_behavior = 2;
RouteBlockedBehavior route_blocked_behavior = 3;
}
// A NavigateRoute request message specifies a route of waypoints/edges and parameters
// about how to get there. Like NavigateTo, this command returns immediately upon
// processing and provides a command_id that the user can use along with a NavigationFeedbackRequest
// RPC to poll the system for feedback on this command. The RPC does not block until the route is
// completed.
message NavigateRouteRequest {
// Common request header.
RequestHeader header = 1;
// The Lease to show ownership of the robot.
repeated Lease leases = 2;
// A route for the robot to follow.
Route route = 3;
// What should the robot do if it is not at the expected point in the route, or the route is
// blocked.
RouteFollowingParams route_follow_params = 9;
// How to travel the route.
TravelParams travel_params = 4;
// The timestamp (in robot time) that the navigation command is valid until.
google.protobuf.Timestamp end_time = 5;
// Identifier provided by the time sync service to verify time sync between robot and client.
string clock_identifier = 6;
// If provided, graph_nav will move the robot to an SE2 pose relative to the final waypoint
// in the route.
// Note that the robot will treat this as a simple goto request. It will first arrive at the
// destination waypoint, and then travel in a straight line from the destination waypoint to the
// offset goal, attempting to avoid obstacles along the way.
SE2Pose destination_waypoint_tform_body_goal = 7;
// Unique identifier for the command. If 0, this is a new command, otherwise it is a
// continuation of an existing command.
uint32 command_id = 8;
}
// Response to a NavigateRouteRequest. This is returned immediately after the request is processed.
// A command_id is provided to specify the ID that the user may use to poll the system for feedback
// on the NavigateRoute command.
message NavigateRouteResponse {
// Common response header.
ResponseHeader header = 1;
// Details about how the lease was used.
repeated LeaseUseResult lease_use_results = 2;
enum Status {
// An unknown / unexpected error occurred.
STATUS_UNKNOWN = 0;
// Request was accepted.
STATUS_OK = 1;
// [Time Error] Client has not done timesync with robot.
STATUS_NO_TIMESYNC = 2;
// [Time Error] The command was received after its end time had already passed.
STATUS_EXPIRED = 3;
// [Time Error] The command end time was too far in the future.
STATUS_TOO_DISTANT = 4;
// [Robot State Error] Cannot navigate a route if the robot has a critical
// perception fault, or behavior fault, or LIDAR not working.
STATUS_ROBOT_IMPAIRED = 5;
// [Robot State Error] Cannot navigate a route while recording a map.
STATUS_RECORDING = 6;
// [Route Error] One or more waypoints/edges are not in the map.
STATUS_UNKNOWN_ROUTE_ELEMENTS = 8;
// [Route Error] One or more edges do not connect to expected waypoints.
STATUS_INVALID_EDGE = 9;
// [Route Error] There is no path to the specified route.
STATUS_NO_PATH = 20;
// [Route Error] Route contained a constraint fault.
STATUS_CONSTRAINT_FAULT = 11;
// [Route Error] Route contained too many waypoints with low-quality features.
STATUS_FEATURE_DESERT = 13;
// [Route Error] Happens when you try to issue a navigate route while the robot is lost.
STATUS_LOST = 14;
// [Route Error] Happens when the current localization doesn't refer to any waypoint
// in the route (possibly uninitialized localization).
STATUS_NOT_LOCALIZED_TO_ROUTE = 16;
// [Route Error] Happens when the current localization doesn't refer to any waypoint in the
// map (possibly uninitialized localization).
STATUS_NOT_LOCALIZED_TO_MAP = 19;
// [Wrestling Errors] Happens when graph nav refuses to follow the route you specified. Try
// saying please?
STATUS_COULD_NOT_UPDATE_ROUTE = 15;
// [Route Error] Happens when you try to issue a navigate to while the robot is stuck.
// Navigate a different route, or clear the route and try again.
STATUS_STUCK = 17;
// [Request Error] Happens when you try to continue a command that was either expired, or
// had an unrecognized id.
STATUS_UNRECOGNIZED_COMMAND = 18;
// [Route Error] Happens when you try to navigate along a route and a needed callback is no
// longer available.
STATUS_AREA_CALLBACK_ERROR = 21;
}
// Return status for the request.
Status status = 3;
// If the status is ROBOT_IMPAIRED, this is why the robot is impaired.
RobotImpairedState impaired_state = 7;
// Unique identifier for the command, If 0, command was not accepted.
uint32 command_id = 4;
// On a relevant error status code, these fields contain the waypoint/edge IDs that caused the
// error.
repeated string error_waypoint_ids = 5;
// On a relevant error status code (STATUS_INVALID_EDGE), this is populated with the edge ID's
// that cased the error.
repeated Edge.Id error_edge_ids = 6;
// Errors about Area Callbacks in the map.
AreaCallbackServiceError area_callback_error = 8;
}
// Parameters controlling how the robot will navigate to a GPS coordinate.
message GPSNavigationParams {
// The goal position as latitude/longitude. Height is ignored.
bosdyn.api.gps.LLH goal_llh = 1;
// Counter-clockwise rotation in radians around the "up" axis that the robot will try to achieve
// at the goal. This is a bearing around the "up" axis such that East points to zero yaw, and
// West is pi radians yaw. If not provided, the robot will try to achieve any allowable
// orientation at the goal.
google.protobuf.DoubleValue goal_yaw = 2;
// The maximum distance we are willing to accept for the LLH coordinate from the mapped data in
// meters. This is a 2 dimensional measurement (height is not considered). If not filled out,
// Spot will decide based on internal defaults.
google.protobuf.DoubleValue max_distance_from_map = 3;
}
// The NavigateToAnchorRequest can be used to command GraphNav to drive the robot to a specific
// place in an anchoring. GraphNav will find the waypoint that has the shortest path length from
// robot's current position but is still close to the goal. GraphNav will plan a path through the
// map which most efficiently gets the robot to the goal waypoint, and will then travel
// in a straight line from the destination waypoint to the offset goal, attempting to avoid
// obstacles along the way.
// Parameters are provided which influence how GraphNav will generate and follow the path.
// This RPC returns immediately after the request is processed. It does not block until GraphNav
// completes the path to the goal waypoint. The user is expected to periodically check the status
// of the NavigateToAnchor command using the NavigationFeedbackRequest RPC.
message NavigateToAnchorRequest {
// Common request header.
RequestHeader header = 1;
// The Leases to show ownership of the robot and the graph.
repeated Lease leases = 2;
// Users may either choose seed_tform_goal or gps_navigation_params as a goal.
oneof goal {
// The goal, expressed with respect to the seed frame of the current anchoring.
// The robot will use the z value to find the goal waypoint, but the final z height the
// robot achieves will depend on the terrain height at the offset from the goal.
SE3Pose seed_tform_goal = 3;
// If given, the NavigateToAnchor request will instead be interpreted as a command to
// navigate to GPS coordinates. When given, the seed_tform_goal will be ignored,and instead
// the parameters in this message will be used. Otherwise, the NavigateToAnchorRequest works
// identically to when seed_tform_goal is used instead. For example, the TravelParams will
// be respected for this kind of goal.
GPSNavigationParams gps_navigation_params = 11;
}
// These parameters control selection of the goal waypoint. In seed frame, they are the x, y,
// and z tolerances with respect to the goal pose within which waypoints will be considered.
// If these values are negative, or too small, reasonable defaults will be used.
Vec3 goal_waypoint_rt_seed_ewrt_seed_tolerance = 4;
// Preferences on how to pick the route.
RouteGenParams route_params = 6;
// Parameters that define how to traverse and end the route.
TravelParams travel_params = 7;
// The timestamp (in robot time) that the navigation command is valid until.
google.protobuf.Timestamp end_time = 8;
// Identifier provided by the time sync service to verify time sync between robot and client.
string clock_identifier = 9;
// Unique identifier for the command. If 0, this is a new command, otherwise it is a
// continuation of an existing command. If this is a continuation of an existing command, all
// parameters will be ignored, and the old parameters will be preserved.
uint32 command_id = 10;
}
// Response to a NavigateToAnchorRequest. This is returned immediately after the request is
// processed. A command_id is provided to specify the ID that the user may use to poll the system
// for feedback on the NavigateTo command.
message NavigateToAnchorResponse {
// Common response header.
ResponseHeader header = 1;
// Results of using the various leases.
repeated LeaseUseResult lease_use_results = 2;
enum Status {
// An unknown / unexpected error occurred.
STATUS_UNKNOWN = 0;
// Request was accepted.
STATUS_OK = 1;
// [Time error] Client has not done timesync with robot.
STATUS_NO_TIMESYNC = 2;
// [Time error] The command was received after its end time had already passed.
STATUS_EXPIRED = 3;
// [Time error]The command end time was too far in the future.
STATUS_TOO_DISTANT = 4;
// [Robot State Error] Cannot navigate a route if the robot has a critical
// perception fault, or behavior fault, or LIDAR not working.
STATUS_ROBOT_IMPAIRED = 5;
// [Robot State Error] Cannot navigate a route while recording a map.
STATUS_RECORDING = 6;
// [Route Error] There is no anchoring.
STATUS_NO_ANCHORING = 7;
// [Route Error] There is no path to a waypoint near the specified goal.
// If any waypoints were found (but no path), the error_waypoint_ids field
// will be filled.
STATUS_NO_PATH = 8;
// [Route Error] Route contained too many waypoints with low-quality features.
STATUS_FEATURE_DESERT = 10;
// [Route Error] Happens when you try to issue a navigate to while the robot is lost.
STATUS_LOST = 11;
// [Route Error] Happens when the current localization doesn't refer to any waypoint in the
// map (possibly uninitialized localization).
STATUS_NOT_LOCALIZED_TO_MAP = 13;
// [Wrestling error] Happens when graph nav refuses to follow the route you specified.
STATUS_COULD_NOT_UPDATE_ROUTE = 12;
// [Route Error] Happens when you try to issue a navigate to while the robot is stuck.
// Navigate to a different waypoint, or clear the route and try again.
STATUS_STUCK = 14;
// [Request Error] Happens when you try to continue a command that was either expired, or
// had an unrecognized id.
STATUS_UNRECOGNIZED_COMMAND = 15;
// [Route Error] The pose is invalid, or known to be unachievable (upside-down, etc).
STATUS_INVALID_POSE = 16;
// [Route Error] Happens when you try to navigate along a route and a needed callback is no
// longer available.
STATUS_AREA_CALLBACK_ERROR = 17;
// [Route Error] The command contained an invalid GPS request. See gps_status for more
// details.
STATUS_INVALID_GPS_COMMAND = 18;
}
// Return status for the request.
Status status = 3;
// If the status is ROBOT_IMPAIRED, this is why the robot is impaired.
RobotImpairedState impaired_state = 6;
// Unique identifier for the command, If 0, command was not accepted.
uint32 command_id = 4;
// On a relevant error status code, these fields contain the waypoint/edge IDs that caused the
// error.
repeated string error_waypoint_ids = 5;
// Errors about Area Callbacks in the map.
AreaCallbackServiceError area_callback_error = 7;
// The result of the GPS command, if relevant.
enum GPSStatus {
// An unknown/unexpected error occurred.
GPS_STATUS_UNKNOWN = 0;
// The GPS command was valid and can be executed.
GPS_STATUS_OK = 1;
// The GPS command could not be completed because the map does not have GPS coordinates,
// neither in waypoint annotations nor in the waypoint snapshot data. Please record the map
// using a GPS-enabled robot, or annotate the waypoints with custom GPS coordinates.
GPS_STATUS_NO_COORDS_IN_MAP = 2;
// The GPS command could not be completed because the coordinates passed in were too far
// from any mapped GPS data.
GPS_STATUS_TOO_FAR_FROM_MAP = 3;
}
// Result of the GPS command.
GPSStatus gps_status = 8;
}
// The NavigationFeedback request message uses the command_id of a navigation request to get
// the robot's progress and current status for the command. Note that all commands return
// immediately after they are processed, and the robot will continue to execute the command
// asynchronously until it times out or completes. New commands override old ones.
message NavigationFeedbackRequest {
// Common request header.
RequestHeader header = 1;
// Unique identifier for the command, provided by nav command response.
// Omit to get feedback on currently executing command.
uint32 command_id = 2;
}
// The NavigationFeedback response message returns the robot's
// progress and current status for the command.
message NavigationFeedbackResponse {
// Common response header.
ResponseHeader header = 1;
enum Status {
// An unknown / unexpected error occurred.
STATUS_UNKNOWN = 0;
// The robot is currently, successfully following the route.
STATUS_FOLLOWING_ROUTE = 1;
// The robot has reached the final goal of the navigation request.
STATUS_REACHED_GOAL = 2;
// There's no route currently being navigated.
// This can happen if no command has been issued, or if the graph has been changed during
// navigation.
STATUS_NO_ROUTE = 3;
// Robot is not localized to a route.
STATUS_NO_LOCALIZATION = 4;
// Robot appears to be lost.
STATUS_LOST = 5;
// Robot appears stuck and unable to make progress, but can still navigate to other
// destinations. stuck_reason provides more details on the reason that caused the robot to
// become stuck.
STATUS_STUCK = 6;
// The command expired.
STATUS_COMMAND_TIMED_OUT = 7;
// Cannot navigate a route if the robot has a critical perception fault, or behavior fault,
// or LIDAR not working. See impaired_status for details.
STATUS_ROBOT_IMPAIRED = 8;
// The route constraints were not feasible.
STATUS_CONSTRAINT_FAULT = 11;
// The command was replaced by a new command
STATUS_COMMAND_OVERRIDDEN = 12;
// The localization or route changed mid-traverse.
STATUS_NOT_LOCALIZED_TO_ROUTE = 13;
// The lease is no longer valid.
STATUS_LEASE_ERROR = 14;
// An error occurred with an Area Callback in a way that graph nav was unable to recover
// from. Navigating to another location may also fail.
// Lease errors will be reported via STATUS_LEASE_ERROR instead.
STATUS_AREA_CALLBACK_ERROR = 15;
}
// Return status for the request.
Status status = 2;
// If the status is ROBOT_IMPAIRED, this is why the robot is impaired.
RobotImpairedState impaired_state = 6;
// If the status is AREA_CALLBACK_ERROR, this map will be filled out with the error.
// The key of the map is the region id.
map<string, AreaCallbackError> area_callback_errors = 9;
// Remaining part of current route.
Route remaining_route = 3;
// The completed route taken for this command. Intended primarily for visualization
// of route progress.
// Do not use this to determine if the route is finished, as under certain conditions
// edges can be reported as completed before the route is done. Check the status for
// STATUS_REACHED_GOAL to determine if the robot has finished the route.
CompletedRoute completed_route = 18;
// Estimated length of remaining route.
double remaining_route_length = 17;
// ID of the command this feedback corresponds to.
uint32 command_id = 4;
// The most recent transform describing the robot's pose relative to the navigation goal.
SE3Pose last_ko_tform_goal = 5;
// Indicates whether the robot's body is currently in motion.
SE2TrajectoryCommand.Feedback.BodyMovementStatus body_movement_status = 7;
// Path following mode
bosdyn.api.graph_nav.Edge.Annotations.PathFollowingMode path_following_mode = 8;
// Map of Region IDs with relevant information
map<string, ActiveRegionInformation> active_region_information = 10;
// Data for a Area Callback region
message ActiveRegionInformation {
// human readable name for the region
string description = 1;
// service name for the Area Callback region
string service_name = 2;
// Status of the Area Callback region
AreaCallbackStatus region_status = 3;
enum AreaCallbackStatus {
STATUS_UNKNOWN = 0;
// The robot is navigating the Area Callback region
STATUS_NAVIGATING = 1;
// The robot is waiting for assistance to navigate through the Area Callback region
STATUS_WAITING = 2;
// The Area Callback service is in control of the robot
STATUS_CALLBACK_IN_CONTROL = 3;
}
}
// When the robot is following a route (and Status is STATUS_FOLLOWING_ROUTE), this gives
// additional detail about what the robot is doing to follow that route. When Status is not
// STATUS_FOLLOWING_ROUTE, this will be set to ROUTE_FOLLOWING_STATUS_UNKNOWN.
enum RouteFollowingStatus {
ROUTE_FOLLOWING_STATUS_UNKNOWN = 0;
// The robot is following the nominal path to the goal, either from a request or
// from internal path planning.
ROUTE_FOLLOWING_STATUS_FOLLOWING_ROUTE = 1;
// The robot is trying to get back to the nominal path to the goal, either because
// it was not on the nominal path originally, or because it was moved away from the path.
ROUTE_FOLLOWING_STATUS_RETURNING_TO_ROUTE = 2;
// The robot is taking a different path through the map via edges/waypoints
// to get around a perceived obstacle. This might take it through a different part
// of the building.
ROUTE_FOLLOWING_STATUS_FOLLOWING_ALTERNATE_ROUTE = 3;
// The robot is walking to a different, nearby part of the map to find a path around
// a perceived blockage.
ROUTE_FOLLOWING_STATUS_EXPLORING = 4;
};
// Additional information about what kind of route the robot is following and why.
RouteFollowingStatus route_following_status = 1000;
// Indicates whether the robot thinks its current path is blocked by an obstacle. This will be
// set when Status is STATUS_FOLLOWING_ROUTE, or STATUS_STUCK, and will be
// BLOCKAGE_STATUS_UNKNOWN in all other cases.
enum BlockageStatus {
BLOCKAGE_STATUS_UNKNOWN = 0;
// The robot believes the path forward to be clear of obstacles.
BLOCKAGE_STATUS_ROUTE_CLEAR = 1;
// The robot believes there is an obstacle in the path which it can't get around easily.
// It will attempt to get around the obstacle, and if all else fails it will declare itself
// stuck.
BLOCKAGE_STATUS_ROUTE_BLOCKED_TEMPORARILY = 2;
// The robot has given up trying to get around perceived obstacles in the path and has
// declared itself stuck. This will only ever be set when Status is STATUS_STUCK.
BLOCKAGE_STATUS_STUCK = 3;
};
// Additional information about whether or not the robot believes the current route to be
// blocked.
BlockageStatus blockage_status = 1001;
// If status is STATUS_STUCK, this enum provides reasons differentiating various cases that
// can cause the robot to re
enum StuckReason {
STUCK_REASON_UNKNOWN = 0;
// The robot failed to find a way past an obstacle.
STUCK_REASON_OBSTACLE = 1;
// An area callback reported that it is blocked.
STUCK_REASON_AREA_CALLBACK_BLOCKED = 2;
// An area callback failed, but in a way that the robot is still able to navigate to
// other locations.
STUCK_REASON_AREA_CALLBACK_FAILED = 3;
// The robot has seen the goal and perceived that there is no pose it can navigate to to
// reach the goal. There is no value in rerouting.
STUCK_REASON_GOAL_BLOCKED = 4;
}
// Only filled out if status is STATUS_STUCK to provide additional context.
StuckReason stuck_reason = 11;
}
// The GetLocalizationState request message requests the current localization state and any other
// live data from the robot if desired. The localization consists of a waypoint ID and the relative
// pose of the robot with respect to that waypoint.
message GetLocalizationStateRequest {
// Common request header.
RequestHeader header = 1;
// Return the localization relative to this waypoint, if specified.
string waypoint_id = 8;
// If true, request the live edge-segmented point cloud that was used
// to generate this localization.
bool request_live_point_cloud = 2;
// If true, request the live images from realsense cameras at the time of
// localization.
bool request_live_images = 3;
// If true, request the live terrain maps at the time of localization.
bool request_live_terrain_maps = 4;
// If true, request the live world objects at the time of localization.
bool request_live_world_objects = 5;
// If true, requests the full live robot state at the time of localization.
bool request_live_robot_state = 6;
// If true, the smallest available encoding will be used for the live point cloud
// data. If false, three 32 bit floats will be used per point in the point cloud.
bool compress_live_point_cloud = 7;
// If true, request data about the robot's GPS localization.
bool request_gps_state = 9;
}
// Message describing the state of a remote point cloud service (such as a velodyne).
message RemotePointCloudStatus {
// The name of the point cloud service.
string service_name = 1;
// Boolean indicating if the point cloud service was registered in the robot's directory with