Merge branch 'boston-dynamics:master' into master

Author: amessing-bdai

README.md

@@ -19,7 +19,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 4.0.0 of the C++ SDK. Please review the [Release Notes](docs/cpp_release_notes.md) to see what has changed.
+This is version 4.0.2 of the C++ SDK. Please review the [Release Notes](docs/cpp_release_notes.md) to see what has changed.
 
 ## Contents
 

cpp/CMakeLists.txt

@@ -7,7 +7,7 @@
 # This file is autogenerated.
 
 cmake_minimum_required (VERSION 3.10.2)
-project (bosdyn VERSION 4.0.0)
+project (bosdyn VERSION 4.0.2)
 
 # Dependencies:
 find_package(protobuf REQUIRED)

cpp/bosdyn/client/error_codes/joint_control_stream_error_code.cpp

@@ -0,0 +1,54 @@
+/**
+ * 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).
+ */
+
+
+#include "bosdyn/client/error_codes/joint_control_stream_error_code.h"
+#include "bosdyn/client/error_codes/error_type_condition.h"
+#include "bosdyn/client/error_codes/sdk_error_code.h"
+#include "bosdyn/common/success_condition.h"
+
+namespace {  // anonymous namespace
+
+struct JointControlStreamErrorCodeCategory : std::error_category {
+    const char* name() const noexcept override;
+    std::string message(int ev) const override;
+    bool equivalent(int valcode, const std::error_condition& cond) const noexcept override;
+};
+
+bool JointControlStreamErrorCodeCategory::equivalent(
+    int valcode, const std::error_condition& cond) const noexcept {
+    if (cond == SuccessCondition::Success) return (valcode == 0);
+    if (cond == ErrorTypeCondition::SDKError) return true;
+    return false;
+}
+
+const char* JointControlStreamErrorCodeCategory::name() const noexcept {
+    return "JointControlStreamErrorCode";
+}
+
+std::string JointControlStreamErrorCodeCategory::message(int value) const {
+    switch (static_cast<JointControlStreamErrorCode>(value)) {
+        case JointControlStreamErrorCode::Success:
+            return "Success";
+        case JointControlStreamErrorCode::RequestWriterFailed:
+            return "Getting request writer failed";
+        case JointControlStreamErrorCode::ResponseReaderFailed:
+            return "Getting response reader failed";
+        case JointControlStreamErrorCode::StreamingFailed:
+            return "Streaming for joint control failed";
+    }
+    return "(JointControlStreamErrorCode: unrecognized error)";
+}
+
+const JointControlStreamErrorCodeCategory JointControlStreamErrorCodeCategory_category{};
+
+}  // anonymous namespace
+
+std::error_code make_error_code(JointControlStreamErrorCode value) {
+    return {static_cast<int>(value), JointControlStreamErrorCodeCategory_category};
+}

cpp/bosdyn/client/error_codes/joint_control_stream_error_code.h

@@ -0,0 +1,32 @@
+/**
+ * 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).
+ */
+
+
+#pragma once
+
+#include <system_error>
+
+// Need to be more specific or removed when streaming client is changed to use general form as we
+// did in other clients.
+enum class JointControlStreamErrorCode {
+    // Success
+    Success = 0,
+    // Getting request_writer failed
+    RequestWriterFailed = 1,
+    // Getting response reader failed
+    ResponseReaderFailed = 2,
+    // Streaming for joint control failed
+    StreamingFailed = 3,
+};
+
+namespace std {
+template <>
+struct is_error_code_enum<JointControlStreamErrorCode> : true_type {};
+}  // namespace std
+
+std::error_code make_error_code(JointControlStreamErrorCode);

cpp/bosdyn/client/robot_command/robot_command_builder.cpp

@@ -52,6 +52,12 @@ ::bosdyn::api::RobotCommand SafePowerOffCommand() {
     return command;
 }
 
+::bosdyn::api::RobotCommand JointCommand() {
+    ::bosdyn::api::RobotCommand command;
+    command.mutable_full_body_command()->mutable_joint_request();
+    return command;
+}
+
 ::bosdyn::api::RobotCommand SitCommand(const ::bosdyn::api::spot::MobilityParams& params) {
     ::bosdyn::api::RobotCommand command;
     command.mutable_synchronized_command()->mutable_mobility_command()->mutable_params()->CopyFrom(

cpp/bosdyn/client/robot_command/robot_command_builder.h

@@ -65,6 +65,8 @@ ::bosdyn::api::RobotCommand SelfrightCommand();
  */
 ::bosdyn::api::RobotCommand SafePowerOffCommand();
 
+::bosdyn::api::RobotCommand JointCommand();
+
 /**
  * Command the robot to sit.
  *

cpp/bosdyn/client/robot_command/robot_command_helpers.cpp

@@ -67,15 +67,23 @@ const std::error_category& BlockingRobotCommandErrorCategory() {
 RobotCommandFeedbackResultType BlockUntilArmArrives(
     ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id, int timeout_sec,
     int poll_period_msec) {
+    return BlockUntilArmArrives(robot_command_client, cmd_id, std::chrono::seconds(timeout_sec),
+                                std::chrono::milliseconds(poll_period_msec));
+}
+
+RobotCommandFeedbackResultType BlockUntilArmArrives(
+    ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id,
+    ::bosdyn::common::Duration timeout, ::bosdyn::common::Duration poll_period) {
     ::bosdyn::api::RobotCommandFeedbackRequest feedback_req;
     feedback_req.set_robot_command_id(cmd_id);
 
     ::bosdyn::client::RobotCommandFeedbackResultType feedback_res;
 
-    auto start_time = ::bosdyn::common::NowNsec();
-    const auto end_time = start_time + ::bosdyn::common::SecToNsec(timeout_sec);
+    auto start_time = ::bosdyn::common::NsecSinceEpoch();
+    const auto end_time = start_time + timeout;
 
-    while (timeout_sec <= 0 || ::bosdyn::common::NowNsec() < end_time) {
+    while (timeout <= ::bosdyn::common::Duration(0) ||
+           ::bosdyn::common::NsecSinceEpoch() < end_time) {
         feedback_res = robot_command_client.RobotCommandFeedback(feedback_req);
         // If the RPC timed out but this function hasn't timed out, keep trying.
         if (!feedback_res.status &&
@@ -137,7 +145,7 @@ RobotCommandFeedbackResultType BlockUntilArmArrives(
                     feedback_res.move()};
         }
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(poll_period_msec));
+        std::this_thread::sleep_for(poll_period);
     }
     return {{BlockingRobotCommandErrorCode::CommandTimeoutError, "ArmCommand failed or timed out."},
             feedback_res.response};
@@ -146,15 +154,23 @@ RobotCommandFeedbackResultType BlockUntilArmArrives(
 RobotCommandFeedbackResultType BlockUntilGripperArrives(
     ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id, int timeout_sec,
     int poll_period_msec) {
+    return BlockUntilGripperArrives(robot_command_client, cmd_id, std::chrono::seconds(timeout_sec),
+                                    std::chrono::milliseconds(poll_period_msec));
+}
+
+RobotCommandFeedbackResultType BlockUntilGripperArrives(
+    ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id,
+    ::bosdyn::common::Duration timeout, ::bosdyn::common::Duration poll_period) {
     ::bosdyn::api::RobotCommandFeedbackRequest feedback_req;
     feedback_req.set_robot_command_id(cmd_id);
 
     ::bosdyn::client::RobotCommandFeedbackResultType feedback_res;
 
-    auto start_time = ::bosdyn::common::NowNsec();
-    const auto end_time = start_time + ::bosdyn::common::SecToNsec(timeout_sec);
+    auto start_time = ::bosdyn::common::NsecSinceEpoch();
+    const auto end_time = start_time + timeout;
 
-    while (timeout_sec <= 0 || ::bosdyn::common::NowNsec() < end_time) {
+    while (timeout <= ::bosdyn::common::Duration(0) ||
+           ::bosdyn::common::NsecSinceEpoch() < end_time) {
         feedback_res = robot_command_client.RobotCommandFeedback(feedback_req);
         // If the RPC timed out but this function hasn't timed out, keep trying.
         if (!feedback_res.status &&
@@ -174,12 +190,48 @@ RobotCommandFeedbackResultType BlockUntilGripperArrives(
             return {{BlockingRobotCommandErrorCode::Success}, feedback_res.move()};
         }
 
-        std::this_thread::sleep_for(std::chrono::milliseconds(poll_period_msec));
+        std::this_thread::sleep_for(poll_period);
     }
     return {{BlockingRobotCommandErrorCode::CommandTimeoutError,
              "The GripperCommand failed or timed out."},
             feedback_res.move()};
 }
+
+RobotCommandFeedbackResultType BlockUntilStandComplete(
+    ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id,
+    ::bosdyn::common::Duration timeout, ::bosdyn::common::Duration poll_period) {
+    ::bosdyn::api::RobotCommandFeedbackRequest feedback_req;
+    feedback_req.set_robot_command_id(cmd_id);
+
+    ::bosdyn::client::RobotCommandFeedbackResultType feedback_res;
+
+    auto start_time = ::bosdyn::common::NsecSinceEpoch();
+    const auto end_time = start_time + timeout;
+
+    while (timeout <= ::bosdyn::common::Duration(0) ||
+           ::bosdyn::common::NsecSinceEpoch() < end_time) {
+        feedback_res = robot_command_client.RobotCommandFeedback(feedback_req);
+        // If the RPC timed out but this function hasn't timed out, keep trying.
+        if (!feedback_res.status &&
+            feedback_res.status.code() != ::bosdyn::client::RPCErrorCode::TimedOutError) {
+            return feedback_res;
+        }
+
+        auto stand_feedback = feedback_res.response.feedback()
+                                  .synchronized_feedback()
+                                  .mobility_command_feedback()
+                                  .stand_feedback();
+
+        if (stand_feedback.status() == ::bosdyn::api::StandCommand::Feedback::STATUS_IS_STANDING) {
+            return {{BlockingRobotCommandErrorCode::Success}, feedback_res.move()};
+        }
+
+        std::this_thread::sleep_for(poll_period);
+    }
+    return {{BlockingRobotCommandErrorCode::CommandTimeoutError,
+             "The StandCommand failed or timed out."},
+            feedback_res.move()};
+}
 }  // namespace client
 
 }  // namespace bosdyn

cpp/bosdyn/client/robot_command/robot_command_helpers.h

@@ -45,10 +45,33 @@ std::error_code make_error_code(BlockingRobotCommandErrorCode);
  * move was canceled (the arm failed to reach the goal). See the proto definitions in
  * arm_command.proto for more information about why a trajectory would succeed or fail.
  */
-RobotCommandFeedbackResultType BlockUntilArmArrives(
+[[deprecated]] RobotCommandFeedbackResultType BlockUntilArmArrives(
     ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id, int timeout_sec = 0,
     int poll_period_msec = 100);
 
+/**
+ * Blocks until the arm achieves a finishing state for the specific arm command. This helper will
+ * block and check the feedback for ArmCartesianCommand, GazeCommand, ArmJointMoveCommand,
+ * NamedArmPositionsCommand, and ArmImpedanceCommand.
+ *
+ * @param robot_command_client (bosdyn.client.RobotCommandClient): The robot command client, used to
+ * request command feedback.
+ * @param cmd_id (int): The command ID returned by the robot when the arm movement command was sent.
+ * @param timeout (Duration): Duration after which we'll return no matter what the
+ * robot's state is. If unset, 0, or negative, this function will never time out and only return
+ * when there is finished command feedback.
+ * @param poll_period (Duration): Duration to wait between requesting feedback updates. Default is
+ * 100 ms.
+ *
+ * @return True if successfully got to the end of the trajectory, False if the arm stalled or the
+ * move was canceled (the arm failed to reach the goal). See the proto definitions in
+ * arm_command.proto for more information about why a trajectory would succeed or fail.
+ */
+RobotCommandFeedbackResultType BlockUntilArmArrives(
+    ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id,
+    ::bosdyn::common::Duration timeout = std::chrono::seconds(0),
+    ::bosdyn::common::Duration poll_period = std::chrono::milliseconds(100));
+
 /**
  * Blocks until the gripper achieves a finishing state for a ClawGripperCommand.
  *
@@ -65,9 +88,49 @@ RobotCommandFeedbackResultType BlockUntilArmArrives(
  * @return True if the gripper successfully reached the goal or is applying force on something,
  * False if the gripper failed to reach the goal.
  */
-RobotCommandFeedbackResultType BlockUntilGripperArrives(
+[[deprecated]] RobotCommandFeedbackResultType BlockUntilGripperArrives(
     ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id, int timeout_sec = 0,
     int poll_period_msec = 100);
+
+/**
+ * Blocks until the gripper achieves a finishing state for a ClawGripperCommand.
+ *
+ * @param robot_command_client (bosdyn.client.RobotCommandClient): The robot command client, used to
+ * request command feedback.
+ * @param cmd_id (int): The command ID returned by the robot when the arm movement command was sent.
+ * @param timeout (Duration): Duration after which we'll return no matter what the
+ * robot's state is. If unset, 0, or negative, this function will never time out and only return
+ * when there is finished command feedback.
+ * @param poll_period (Duration): Duration to wait between requesting feedback updates. Default is
+ * 100 ms.
+ *
+ * @return True if the gripper successfully reached the goal or is applying force on something,
+ * False if the gripper failed to reach the goal.
+ */
+RobotCommandFeedbackResultType BlockUntilGripperArrives(
+    ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id,
+    ::bosdyn::common::Duration timeout = std::chrono::seconds(0),
+    ::bosdyn::common::Duration poll_period = std::chrono::milliseconds(100));
+
+/**
+ * Blocks until the Robot complete StandCommand
+ *
+ * @param robot_command_client (bosdyn.client.RobotCommandClient): The robot command client, used to
+ * request command feedback.
+ * @param cmd_id (int): The command ID returned by the robot when the arm movement command was sent.
+ * @param timeout (Duration): Duration after which we'll return no matter what the
+ * robot's state is. If unset, 0, or negative, this function will never time out and only return
+ * when there is finished command feedback.
+ * @param poll_period (Duration): Duration to wait between requesting feedback updates. Default is
+ * 100 ms.
+ *
+ * @return True if the robot successfully completed stand.
+ * False if the robot failed to reach the stand pose.
+ */
+RobotCommandFeedbackResultType BlockUntilStandComplete(
+    ::bosdyn::client::RobotCommandClient& robot_command_client, int cmd_id,
+    ::bosdyn::common::Duration timeout = std::chrono::seconds(10),
+    ::bosdyn::common::Duration poll_period = std::chrono::milliseconds(100));
 }  // namespace client
 
 }  // namespace bosdyn

cpp/bosdyn/client/robot_command/robot_command_streaming_client.cpp

@@ -0,0 +1,48 @@
+/**
+ * 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).
+ */
+
+
+#include "robot_command_streaming_client.h"
+
+namespace bosdyn {
+
+namespace client {
+
+const char* RobotCommandStreamingClient::s_default_service_name = "robot-command-streaming";
+
+const char* RobotCommandStreamingClient::s_service_type = "bosdyn.api.RobotCommandStreamingService";
+
+JointControlStreamResultType RobotCommandStreamingClient::JointControlStream(
+    const ::bosdyn::api::JointControlStreamRequest& request) {
+    if (!m_request_writer) {
+        m_request_writer = m_stub->JointControlStream(&m_context, &m_response);
+        if (!m_request_writer) {
+            return {::bosdyn::common::Status(JointControlStreamErrorCode::RequestWriterFailed),
+                    std::move(m_response)};
+        }
+    }
+
+    if (!m_request_writer->Write(request)) {
+        return {::bosdyn::common::Status(JointControlStreamErrorCode::StreamingFailed),
+                std::move(m_response)};
+    }
+
+    return {::bosdyn::common::Status(JointControlStreamErrorCode::Success)};
+}
+
+ServiceClient::QualityOfService RobotCommandStreamingClient::GetQualityOfService() const {
+    return QualityOfService::NORMAL;
+}
+
+void RobotCommandStreamingClient::SetComms(const std::shared_ptr<grpc::ChannelInterface>& channel) {
+    m_stub.reset(new ::bosdyn::api::RobotCommandStreamingService::Stub(channel));
+}
+
+}  // namespace client
+
+}  // namespace bosdyn