diff --git a/README.md b/README.md index 04ceb2b..17774b4 100644 --- a/README.md +++ b/README.md @@ -21,7 +21,7 @@ The official Spot SDK documentation also contains information relevant to the C+ - [Payload developer documentation](https://dev.bostondynamics.com/docs/payload/readme). Payloads add additional sensing, communication, and control capabilities beyond what the base platform provides. The Payload ICD covers the mechanical, electrical, and software interfaces that Spot supports. - [Spot API protocol definition](https://dev.bostondynamics.com/docs/protos/readme). This reference guide covers the details of the protocol applications used to communicate to Spot. Application developers who wish to use a language other than Python can implement clients that speak the protocol. -This is version 5.0.1 of the C++ SDK. Please review the [Release Notes](docs/cpp_release_notes.md) to see what has changed. +This is version 5.1.0 of the C++ SDK. Please review the [Release Notes](docs/cpp_release_notes.md) to see what has changed. ## Contents diff --git a/cpp/CMakeLists.txt b/cpp/CMakeLists.txt index d2b2352..3c807ce 100644 --- a/cpp/CMakeLists.txt +++ b/cpp/CMakeLists.txt @@ -7,7 +7,7 @@ # This file is autogenerated. cmake_minimum_required (VERSION 3.10.2) -project (bosdyn VERSION 5.0.1) +project (bosdyn VERSION 5.1.0) # Dependencies: find_package(Protobuf REQUIRED) diff --git a/cpp/bosdyn/client/data_buffer/signal_schema_key.h b/cpp/bosdyn/client/data_buffer/signal_schema_key.h index bfa21cb..b7829c4 100644 --- a/cpp/bosdyn/client/data_buffer/signal_schema_key.h +++ b/cpp/bosdyn/client/data_buffer/signal_schema_key.h @@ -9,6 +9,7 @@ #pragma once +#include #include namespace bosdyn { diff --git a/cpp/bosdyn/client/data_chunk/data_chunking.h b/cpp/bosdyn/client/data_chunk/data_chunking.h index a4a8d3b..8b231c8 100644 --- a/cpp/bosdyn/client/data_chunk/data_chunking.h +++ b/cpp/bosdyn/client/data_chunk/data_chunking.h @@ -9,6 +9,8 @@ #pragma once +#include "google/protobuf/io/coded_stream.h" +#include "google/protobuf/io/zero_copy_stream_impl_lite.h" #include "bosdyn/client/error_codes/sdk_error_code.h" #include "bosdyn/client/service_client/result.h" @@ -42,8 +44,12 @@ Result MessageFromDataChunks(const std::vector // This macro goes in some helper file. -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ns, ns_def, ns_def_parenthesis) \ - namespace std { \ - template <> \ - struct is_error_code_enum : public true_type {}; \ - } \ - ns_def std::error_code make_error_code(ns::enumclass); \ - ns_def_parenthesis +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ns) \ + namespace std { \ + template <> \ + struct is_error_code_enum<::ns::enumclass> : public true_type {}; \ + } \ + namespace ns { \ + std::error_code make_error_code(::ns::enumclass); \ + } // Define wrapper for macro above for each namespace, so it is easier to call the macro -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_API(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api, \ - namespace bosdyn{namespace api{ \ - , \ - }}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_AUTO_RETURN(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::auto_return, \ - namespace bosdyn{namespace api{namespace auto_return{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_AUTOWALK(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::autowalk, \ - namespace bosdyn{namespace api{namespace autowalk{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_KEEPALIVE(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::keepalive, \ - namespace bosdyn{namespace api{namespace keepalive{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_LOG_STATUS(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::log_status, \ - namespace bosdyn{namespace api{namespace log_status{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_SPOT(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::spot, \ - namespace bosdyn{namespace api{namespace spot{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_MISSION(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::mission, \ - namespace bosdyn{namespace api{namespace mission{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_GRAPHNAV(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::graph_nav, \ - namespace bosdyn{namespace api{namespace graph_nav{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_METRICS(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::metrics_logging, \ - namespace bosdyn{namespace api{namespace metrics_logging{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_INTERNAL(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::internal, \ - namespace bosdyn{namespace api{namespace internal{ \ - , \ - }}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_LOCALNAV(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER( \ - enumclass, ::bosdyn::api::internal::localnav, \ - namespace bosdyn{namespace api{namespace internal{namespace localnav{ \ - , \ - }}}}) - -#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_DOCKING(enumclass) \ - DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, ::bosdyn::api::docking, \ - namespace bosdyn{namespace api{namespace docking{ \ - , \ - }}}) +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_API(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_AUTO_RETURN(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::auto_return) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_AUTOWALK(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::autowalk) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_KEEPALIVE(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::keepalive) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_LOG_STATUS(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::log_status) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_SPOT(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::spot) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_MISSION(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::mission) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_GRAPHNAV(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::graph_nav) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_METRICS(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::metrics_logging) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_INTERNAL(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::internal) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_LOCALNAV(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::internal::localnav) + +#define DEFINE_PROTO_ENUM_ERRORCODE_HEADER_DOCKING(enumclass) \ + DEFINE_PROTO_ENUM_ERRORCODE_HEADER(enumclass, bosdyn::api::docking) diff --git a/cpp/bosdyn/client/error_codes/proto_enum_to_stderror_macro_source.h b/cpp/bosdyn/client/error_codes/proto_enum_to_stderror_macro_source.h index 012f3ef..15c628c 100644 --- a/cpp/bosdyn/client/error_codes/proto_enum_to_stderror_macro_source.h +++ b/cpp/bosdyn/client/error_codes/proto_enum_to_stderror_macro_source.h @@ -11,8 +11,7 @@ #include "bosdyn/common/success_condition.h" // This macro goes in some helper file. -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ns, ns_def, ns_def_parenthesis, \ - equivalent_condition) \ +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ns, equivalent_condition) \ namespace { \ struct enumclass##Category : public std::error_category { \ const char* name() const noexcept override { return #enumclass; } \ @@ -32,100 +31,49 @@ } \ const enumclass##Category enumclass##Category_category{}; \ } \ - ns_def std::error_code make_error_code(enumclass value) { \ + namespace ns { \ + std::error_code make_error_code(enumclass value) { \ return {static_cast(value), enumclass##Category_category}; \ } \ - ns_def_parenthesis + } // Define wrapper for macro above for each namespace, so it is easier to call the macro #define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_API(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api, \ - namespace bosdyn{namespace api{ \ - , \ - }}, \ - equivalent_condition) + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_AUTO_RETURN(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::auto_return, \ - namespace bosdyn{namespace api{namespace auto_return{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_AUTO_RETURN(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::auto_return, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_AUTOWALK(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::autowalk, \ - namespace bosdyn{namespace api{namespace autowalk{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_AUTOWALK(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::autowalk, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_KEEPALIVE(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::keepalive, \ - namespace bosdyn{namespace api{namespace keepalive{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_KEEPALIVE(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::keepalive, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_LOG_STATUS(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::log_status, \ - namespace bosdyn{namespace api{namespace log_status{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_LOG_STATUS(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::log_status, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_SPOT(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::spot, \ - namespace bosdyn{namespace api{namespace spot{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_SPOT(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::spot, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_MISSION(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::mission, \ - namespace bosdyn{namespace api{namespace mission{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_MISSION(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::mission, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_GRAPHNAV(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::graph_nav, \ - namespace bosdyn{namespace api{namespace graph_nav{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_GRAPHNAV(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::graph_nav, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_METRICS(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::metrics_logging, \ - namespace bosdyn{namespace api{namespace metrics_logging{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_METRICS(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::metrics_logging, equivalent_condition) -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_INTERNAL(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::internal, \ - namespace bosdyn{namespace api{namespace internal{ \ - , \ - }}}, \ - equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_INTERNAL(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::internal, equivalent_condition) #define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_LOCALNAV(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL( \ - enumclass, ::bosdyn::api::internal::localnav, \ - namespace bosdyn{namespace api{namespace internal{namespace localnav{ \ - , \ - }}}}, \ - equivalent_condition) - -#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_DOCKING(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::docking, \ - namespace bosdyn{namespace api{namespace docking{ \ - , \ - }}}, \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::internal::localnav, \ equivalent_condition) +#define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_DOCKING(enumclass, equivalent_condition) \ + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::docking, equivalent_condition) + #define DEFINE_PROTO_ENUM_ERRORCODE_IMPL_LONG_RANGE_MAP(enumclass, equivalent_condition) \ - DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, ::bosdyn::api::internal, \ - namespace bosdyn{namespace api{namespace internal{ \ - , \ - }}}, \ - equivalent_condition) + DEFINE_PROTO_ENUM_ERRORCODE_IMPL(enumclass, bosdyn::api::internal, equivalent_condition) diff --git a/cpp/bosdyn/client/graph_nav/graph_nav_client.cpp b/cpp/bosdyn/client/graph_nav/graph_nav_client.cpp index 307c6a1..348b4c2 100644 --- a/cpp/bosdyn/client/graph_nav/graph_nav_client.cpp +++ b/cpp/bosdyn/client/graph_nav/graph_nav_client.cpp @@ -548,6 +548,74 @@ void GraphNavClient::OnUploadEdgeSnapshotComplete( promise.set_value({ret_status, std::move(response)}); } +std::shared_future GraphNavClient::UploadSnapshotsAsync( + ::bosdyn::api::graph_nav::UploadSnapshotsRequest::Snapshots& input_request, + const RPCParameters& parameters) { + std::promise response; + std::shared_future future = response.get_future(); + BOSDYN_ASSERT_PRECONDITION(m_stub != nullptr, "Stub for service is unset!"); + + std::vector<::bosdyn::api::DataChunk> chunks; + std::vector<::bosdyn::api::graph_nav::UploadSnapshotsRequest> requests; + ::bosdyn::common::Status status = + MessageToDataChunks<::bosdyn::api::graph_nav::UploadSnapshotsRequest::Snapshots>( + input_request, &chunks); + if (!status) { + response.set_value({status, {}}); + return future; + } + + for (auto& chunk : chunks) { + ::bosdyn::api::graph_nav::UploadSnapshotsRequest request; + + auto request_chunk = request.mutable_chunk(); + *request_chunk = std::move(chunk); + + // For each request, apply the lease processor to automatically include a lease. + auto lease_status = ProcessRequestWithLease(&request, m_lease_wallet.get(), + ::bosdyn::client::kBodyResource); + if (!lease_status) { + // Failed to set a lease with the lease wallet. Return early since the request will fail + // without a lease. + response.set_value({lease_status, {}}); + return future; + } + + requests.emplace_back(std::move(request)); + } + + MessagePumpCallBase* one_time = + InitiateRequestStreamAsyncCall<::bosdyn::api::graph_nav::UploadSnapshotsRequest, + ::bosdyn::api::graph_nav::UploadSnapshotsResponse, + ::bosdyn::api::graph_nav::UploadSnapshotsResponse>( + std::move(requests), + std::bind( + &::bosdyn::api::graph_nav::GraphNavService::StubInterface::AsyncUploadSnapshots, + m_stub.get(), _1, _2, _3, _4), + std::bind(&GraphNavClient::OnUploadSnapshotsComplete, this, _1, _2, _3, _4, _5), + std::move(response), parameters); + + return future; +} + +UploadSnapshotsResultType GraphNavClient::UploadSnapshots( + ::bosdyn::api::graph_nav::UploadSnapshotsRequest::Snapshots& input_request, + const RPCParameters& parameters) { + return UploadSnapshotsAsync(input_request, parameters).get(); +} + +void GraphNavClient::OnUploadSnapshotsComplete( + MessagePumpCallBase* call, + const std::vector<::bosdyn::api::graph_nav::UploadSnapshotsRequest>&& request, + ::bosdyn::api::graph_nav::UploadSnapshotsResponse&& response, const grpc::Status& status, + std::promise promise) { + ::bosdyn::common::Status ret_status = ProcessResponseWithLeaseAndGetFinalStatus< + ::bosdyn::api::graph_nav::UploadSnapshotsResponse>(status, response, SDKErrorCode::Success, + m_lease_wallet.get()); + + promise.set_value({ret_status, std::move(response)}); +} + std::shared_future GraphNavClient::DownloadWaypointSnapshotAsync( ::bosdyn::api::graph_nav::DownloadWaypointSnapshotRequest& request, diff --git a/cpp/bosdyn/client/graph_nav/graph_nav_client.h b/cpp/bosdyn/client/graph_nav/graph_nav_client.h index 3114748..f77fb0a 100644 --- a/cpp/bosdyn/client/graph_nav/graph_nav_client.h +++ b/cpp/bosdyn/client/graph_nav/graph_nav_client.h @@ -54,6 +54,9 @@ typedef Result<::bosdyn::api::graph_nav::UploadWaypointSnapshotResponse> // Return type for the UploadEdgeSnapshot method. typedef Result<::bosdyn::api::graph_nav::UploadEdgeSnapshotResponse> UploadEdgeSnapshotResultType; +// Return type for the UploadSnapshots method. +typedef Result<::bosdyn::api::graph_nav::UploadSnapshotsResponse> UploadSnapshotsResultType; + // Return type for the DownloadWaypointSnapshot method. typedef Result<::bosdyn::api::graph_nav::WaypointSnapshot> DownloadWaypointSnapshotResultType; @@ -177,6 +180,16 @@ class GraphNavClient : public ServiceClient { ::bosdyn::api::graph_nav::EdgeSnapshot& request, const RPCParameters& parameters = RPCParameters()); + // Asynchronous method to execute a UploadSnapshots request. + std::shared_future UploadSnapshotsAsync( + ::bosdyn::api::graph_nav::UploadSnapshotsRequest::Snapshots& request, + const RPCParameters& parameters = RPCParameters()); + + // Synchronous method to execute a UploadSnapshots request. + UploadSnapshotsResultType UploadSnapshots( + ::bosdyn::api::graph_nav::UploadSnapshotsRequest::Snapshots& request, + const RPCParameters& parameters = RPCParameters()); + // Asynchronous method to execute a DownloadWaypointSnapshot request. Only one response // message is returned with all the data chunks received from the service concatenated // together. @@ -335,6 +348,14 @@ class GraphNavClient : public ServiceClient { ::bosdyn::api::graph_nav::UploadEdgeSnapshotResponse&& response, const grpc::Status& status, std::promise promise); + // Callback that will return the UploadSnapshotsResponse message after UploadSnapshots rpc + // returns to the client. + void OnUploadSnapshotsComplete( + MessagePumpCallBase* call, + const std::vector<::bosdyn::api::graph_nav::UploadSnapshotsRequest>&& request, + ::bosdyn::api::graph_nav::UploadSnapshotsResponse&& response, const grpc::Status& status, + std::promise promise); + // Callback that will return the DownloadWaypointSnapshotResponse message after // DownloadWaypointSnapshot rpc returns to the client. void OnDownloadWaypointSnapshotComplete( diff --git a/cpp/bosdyn/client/robot/robot.cpp b/cpp/bosdyn/client/robot/robot.cpp index a315b1f..8860e44 100644 --- a/cpp/bosdyn/client/robot/robot.cpp +++ b/cpp/bosdyn/client/robot/robot.cpp @@ -166,14 +166,17 @@ Result> Robot::EnsureChannel( std::map::const_iterator endpoint_iter = m_bootstrap_endpoints_by_name.find(service_name); if (endpoint_iter == m_bootstrap_endpoints_by_name.end()) { + std::unique_lock lock(m_authorities_and_endpoints_mutex); endpoint_iter = m_endpoints_by_name.find(service_name); if (endpoint_iter == m_endpoints_by_name.end()) { // Sync with Directory to get the endpoints. + lock.unlock(); Result> services_result = ListServices(); if (!services_result.status) { return {services_result.status, nullptr}; } + lock.lock(); endpoint_iter = m_endpoints_by_name.find(service_name); if (endpoint_iter == m_endpoints_by_name.end()) { return {::bosdyn::common::Status(ClientCreationErrorCode::UnregisteredService, @@ -199,14 +202,17 @@ Result> Robot::EnsureChannel( std::map::const_iterator authority_iter = m_bootstrap_authorities_by_name.find(service_name); if (authority_iter == m_bootstrap_authorities_by_name.end()) { + std::unique_lock lock(m_authorities_and_endpoints_mutex); authority_iter = m_authorities_by_name.find(service_name); if (authority_iter == m_authorities_by_name.end()) { // Sync with Directory to get the authorities. + lock.unlock(); Result> services_result = ListServices(); if (!services_result.status) { return {services_result.status, nullptr}; } + lock.lock(); authority_iter = m_authorities_by_name.find(service_name); if (authority_iter == m_authorities_by_name.end()) { return {::bosdyn::common::Status(ClientCreationErrorCode::UnregisteredService, @@ -331,10 +337,13 @@ Result> Robot::UpdateInformationFromLis return {result.status, {}}; } - m_authorities_by_name.clear(); - for (const auto& entry : result.response.service_entries()) { - service_list.push_back(entry); - m_authorities_by_name[entry.name()] = entry.authority(); + { + std::lock_guard lock(m_authorities_and_endpoints_mutex); + m_authorities_by_name.clear(); + for (const auto& entry : result.response.service_entries()) { + service_list.push_back(entry); + m_authorities_by_name[entry.name()] = entry.authority(); + } } return {::bosdyn::common::Status(SDKErrorCode::Success), std::move(service_list)}; diff --git a/cpp/bosdyn/client/robot/robot.h b/cpp/bosdyn/client/robot/robot.h index 3c80886..224dc6c 100644 --- a/cpp/bosdyn/client/robot/robot.h +++ b/cpp/bosdyn/client/robot/robot.h @@ -352,6 +352,9 @@ class Robot { // name and value represents the authority. const std::map m_bootstrap_authorities_by_name; + // Protect access to the authorities and endpoints maps. + std::mutex m_authorities_and_endpoints_mutex; + // Map to store authorities for the services as received from the Directory service. Key in the // map represents the service name and value represents the authority. std::map m_authorities_by_name; diff --git a/cpp/bosdyn/common/common_header_handling.cpp b/cpp/bosdyn/common/common_header_handling.cpp index 6965feb..95eca39 100644 --- a/cpp/bosdyn/common/common_header_handling.cpp +++ b/cpp/bosdyn/common/common_header_handling.cpp @@ -37,7 +37,7 @@ void PrepareResponseHeader(const ::bosdyn::api::RequestHeader& request_header, void StripLargeByteFields(::google::protobuf::Message* proto_message) { // Operates on Request and Response messages. - const std::string message_type = proto_message->GetTypeName(); + const std::string& message_type = proto_message->GetTypeName(); const auto& whitelist_map = ::bosdyn::common::strip_messages::kWhitelistedBytesFieldsMap; auto strip_message_func = whitelist_map.find(message_type); if (strip_message_func != whitelist_map.end()) { diff --git a/cpp/bosdyn/common/string_util.cpp b/cpp/bosdyn/common/string_util.cpp index fa29b58..5d1145b 100644 --- a/cpp/bosdyn/common/string_util.cpp +++ b/cpp/bosdyn/common/string_util.cpp @@ -9,6 +9,7 @@ #include "string_util.h" +#include #include namespace bosdyn { @@ -32,6 +33,7 @@ std::string RemoveSuffix(const std::string& str, const std::string& suffix) { return EndsWith(str, suffix) ? str.substr(0, str.length() - suffix.length()) : str; } + } // namespace common } // namespace bosdyn diff --git a/cpp/bosdyn/common/string_util.h b/cpp/bosdyn/common/string_util.h index f95fba4..1646615 100644 --- a/cpp/bosdyn/common/string_util.h +++ b/cpp/bosdyn/common/string_util.h @@ -24,6 +24,7 @@ bool StartsWith(const std::string& str, const std::string& start); /// If str ends with suffix, return the part preceding, otherwise the original value of str. std::string RemoveSuffix(const std::string& str, const std::string& suffix); + } // namespace common } // namespace bosdyn diff --git a/cpp/bosdyn/common/strip_bytes_fields.cpp b/cpp/bosdyn/common/strip_bytes_fields.cpp index 1b50034..4cb0809 100644 --- a/cpp/bosdyn/common/strip_bytes_fields.cpp +++ b/cpp/bosdyn/common/strip_bytes_fields.cpp @@ -24,6 +24,7 @@ std::map kWhitelistedBytesFie {"bosdyn.api.GetPointCloudResponse", StripPointCloudResponse}, {"bosdyn.api.graph_nav.UploadWaypointSnapshotRequest", StripUploadWaypointRequest}, {"bosdyn.api.graph_nav.UploadEdgeSnapshotRequest", StripUploadEdgeRequest}, + {"bosdyn.api.graph_nav.UploadSnapshotsRequest", StripUploadSnapshotsRequest}, {"bosdyn.api.graph_nav.DownloadWaypointSnapshotResponse", StripDownloadWaypointResponse}, {"bosdyn.api.graph_nav.DownloadEdgeSnapshotResponse", StripDownloadEdgeResponse}, {"bosdyn.api.RecordDataBlobsRequest", StripRecordDataBlobsRequest}, @@ -84,6 +85,15 @@ bool StripUploadEdgeRequest(::google::protobuf::Message* proto_message) { return false; } +bool StripUploadSnapshotsRequest(::google::protobuf::Message* proto_message) { + if (auto* upload_msg = + dynamic_cast<::bosdyn::api::graph_nav::UploadSnapshotsRequest*>(proto_message)) { + upload_msg->mutable_chunk()->clear_data(); + return true; + } + return false; +} + bool StripDownloadWaypointResponse(::google::protobuf::Message* proto_message) { if (auto* download_msg = dynamic_cast<::bosdyn::api::graph_nav::DownloadWaypointSnapshotResponse*>( diff --git a/cpp/bosdyn/common/strip_bytes_fields.h b/cpp/bosdyn/common/strip_bytes_fields.h index 21a7714..c407784 100644 --- a/cpp/bosdyn/common/strip_bytes_fields.h +++ b/cpp/bosdyn/common/strip_bytes_fields.h @@ -39,6 +39,7 @@ bool StripLocalGridResponse(::google::protobuf::Message* proto_message); bool StripPointCloudResponse(::google::protobuf::Message* proto_message); bool StripUploadWaypointRequest(::google::protobuf::Message* proto_message); bool StripUploadEdgeRequest(::google::protobuf::Message* proto_message); +bool StripUploadSnapshotsRequest(::google::protobuf::Message* proto_message); bool StripDownloadWaypointResponse(::google::protobuf::Message* proto_message); bool StripDownloadEdgeResponse(::google::protobuf::Message* proto_message); bool StripRecordDataBlobsRequest(::google::protobuf::Message* proto_message); diff --git a/cpp/bosdyn/common/time.cpp b/cpp/bosdyn/common/time.cpp index d7c11fb..1675a37 100644 --- a/cpp/bosdyn/common/time.cpp +++ b/cpp/bosdyn/common/time.cpp @@ -12,7 +12,11 @@ #include +#include #include +#include +#include +#include #include #include "bosdyn/common/numbers.h" @@ -28,18 +32,29 @@ ::std::chrono::nanoseconds _default_clock_fn() { ::std::chrono::system_clock::now().time_since_epoch()); } -ClockFn _clock_fn = _default_clock_fn; +std::shared_ptr& clock_fn() { + // Using a static _local_ variable here avoids the static initialization order fiasco. + // See https://isocpp.org/wiki/faq/ctors#static-init-order-on-first-use. + static auto ret = std::make_shared(_default_clock_fn); + return ret; +} } // namespace using TimeUtil = google::protobuf::util::TimeUtil; /// Set a clock function which overrides default functionality of NowNsec. -void SetClock(const ClockFn& fn) { _clock_fn = fn; } +void SetClock(const ClockFn& fn) { + auto fn_shared = std::make_shared(fn); + std::atomic_store_explicit(&clock_fn(), fn_shared, std::memory_order_release); +} -void RestoreDefaultClock() { _clock_fn = _default_clock_fn; } +void RestoreDefaultClock() { SetClock(_default_clock_fn); } -std::chrono::nanoseconds NsecSinceEpoch() { return _clock_fn(); } +std::chrono::nanoseconds NsecSinceEpoch() { + auto fn_shared = std::atomic_load_explicit(&clock_fn(), std::memory_order_acquire); + return (*fn_shared)(); +} std::chrono::seconds SecSinceEpoch() { return ::std::chrono::duration_cast<::std::chrono::seconds>(NsecSinceEpoch()); diff --git a/cpp/bosdyn/math/proto_math.cpp b/cpp/bosdyn/math/proto_math.cpp index ba305aa..cc3567b 100644 --- a/cpp/bosdyn/math/proto_math.cpp +++ b/cpp/bosdyn/math/proto_math.cpp @@ -329,6 +329,13 @@ bool Equals(const ::bosdyn::api::SE3Pose& a_T_b, const ::bosdyn::api::SE3Pose& b return (a_eq_b || a_eq_negative_b); } +Eigen::Isometry2d EigenFromApiProto(const ::bosdyn::api::SE2Pose& a_T_b) { + Eigen::Matrix ret = Eigen::Matrix::Identity(); + ret.block<2, 2>(0, 0) = ToMatrix(a_T_b.angle()); + ret.block<2, 1>(0, 2) = EigenFromApiProto(a_T_b.position()); + return Eigen::Isometry2d(ret); +} + Eigen::Isometry3d EigenFromApiProto(const ::bosdyn::api::SE3Pose& a_T_b) { Eigen::Matrix ret = Eigen::Matrix::Identity(); ret.block<3, 3>(0, 0) = EigenFromApiProto(a_T_b.rotation()).toRotationMatrix(); diff --git a/cpp/bosdyn/math/proto_math.h b/cpp/bosdyn/math/proto_math.h index 4910da9..a98727a 100644 --- a/cpp/bosdyn/math/proto_math.h +++ b/cpp/bosdyn/math/proto_math.h @@ -84,6 +84,7 @@ ::bosdyn::api::SE2Pose operator*(const ::bosdyn::api::SE2Pose& a_T_b, const ::bosdyn::api::SE2Pose& b_T_c); bool operator==(const ::bosdyn::api::SE2Pose& a_T_b, const ::bosdyn::api::SE2Pose& b_T_c); ::bosdyn::api::Vec2 operator*(const ::bosdyn::api::SE2Pose& a_T_b, const ::bosdyn::api::Vec2& p); +Eigen::Isometry2d EigenFromApiProto(const ::bosdyn::api::SE2Pose& a_T_b); // SE(2) Velocity Helpers bool operator==(const ::bosdyn::api::SE2Velocity& vel_a, const ::bosdyn::api::SE2Velocity& vel_b); diff --git a/docs/cpp_release_notes.md b/docs/cpp_release_notes.md index 0bc9cef..b4605c3 100644 --- a/docs/cpp_release_notes.md +++ b/docs/cpp_release_notes.md @@ -8,6 +8,38 @@ Development Kit License (20191101-BDSDK-SL). # Spot C++ SDK Release Notes +## Spot C++ SDK version 5.1.0 BETA + +### Bug Fixes and Improvements + +#### API + +Please see the [General Release Notes](https://dev.bostondynamics.com/docs/release_notes) for a description of the API changes included in release 5.1.0. + +#### SDK + +**Clients** + +- Fixed a potential segmentation fault when calling `ListServices()` on a `Robot` instance from multiple threads. Previously, while client creation and client objects were thread-safe, the internal updates to the robot's maps of authorities and endpoints were not. This could result in a crash if two threads called `ListServices()` concurrently. + +- Added `UploadSnapshots` and `UploadSnapshotsAsync` methods to [GraphNavClient](../cpp/bosdyn/client/graph_nav/graph_nav_client.cpp). + +**Helper Functions** + +- Added `EigenFromApiProto(const ::bosdyn::api::SE2Pose&)` to [proto_math.cpp](../cpp/bosdyn/math/proto_math.cpp). This function converts a `::bosdyn::api::SE2Pose` proto message into an `Eigen::Isometry2d`, constructing the corresponding 2D transformation matrix from the proto's angle and position fields. + +## Spot C++ SDK version 5.0.1.2 BETA + +There have been no changes in the C++ SDK between 5.0.1.1 and this version. + +Please see the [General Release Notes](https://dev.bostondynamics.com/docs/release_notes) for a description of the API changes included in this release. + +## Spot C++ SDK version 5.0.1.1 BETA + +There have been no changes in the C++ SDK between 5.0.1 and this version. + +Please see the [General Release Notes](https://dev.bostondynamics.com/docs/release_notes) for a description of the API changes included in this release. + ## Spot C++ SDK version 5.0.1 BETA ### Bug Fixes and Improvements diff --git a/protos/bosdyn/api/audio_visual.proto b/protos/bosdyn/api/audio_visual.proto index 0a3d602..6b38c97 100644 --- a/protos/bosdyn/api/audio_visual.proto +++ b/protos/bosdyn/api/audio_visual.proto @@ -294,6 +294,64 @@ message StopBehaviorResponse { Status status = 2; } +/* ADD BEHAVIOR */ +// Before you consider adding your own AV behaviors, please consider that +// the ‘solid lights on’ behavior can overheat the AV board if you leave +// it on for too long with certain RGB values. +message AddOrModifyBehaviorRequest { + RequestHeader header = 1; + // New behavior's name. + string name = 2; + // Behavior to add. + AudioVisualBehavior behavior = 3; +} + +message AddOrModifyBehaviorResponse { + enum Status { + STATUS_UNKNOWN = 0; + // The behavior was added successfully. + STATUS_SUCCESS = 1; + // A permanent behavior cannot be modified. + STATUS_MODIFY_PERMANENT = 2; + // The request contained an invalid field. + STATUS_INVALID = 3; + } + + ResponseHeader header = 1; + Status status = 2; + + repeated string deprecated_invalid_fields = 3 [deprecated = true]; + + // Human readable error message. Only set for STATUS_INVALID. + string error_message = 5; + + // Only for STATUS_SUCCESS, specifies the resulting LiveAudioVisualBehavior object. + LiveAudioVisualBehavior live_behavior = 4; +} + +/* DELETE BEHAVIOR */ +message DeleteBehaviorsRequest { + RequestHeader header = 1; + // Behavior names to remove. + repeated string behavior_names = 2; +} + +message DeleteBehaviorsResponse { + enum Status { + STATUS_UNKNOWN = 0; + // Behaviors were deleted successfully. + STATUS_SUCCESS = 1; + // A specified behavior name was not present in the system. + STATUS_DOES_NOT_EXIST = 2; + // A permanent behavior cannot be deleted. + STATUS_DELETE_PERMANENT = 3; + } + + ResponseHeader header = 1; + Status status = 2; + repeated LiveAudioVisualBehavior deleted_behaviors = 3; +} + /* LIST BEHAVIORS */ message ListBehaviorsRequest { RequestHeader header = 1; diff --git a/protos/bosdyn/api/audio_visual_service.proto b/protos/bosdyn/api/audio_visual_service.proto index bbf43c9..1f4b119 100644 --- a/protos/bosdyn/api/audio_visual_service.proto +++ b/protos/bosdyn/api/audio_visual_service.proto @@ -20,6 +20,14 @@ service AudioVisualService { // Stop an AudioVisualBehavior. rpc StopBehavior(StopBehaviorRequest) returns (StopBehaviorResponse) {} + // Add a new or modify an existing AudioVisualBehavior. + // Before you consider adding your own AV behaviors, please consider that + // the ‘solid lights on’ behavior can overheat the AV board if you leave + // it on for too long with certain RGB values. + rpc AddOrModifyBehavior(AddOrModifyBehaviorRequest) returns (AddOrModifyBehaviorResponse) {} + + // Delete one or more AudioVisualBehaviors. + rpc DeleteBehaviors(DeleteBehaviorsRequest) returns (DeleteBehaviorsResponse) {} // List all AudioVisualBehaviors currently added to the AudioVisual Service. rpc ListBehaviors(ListBehaviorsRequest) returns (ListBehaviorsResponse) {} diff --git a/protos/bosdyn/api/autowalk/walks.proto b/protos/bosdyn/api/autowalk/walks.proto index 8677ff5..d7dd54d 100644 --- a/protos/bosdyn/api/autowalk/walks.proto +++ b/protos/bosdyn/api/autowalk/walks.proto @@ -13,6 +13,7 @@ option java_outer_classname = "WalksProto"; import "google/protobuf/duration.proto"; +import "google/protobuf/wrappers.proto"; import "bosdyn/api/mission/nodes.proto"; import "bosdyn/api/mission/util.proto"; import "bosdyn/api/data_acquisition.proto"; @@ -59,6 +60,16 @@ message Walk { // walk). ChoreographyItems choreography_items = 9; + // Interrupts can be used to interrupt the normal flow of an autowalk. + // This could be useful for: + // - Getting the robot to power off when an onboard sensor determines an + // unsafe condition. + // - Having an external system that's internet connected inform the robot + // it's going to rain, so that the robot can return to the dock early. + // - Temporarily pausing the mission to get a closer look at a detected + // anomaly. + WalkInterrupt interrupts = 10; + } // These parameters apply to the entire autowalk. @@ -313,6 +324,10 @@ message Target { TARGET_STOW_BEHAVIOR_ALWAYS = 3; } TargetStowBehavior target_stow_behavior = 5; + + // If true, the robot is allowed to continue to perform the action + // Before it comes to a complete stop. + bool success_when_goal_area_reached = 6; } // An Action is what the robot should do at a location. For example, the user @@ -333,6 +348,8 @@ message Action { // element name. AcquireDataRequest acquire_data_request = 1; bosdyn.api.mission.DataAcquisition.CompletionBehavior completion_behavior = 2; + + // Last known Data Acquisition capabilities. AcquisitionCapabilityList last_known_capabilities = 3; @@ -390,6 +407,18 @@ message Action { message ExecuteChoreography { // The name of the sequence to play. string sequence_name = 1; + + message PromptParams { + // Should the dance start time be determined before the dance plays + // at runtime from user input + bool prompt_for_start_time = 1; + + // How long will the prompt run before timing out. If unset, + // the default value of 10 minutes will be used. + google.protobuf.DoubleValue prompt_timeout = 2; + } + + PromptParams prompt_params = 2; } oneof action { @@ -488,6 +517,7 @@ message ActionWrapper { message ArmSensorPointing { // Arm Joint Move Command // The joint trajectory to execute in the initial rough pointing joint move. + // Can be unset - if it is, robot will skip this step. bosdyn.api.ArmJointTrajectory joint_trajectory = 2; // Arm Cartesian Command @@ -497,6 +527,7 @@ message ActionWrapper { bosdyn.api.SE3Pose wrist_tform_tool = 3; // A 3D pose trajectory for the tool expressed in target frame, + // Can be unset - if it is, robot will skip this step. bosdyn.api.SE3Trajectory pose_trajectory_rt_target = 4; // Robot desired stance relative to waypoint @@ -604,6 +635,49 @@ message FailureBehavior { } } +// How are multiple interruptions handled? +// - SafePowerOff can never be overridden, and can override everything. +// - ReturnToStartAndTerminate can only be overridden by SafePowerOff, +// and can override everything else. +// - Node can't override anything except an actively running element. +// If multiple interrupts with "Node" as the condition_behavior are +// triggered, the first one triggered will run. When that behavior +// completes, all signals will be revaluated, and if the other +// interrupt is still high then that behavior will be triggered. +message WalkInterrupt { + message DaqPluginSignal { + reserved 1; + DataAcquisitionCapability capability = 4; + DataCapture data_capture = 2; + + // The DataCapture will be sent as part of a LiveDataRequest + // to the data acquisition plugin service. The service will return + // a CapabilityLiveData for that data_capture, which will be embedded + // in the mission blackboard at the following name, for use + // in the condition node. + string blackboard_name = 3; + } + + message ConditionAndBehavior { + // Only used for things like the autowalk report. + string name = 1; + + // Should evaluate to true when this interrupt should occur. + oneof condition { + bosdyn.api.mission.Condition condition_node = 2; + } + + oneof behavior { + FailureBehavior.SafePowerOff safe_power_off = 3; + FailureBehavior.ReturnToStartAndTerminate return_to_start_and_terminate = 4; + bosdyn.api.mission.Node behavior_node = 5; + } + } + + repeated DaqPluginSignal signals = 1; + repeated ConditionAndBehavior conditions_and_behaviors = 2; +} + // If your mission has docks, autowalk can pause the mission to return // to the dock if the battery gets too low. Use this message to control // when this behavior happens. diff --git a/protos/bosdyn/api/basic_command.proto b/protos/bosdyn/api/basic_command.proto index 5d64443..a18938c 100644 --- a/protos/bosdyn/api/basic_command.proto +++ b/protos/bosdyn/api/basic_command.proto @@ -215,6 +215,10 @@ message SE2TrajectoryCommand { } // Flag indicating if the final requested position was achievable. FinalGoalStatus final_goal_status = 5; + + // Enables clients to monitor the commanded goal trajectory, even if not sending the + // command. + SE2TrajectoryCommand.Request request_information = 6; } } @@ -244,7 +248,8 @@ message SE2VelocityCommand { } message Feedback { - // Planar velocity commands provide no feedback. + // Enables clients to monitor the commanded goal velocity, even if not sending the command. + SE2VelocityCommand.Request request_information = 1; } } @@ -630,6 +635,7 @@ message JointCommand { // clear the behavior fault via the Robot Command Service. Values less than or equal to 0 // will be considered invalid and must be sent in every UpdateRequest for use. google.protobuf.FloatValue velocity_safety_limit = 9; + } message ContactAdvice { diff --git a/protos/bosdyn/api/data_acquisition.proto b/protos/bosdyn/api/data_acquisition.proto index 306bf02..7a01589 100644 --- a/protos/bosdyn/api/data_acquisition.proto +++ b/protos/bosdyn/api/data_acquisition.proto @@ -44,6 +44,7 @@ message DataAcquisitionCapability { // Capability has live data available via GetLiveData RPC. bool has_live_data = 6; + } // Description of an image acquisition capability. The image acquisition capabilities will be @@ -77,6 +78,7 @@ message NetworkComputeCapability { AvailableModels models = 3; } + // A list of all capabilities (data and images) that a specific data acquisition plugin service can // successfully acquire and save the data specified in each capability. message AcquisitionCapabilityList { @@ -88,6 +90,7 @@ message AcquisitionCapabilityList { // List of network compute capabilities. repeated NetworkComputeCapability network_compute_sources = 5; + } // The CaptureActionId describes the entire capture action for an AcquireData request and will be @@ -167,6 +170,13 @@ message AssociatedAlertData { AlertData alert_data = 2; } +// An order for a given capture. This is used to determine the order in which captures are +// processed and saved. +message CaptureOrdering { + // The order value. Lower values indicate higher priority and will be executed first. + int32 value = 1; +} + // An individual capture which can be specified in the AcquireData request to identify a piece of // image data to be collected. message ImageSourceCapture { @@ -184,6 +194,9 @@ message ImageSourceCapture { // DEPRECATED as of 3.2.0. Use image_request instead. // Specific pixel format to capture reported by the ImageAcquisitionCapability message. Image.PixelFormat pixel_format = 3 [deprecated = true]; + + // The order in which this capture should be performed relative to the other captures. + CaptureOrdering order = 5; } // An individual capture which can be specified in the AcquireData or LiveData request to identify @@ -196,6 +209,9 @@ message DataCapture { // Values passed to the service at capture time. // See the DictParam.Spec in DataAcquisitionCapability. DictParam custom_params = 9; + + // The order in which this capture should be performed relative to the other captures. + CaptureOrdering order = 10; } message NetworkComputeCapture { @@ -208,6 +224,9 @@ message NetworkComputeCapture { // Which service to use. NetworkComputeServerConfiguration server_config = 2; + + // The order in which this capture should be performed relative to the other captures. + CaptureOrdering order = 4; } @@ -502,3 +521,4 @@ message LiveDataResponse { reserved 2; } + diff --git a/protos/bosdyn/api/data_acquisition_service.proto b/protos/bosdyn/api/data_acquisition_service.proto index a248afe..edc998d 100644 --- a/protos/bosdyn/api/data_acquisition_service.proto +++ b/protos/bosdyn/api/data_acquisition_service.proto @@ -32,4 +32,5 @@ service DataAcquisitionService { // Request live data available from DAQ plugins during teleoperation. // Please use the other RPCs for typical data acquisition. rpc GetLiveData(LiveDataRequest) returns (LiveDataResponse); + } diff --git a/protos/bosdyn/api/docking/docking.proto b/protos/bosdyn/api/docking/docking.proto index 4b26f5b..2edfa13 100644 --- a/protos/bosdyn/api/docking/docking.proto +++ b/protos/bosdyn/api/docking/docking.proto @@ -175,14 +175,13 @@ message DockingCommandFeedbackResponse { enum DockType { // Unknown type of dock DOCK_TYPE_UNKNOWN = 0; - - - // Prototype version SpotDock - DOCK_TYPE_CONTACT_PROTOTYPE = 2; // Production version SpotDock DOCK_TYPE_SPOT_DOCK = 3; // Production version SpotDock located in the dog house. DOCK_TYPE_SPOT_DOGHOUSE = 4; + + + reserved 1, 2; } // The configuration of a range of dock ID's diff --git a/protos/bosdyn/api/fiducial_purpose.proto b/protos/bosdyn/api/fiducial_purpose.proto new file mode 100644 index 0000000..7e6c341 --- /dev/null +++ b/protos/bosdyn/api/fiducial_purpose.proto @@ -0,0 +1,23 @@ +// 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; +option go_package = "bosdyn/api/FiducialPurpose"; +option java_outer_classname = "FiducialPurposeProto"; + +// A purpose of a fiducial. +enum FiducialPurpose { + UNKNOWN = 0; + // Fiducial is used for localization. (needs to be unique and static) + LOCALIZATION = 1; + // Fiducial is used as a docking target. + DOCK = 2; + // Fiducial is used interrupts + INTERRUPT = 3; + // Fiducial is used for log triggering. + LOG_TRIGGERING = 4; +} diff --git a/protos/bosdyn/api/graph_nav/graph_nav.proto b/protos/bosdyn/api/graph_nav/graph_nav.proto index c7fdb4a..c2a1971 100644 --- a/protos/bosdyn/api/graph_nav/graph_nav.proto +++ b/protos/bosdyn/api/graph_nav/graph_nav.proto @@ -227,6 +227,10 @@ message SetLocalizationResponse { message RouteGenParams { + + // When navigating after getting stuck, setting this to true will cause the robot to first + // backtrack from its stuck location to the start waypoint before starting the new route. + bool backtrack_to_start_waypoint = 2; } // Parameters describing how to travel along a route. @@ -898,6 +902,20 @@ message NavigationFeedbackResponse { // Map of Region IDs with relevant information map active_region_information = 10; + // The robot may achieve parts of the goal before the status is STATUS_REACHED_GOAL. + // This enum captures intermediate states. + enum GoalStatus { + // Unset. + GOAL_STATUS_UNKNOWN = 0; + // The robot has not yet reached the destination. + GOAL_STATUS_NOT_REACHED = 1; + // The robot is within the destination's goal specified in the TravelParams. + GOAL_STATUS_IN_GOAL_AREA = 2; + // The robot is standing at the final goal point. + GOAL_STATUS_STANDING_AT_GOAL = 3; + } + GoalStatus goal_status = 19; + // Data for a Area Callback region message ActiveRegionInformation { // human readable name for the region @@ -1388,6 +1406,37 @@ message UploadEdgeSnapshotResponse { MapStats map_stats = 3; } +// Like the individual waypoint/edge RPCs, but for N snapshots of both edges and waypoints. +// graph_nav only processes complete Snapshots so large protos are discouraged; +// any network interruption would require the data to be resent. Clients are +// encouraged to send data in batches on the order of a few MB to strike a +// balance between eliminating per-RPC overhead and recovering from errors. +message UploadSnapshotsRequest { + RequestHeader header = 1; + message Snapshots { + repeated WaypointSnapshot waypoint_snapshots = 1; + repeated EdgeSnapshot edge_snapshots = 2; + } + DataChunk chunk = 2; // This should be interpreted as a serialized Snapshots. + Lease lease = 3; +} + +message UploadSnapshotsResponse { + ResponseHeader header = 1; + LeaseUseResult lease_use_result = 2; + + // If waypoint snapshots were uploaded, sensor compatibilty is checked. + enum Status { + STATUS_UNKNOWN = 0; + STATUS_OK = 1; + STATUS_INCOMPATIBLE_SENSORS = 2; + } + Status status = 3; + SensorCompatibilityStatus sensor_status = 4; + + MapStats map_stats = 5; +} + // The DownloadWaypointSnapshot request asks for a specific waypoint snapshot id to // be downloaded and has parameters to decrease the amount of data downloaded. After // recording a map, first call the DownloadGraph RPC. Then, for each waypoint snapshot id, diff --git a/protos/bosdyn/api/graph_nav/graph_nav_service.proto b/protos/bosdyn/api/graph_nav/graph_nav_service.proto index f2abb1d..2c07dfe 100644 --- a/protos/bosdyn/api/graph_nav/graph_nav_service.proto +++ b/protos/bosdyn/api/graph_nav/graph_nav_service.proto @@ -68,6 +68,9 @@ service GraphNavService { // Uploads large edge snapshot as a stream for a particular edge. rpc UploadEdgeSnapshot(stream UploadEdgeSnapshotRequest) returns (UploadEdgeSnapshotResponse) {} + // Uploads multiple waypoint+edge snapshots. + rpc UploadSnapshots(stream UploadSnapshotsRequest) returns (UploadSnapshotsResponse) {} + // Download waypoint data from the server. If the snapshot exists in disk cache, it will be // loaded. rpc DownloadWaypointSnapshot(DownloadWaypointSnapshotRequest) diff --git a/protos/bosdyn/api/graph_nav/map.proto b/protos/bosdyn/api/graph_nav/map.proto index a84f159..1a906af 100644 --- a/protos/bosdyn/api/graph_nav/map.proto +++ b/protos/bosdyn/api/graph_nav/map.proto @@ -398,6 +398,13 @@ message Edge { GROUND_CLUTTER_FROM_FOOTFALLS = 2; } GroundClutterAvoidanceMode ground_clutter_mode = 17; + + message AudioVisualSettings { + // Behavior to run while traversing this edge. + string behavior_name = 1; + } + + AudioVisualSettings audio_visual_settings = 19; } // Annotations specific to the current edge. Annotations annotations = 4; diff --git a/protos/bosdyn/api/hazard_avoidance.proto b/protos/bosdyn/api/hazard_avoidance.proto new file mode 100644 index 0000000..bf45c11 --- /dev/null +++ b/protos/bosdyn/api/hazard_avoidance.proto @@ -0,0 +1,147 @@ +// 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; +option go_package = "bosdyn/api/hazard_avoidance"; + +option java_outer_classname = "HazardAvoidanceProto"; + +import "google/protobuf/timestamp.proto"; +import "google/protobuf/duration.proto"; + +import "bosdyn/api/geometry.proto"; +import "bosdyn/api/header.proto"; +import "bosdyn/api/image.proto"; +import "bosdyn/api/point_cloud.proto"; + +// Hazard data to add for tracking. +message HazardObservation { + // Proto to wrap a list of circles. + message CircleList { + repeated Circle circles = 1; + } + + // The time at which the hazard was observed. + google.protobuf.Timestamp acquisition_time = 1; + + oneof observation_data { + // A point cloud associated with this hazard. + PointCloud point_cloud = 2; + + // A segmented depth image; all positive pixels will be associated to this hazard. + // segmented_depth.shot.image must be type PIXEL_FORMAT_DEPTH_U16. + ImageCaptureAndSource segmented_depth = 3; + + // An oriented box to describe the footprint of the hazard. Must be in the vision frame. + Box2WithFrame box = 8; + + // A circle in the vision frame's XY plane to describe the footprint of the hazard. + Circle circle = 9; + + // Multiple circles in the vision frame's XY plane that describe the hazard. + CircleList circle_list = 10; + } + + enum HazardType { + // Not specified - not a valid value. + TYPE_UNKNOWN = 0; + + // Robot should avoid stepping on, with low penalty for doing so. + TYPE_PREFER_AVOID_WEAK = 1; + + // Robot should avoid stepping on, with high penalty for doing so. + TYPE_PREFER_AVOID_STRONG = 2; + + // Robot should never step on, but it can move over. + TYPE_NEVER_STEP_ON = 3; + + // Robot should never step on or move over. + TYPE_NEVER_STEP_ACROSS = 4; + + // Safe terrain - the robot should prefer stepping here. + TYPE_PREFER_STEP_ON = 5; + + // Robot should never step on, but it can move over. + // Positive margin will be treated as TYPE_PREFER_AVOID_WEAK. + // Negative margin will be unaffected. + TYPE_NEVER_STEP_ON_AVOID_MARGIN = 6; + + // Robot should never step on or move over. + // Positive margin will be treated as TYPE_PREFER_AVOID_STRONG. + // Negative margin will be unaffected. + TYPE_NEVER_STEP_ACROSS_AVOID_MARGIN = 7; + } + + // The type of hazard. + HazardType type = 4; + + // Confidence in the observation. + float likelihood = 5; + + // A label associated with the observation. + string semantic_label = 6; + + // Margin to expand (or contract if negative) the hazard by in meters. + float margin = 7; +} + +// Request to add multiple hazard observations. +message AddHazardsRequest { + RequestHeader header = 1; + repeated HazardObservation hazards = 2; + + // Body pose in VO at the time of the request. + // If not provided, will attempt to extract from the observation data. + SE3Pose vision_tform_body = 3; + + // If true, will write directly into the published map and not to the internally tracked map. + bool skip_aggregation = 4; + + // If skip_aggregation is true, max seconds for the observations to be kept around. + google.protobuf.Duration max_unaggregated_update_age = 5; + + // Identifier for the source of the observations. + string hazard_source = 6; +} + +// Result of adding a single hazard observation. +message AddHazardResult { + enum Status { + STATUS_UNKNOWN = 0; // Unset. + STATUS_HAZARDS_UPDATED = 1; // Updated the hazard map. + STATUS_IGNORED = 2; // No new hazard information was added. + STATUS_INVALID_DATA = 5; // The HazardObservation contained errors. + + reserved 3, 4; + } + + // Status of the detection. + Status status = 1; +} + +// Response for adding multiple hazard observations. +message AddHazardsResponse { + ResponseHeader header = 1; + repeated AddHazardResult add_hazard_results = 2; + + // Convenience value to check how many results returned STATUS_HAZARDS_UPDATED. + int32 num_hazards_updated = 3; +} + +// Request to get the status of the service. +message GetHazardServiceStatusRequest { + RequestHeader header = 1; +} + +// Response for getting the status of the service. +message GetHazardServiceStatusResponse { + ResponseHeader header = 1; + + // Time since last observation. Can be used to check if the extension is running. + google.protobuf.Duration time_since_last_observation = 2; +} diff --git a/protos/bosdyn/api/hazard_avoidance_service.proto b/protos/bosdyn/api/hazard_avoidance_service.proto new file mode 100644 index 0000000..ffe9795 --- /dev/null +++ b/protos/bosdyn/api/hazard_avoidance_service.proto @@ -0,0 +1,25 @@ +// 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; +option go_package = "bosdyn/api/hazard_avoidance_service"; + +option java_outer_classname = "HazardAvoidanceServiceProto"; + +import "bosdyn/api/hazard_avoidance.proto"; + +// The hazard avoidance service provides a way to aggregate and track observations of hazardous +// terrains/structures around the robot. +service HazardAvoidanceService { + // Request to add new hazard observations. + rpc AddHazards(AddHazardsRequest) returns (AddHazardsResponse) {} + + // Request to check if the extension is running. + rpc GetHazardServiceStatus(GetHazardServiceStatusRequest) + returns (GetHazardServiceStatusResponse) {} +} diff --git a/protos/bosdyn/api/log_status/log_status.proto b/protos/bosdyn/api/log_status/log_status.proto index 6927070..4ae5fce 100644 --- a/protos/bosdyn/api/log_status/log_status.proto +++ b/protos/bosdyn/api/log_status/log_status.proto @@ -9,6 +9,7 @@ syntax = "proto3"; package bosdyn.api.log_status; option go_package = "bosdyn/api/log_status/log_status"; +import "bosdyn/api/data_buffer.proto"; import "bosdyn/api/header.proto"; import "google/protobuf/duration.proto"; import "google/protobuf/timestamp.proto"; @@ -32,6 +33,9 @@ message LogStatus { TYPE_UNKNOWN = 0; TYPE_EXPERIMENT = 1; // A log into the future. TYPE_RETRO = 2; // A retroactive log. + TYPE_EVENT = 3; // A log created automatically from an event. + TYPE_DATA = 4; // A log created from a data log request. + TYPE_CONCURRENT = 5; // An experiment log that allows concurrency. } // Id of the response log. @@ -42,6 +46,16 @@ message LogStatus { // Type of log. Type type = 3; + + // Timestamp of the start of an experiment log, in robot time. + google.protobuf.Timestamp start_time = 4; + + // Timestamp of the end of an experiment log, in robot time. + google.protobuf.Timestamp end_time = 5; + + // Current duration of an experiment log, in robot time. This is not the total runtime, but + // rather how long it has run as of now. + google.protobuf.Duration run_time = 6; } message GetLogStatusRequest { @@ -90,6 +104,43 @@ message GetActiveLogStatusesResponse { repeated LogStatus log_statuses = 3; } +message StartConcurrentLogRequest { + // Common request header. + bosdyn.api.RequestHeader header = 1; + + // How long into the future should this log end? + google.protobuf.Duration keep_alive = 2; + + // How long into the past should text logs extend? (default 0) + google.protobuf.Duration past_textlog_duration = 3; + + // Specify an event for the request. The data included in the log will be defined by this event, + // if the robot has data associated with the event. + bosdyn.api.Event event = 4; +} + +message StartConcurrentLogResponse { + enum Status { + STATUS_UNKNOWN = 0; + STATUS_OK = 1; + STATUS_EXPERIMENT_LOG_RUNNING = 2; // An experiment log is already running. + STATUS_CONCURRENCY_LIMIT_REACHED = 3; // Maximum number of logs running concurrently. + STATUS_NO_DATA_FOR_EVENT = 4; // The specified event has no data associated with it + } + + // Common response header. + bosdyn.api.ResponseHeader header = 1; + + // Response status. + Status status = 2; + + // Status of the created log. + LogStatus log_status = 3; + + // Timestamp of the end of the log, in robot time. + google.protobuf.Timestamp end_time = 4; +} + message StartRetroLogRequest { // Common request header. bosdyn.api.RequestHeader header = 1; @@ -125,6 +176,9 @@ message StartExperimentLogRequest { // How long into the future should this log end? google.protobuf.Duration keep_alive = 2; + + // How long into the past should text logs extend? (default 0) + google.protobuf.Duration past_textlog_duration = 3; } message StartExperimentLogResponse { diff --git a/protos/bosdyn/api/log_status/log_status_service.proto b/protos/bosdyn/api/log_status/log_status_service.proto index 4fc0e00..a55fa90 100644 --- a/protos/bosdyn/api/log_status/log_status_service.proto +++ b/protos/bosdyn/api/log_status/log_status_service.proto @@ -37,6 +37,12 @@ service LogStatusService { // Only one experiment log can be run at a time. rpc StartExperimentLog(StartExperimentLogRequest) returns (StartExperimentLogResponse); + // Given a duration T and an event E, starts an experiment log that will include any data + // the robot associates with E, with a keepalive/watchdog timer of T. However, unlike an + // experiment log it allows for concurrency, so E must be provided to constrain the data + // included in the log. + rpc StartConcurrentLog(StartConcurrentLogRequest) returns (StartConcurrentLogResponse); + // UpdateExperimentLog(id, T) will update the keepalive/watchdog timer of // the log with the provided id if the log is active. The updated duration // of the log will be [t_rpc, t_rpc + T] where t_rpc = time of RPC reception. diff --git a/protos/bosdyn/api/mission/mission.proto b/protos/bosdyn/api/mission/mission.proto index dd3ad2d..ca0b2f6 100644 --- a/protos/bosdyn/api/mission/mission.proto +++ b/protos/bosdyn/api/mission/mission.proto @@ -158,6 +158,10 @@ message Question { // human. Clients should usually avoid generating a UI element to ask such a question. bool for_autonomous_processing = 5; + // Optional string data to help autonomous systems identify questions. + // This data will come from the Prompt asking the question. + string autonomy_key = 8; + // Severity for this question. See comment in Prompt message in nodes.proto for a better // understanding of what levels mean. AlertData.SeverityLevel severity = 6; diff --git a/protos/bosdyn/api/mission/nodes.proto b/protos/bosdyn/api/mission/nodes.proto index a910117..e266cf8 100644 --- a/protos/bosdyn/api/mission/nodes.proto +++ b/protos/bosdyn/api/mission/nodes.proto @@ -100,6 +100,7 @@ message Node { MissionUploadChoreography mission_upload_choreography = 57; CreateMissionText create_mission_text = 59; BosdynQueryStoredCaptures bosdyn_query_stored_captures = 60; + DataAcquisitionLiveData data_acquisition_live_data = 64; } // Defines parameters, used by this node or its children. @@ -301,6 +302,7 @@ message Condition { HANDLE_STALE_FAIL = 3; // return FAILURE status if stale data is read. } HandleStaleness handle_staleness = 6; + } // Get state from the robot. @@ -381,6 +383,7 @@ message BosdynNavigateTo { // Defines robot behavior when route is blocked. Defaults to reroute when route is blocked. bosdyn.api.graph_nav.RouteFollowingParams.RouteBlockedBehavior route_blocked_behavior = 8; + } // Tell the robot to navigate to a pose in the seed frame @@ -408,6 +411,7 @@ message BosdynNavigateToAnchor { // If provided, parameters from this request will be merged with the other parameters defined in // the node and be used in the NavigateToAnchor RPC. string navigate_to_anchor_request_blackboard_key = 8; + } // Tell the robot to navigate a route. @@ -437,6 +441,7 @@ message BosdynNavigateRoute { // If provided, parameters from this request will be merged with the other parameters defined in // the node and be used in the NavigateRoute RPC. string navigate_route_request_blackboard_key = 8; + } // Get GraphNav state from the robot and save it to the blackboard. @@ -560,6 +565,10 @@ message ExecuteChoreography { string host = 2; // The name of the sequence to play. string sequence_name = 3; + + // Optional Blackboard variable that can specify a start time in nanoseconds + // since epoch in the robot's clock to be used for the start time of the sequence. + string start_time_in_blackboard = 4; } // Uploads the given set of animations and choreography sequences to the robot. @@ -612,11 +621,15 @@ message Prompt { oneof answer_spec { // The set of options that can be chosen for this prompt. OptionsList options_list = 9; - // Key to an OptionsList protobuf object on the blackboard. The variable is only read when - // the node starts meaning options cannot change while the node is running. + // Key to a OptionsList protobuf object on the blackboard. Note that this variable is only + // read when the node starts, meaning that options can not change while the node is running. string options_list_in_blackboard = 11; // Custom parameter specification for the answer expected for this prompt. DictParam.Spec custom_params = 10; + // Key to a DictParam.Spec protobuf object on the blackboard. Note that this variable is + // only read when the node starts, meaning that options can not change while the node is + // running. + string custom_params_in_blackboard = 13; } // Child node, run after the prompt has been responded to. @@ -627,6 +640,10 @@ message Prompt { // See the Question message for details. bool for_autonomous_processing = 6; + // Optional string data to help autonomous systems identify questions, this will be + // copied into the Question message. + string autonomy_key = 14; + oneof severity_spec { // Severity for this prompt. Used to determine what sort of alerting // this prompt will trigger. @@ -889,6 +906,9 @@ message DataAcquisition { // This allows the robot to continue on with the mission sooner, but // it will be unaware of failures in processing or storage. COMPLETE_AFTER_ACQUIRED = 2; + // Node is complete after the AcquireData request is sent. + // Note that if the request fails, the node will not be aware of it. + COMPLETE_AFTER_REQUEST = 3; } CompletionBehavior completion_behavior = 4; @@ -933,6 +953,32 @@ message DataAcquisition { } +message DataAcquisitionLiveData { + // Service name for a data acquisition service + // NOTE: Not the plugin! Just the main service + // You pick the plugin by specifying a capture name in DataCapture + string service_name = 1; + + // Host machine of the directory server that the data acquisition service is registered with. + string host = 2; + + message DataCaptureAndBlackboardName { + // What request to send to that plugin + DataCapture data_capture = 1; + + // The DataCapture will be sent as part of a LiveDataRequest + // to the data acquisition plugin service. The plugin service will return + // a CapabilityLiveData for that data_capture, which will be embedded + // in the blackboard at the following name, for use in a child node. + string blackboard_name = 2; + } + + repeated DataCaptureAndBlackboardName data_captures = 3; + + // Child node. Children will have access to the state gathered by this node. + Node child = 4; +} + // Send RetainLease for every Lease the mission service is given via PlayMissionRequest. // Returns RUNNING while there are more leases to retain, SUCCESS once a lease for each resource has // been retained, and FAILURE if any one lease cannot be retained. diff --git a/protos/bosdyn/api/network_compute_bridge.proto b/protos/bosdyn/api/network_compute_bridge.proto index 198206a..7309f09 100644 --- a/protos/bosdyn/api/network_compute_bridge.proto +++ b/protos/bosdyn/api/network_compute_bridge.proto @@ -65,6 +65,7 @@ message ModelData { // Per-model parameters. DictParam.Spec custom_params = 7; + } message ModelLabels { diff --git a/protos/bosdyn/api/power.proto b/protos/bosdyn/api/power.proto index e1fa363..06ca0e3 100644 --- a/protos/bosdyn/api/power.proto +++ b/protos/bosdyn/api/power.proto @@ -87,6 +87,8 @@ message PowerCommandRequest { REQUEST_ON_PAYLOAD_PORTS = 6; // Turn on power to the payload ports. REQUEST_OFF_WIFI_RADIO = 7; // Cut power to the hardware Wi-Fi radio. REQUEST_ON_WIFI_RADIO = 8; // Power on the hardware Wi-Fi radio. + REQUEST_SOFT_REBOOT_ROBOT = + 9; // Soft reboot of base robot computers. Does not cut power to payloads. } Request request = 3; diff --git a/protos/bosdyn/api/robot_state.proto b/protos/bosdyn/api/robot_state.proto index 474525a..a056565 100644 --- a/protos/bosdyn/api/robot_state.proto +++ b/protos/bosdyn/api/robot_state.proto @@ -305,6 +305,10 @@ message SystemFault { // immediately. Severity severity = 7; + + // Diagnostic Troubleshooting Code (DTC). This will directly map to the "code" field in this + // message. + string dtc = 15; } // The robot's current E-Stop states and endpoints. diff --git a/protos/bosdyn/api/service_fault.proto b/protos/bosdyn/api/service_fault.proto index 64824f0..84825e6 100644 --- a/protos/bosdyn/api/service_fault.proto +++ b/protos/bosdyn/api/service_fault.proto @@ -34,6 +34,7 @@ message ServiceFaultId { // Optional. If not set, it will be assigned to the payload associated with the // service_name. string payload_guid = 3; + } // The current service faults for services registered with the robot. diff --git a/protos/bosdyn/api/signals.proto b/protos/bosdyn/api/signals.proto index 58da045..7aa9c04 100644 --- a/protos/bosdyn/api/signals.proto +++ b/protos/bosdyn/api/signals.proto @@ -88,6 +88,15 @@ message SignalSpec { // If multiple conditions are simultaneously met, // the higher severity condition or first in the list will be the accepted alert state. repeated AlertConditionSpec alerts = 3; + // Data type of the signal. + enum DataType { + DATA_TYPE_UNKNOWN = 0; + DATA_TYPE_DOUBLE = 1; + DATA_TYPE_INT = 2; + DATA_TYPE_STRING = 3; + DATA_TYPE_BOOL = 4; + } + DataType data_type = 4; } // The recorded signal data. diff --git a/protos/bosdyn/api/spot/robot_command.proto b/protos/bosdyn/api/spot/robot_command.proto index 1d6a46a..183f8fa 100644 --- a/protos/bosdyn/api/spot/robot_command.proto +++ b/protos/bosdyn/api/spot/robot_command.proto @@ -15,6 +15,7 @@ import "bosdyn/api/geometry.proto"; import "bosdyn/api/trajectory.proto"; import "google/protobuf/wrappers.proto"; + // The locomotion hint specifying the gait of the robot. enum LocomotionHint { option allow_alias = true; @@ -62,6 +63,7 @@ enum SwingHeight { // Params common across spot movement and mobility. message MobilityParams { + // Max allowable velocity at any point in trajectory. SE2VelocityLimit vel_limit = 1; @@ -139,6 +141,7 @@ message MobilityParams { HazardDetectionMode hazard_detection_mode = 18; + } // Parameters for offsetting the body from the normal default. @@ -278,3 +281,4 @@ message BodyExternalForceParams { // Reserved for deprecated fields. reserved 2; } + diff --git a/protos/bosdyn/api/spot_cam/network.proto b/protos/bosdyn/api/spot_cam/network.proto index a6c8969..386e1e9 100644 --- a/protos/bosdyn/api/spot_cam/network.proto +++ b/protos/bosdyn/api/spot_cam/network.proto @@ -123,3 +123,4 @@ message SetICEConfigurationResponse { // Common response header. bosdyn.api.ResponseHeader header = 1; } + diff --git a/protos/bosdyn/api/world_object.proto b/protos/bosdyn/api/world_object.proto index 37e551a..1886db5 100644 --- a/protos/bosdyn/api/world_object.proto +++ b/protos/bosdyn/api/world_object.proto @@ -18,6 +18,7 @@ import "bosdyn/api/image.proto"; import "bosdyn/api/sparse_features.proto"; import "bosdyn/api/stairs.proto"; import "bosdyn/api/gps/registration.proto"; +import "bosdyn/api/fiducial_purpose.proto"; import "google/protobuf/duration.proto"; import "google/protobuf/timestamp.proto"; import "google/protobuf/any.proto"; @@ -57,6 +58,8 @@ message WorldObject { repeated DrawableProperties drawable_properties = 5; // The apriltag properties describe any fiducial identifying an object. AprilTagProperties apriltag_properties = 6; + // Properties of a moving entity. + TrackedEntityProperties tracked_entity_properties = 13; // Property for a user no-go NoGoRegionProperties nogo_region_properties = 14; // The image properties describe any camera and image coordinates associated with an object. @@ -86,6 +89,7 @@ enum WorldObjectType { WORLD_OBJECT_APRILTAG = 2; WORLD_OBJECT_IMAGE_COORDINATES = 5; WORLD_OBJECT_DOCK = 6; + WORLD_OBJECT_TRACKED_ENTITY = 7; WORLD_OBJECT_USER_NOGO = 8; WORLD_OBJECT_STAIRCASE = 9; } @@ -291,6 +295,9 @@ message AprilTagProperties { SE3Covariance detection_covariance = 5; // The frame that the detection covariance is expressed in. string detection_covariance_reference_frame = 6; + + // Purpose of the fiducial. + FiducialPurpose purpose = 10; } message RayProperties { @@ -312,6 +319,57 @@ message BoundingBoxProperties { string frame = 2; } +// Represents an entity (object that possibly has a mind of its own, like a person or vehicle) that +// is being tracked by the world object service. +message TrackedEntityProperties { + // Unique ID of the entity according to the tracking system. Note that + // this does not persist between reboots, and an entity track may be lost + // and regained later, possibly with a new ID. + int32 entity_id = 1; + enum EntityType { + ENTITY_TYPE_UNKNOWN = 0; // Unset. + ENTITY_TYPE_3D_BLOB = 1; // Tracked using 3D blob detection. + ENTITY_TYPE_PERSON = 2; // Tracked using person detection. + ENTITY_TYPE_FORKLIFT = 3; + ENTITY_TYPE_SPOT = 4; + } + + // Current type of the entity. + EntityType entity_type = 2; + // The pose of the entity is expressed in this frame. + string frame = 3; + // Bounding box size expressed in the reference frame. This is the + // side length of the bounding box. + Vec3 size_in_frame = 4; + // The velocity of the object will be expressed in this frame. + string velocity_frame = 5; + // Velocity estimate expressed in the given frame. + Vec3 velocity = 6; + // Number from 0-1 indicating whether we believe this object + // exists. + double likelihood_exists = 8; + // Map from type enum (e.g., "person") to a value from 0-1 indicating + // our belief of how likely this object is the corresponding type. + map type_likelihoods = 11; + // Number of times we've observed this entity. + int32 num_observations = 10; + // The maximum length of the observed velocity vector for this entity. + double max_observed_velocity = 12; + reserved 9; + // Average velocity over a window. The size of the window varies depending on how + // old the entity is. See velocity_window_size_seconds for details. + Vec3 windowed_average_velocity = 14; + // This is the number of seconds in the window used to compute windowed_average_velocity. + double velocity_window_size_seconds = 15; + // This is the absolute magnitude of the velocity over a window -- that is: + // sum_{i in window} (||v_i||) where v_i = the velocity at that time. This is the + // same window size as "velocity_window_size_seconds". Note that this velocity is 2d, projected + // to the x/y/ plane in vision frame. + double windowed_velocity_magnitude = 16; + // Estimate of the entity's direction. + Vec3 direction_in_vision = 17; +} + // The drawing and visualization information for a world object. message DrawableProperties {