skip duplicate torqueOn when already enabled

cmvr-es/service/grpc/server/src/grpc_arm_service.cpp

@@ -360,6 +360,20 @@ grpc::Status setControlDispatchFailure(
             " dispatch");
 }
 
+grpc::Status setAlreadyEnabled(
+    api::CommandHeader_Feedback* response,
+    const std::string& device_id,
+    const char* rpc_name)
+{
+    constexpr const char* kAlreadyEnabledMessage =
+        "RobotArm is already enabled";
+    CMVR_LOG(WARNING) << "[gRPCArmServiceImpl] (" << rpc_name
+                      << "): ignored because the device is already enabled, id="
+                      << device_id;
+    fillFeedback(response, true, kAlreadyEnabledMessage);
+    return grpc::Status::OK;
+}
+
 } // namespace
 
 gRPCArmServiceImpl::gRPCArmServiceImpl()
@@ -423,6 +437,10 @@ grpc::Status gRPCArmServiceImpl::torqueOn(grpc::ServerContext* context,
         if (!arm) {
             return setDeviceNotFound(response, device_id);
         }
+        const auto state = arm->getRobotState();
+        if (state.powered_on) {
+            return setAlreadyEnabled(response, device_id, "torqueOn");
+        }
         ScopedUnaryControlLease control_lease(
             device_id, "torqueOn");
         if (!control_lease.acquired()) {

cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp

@@ -961,7 +961,9 @@ grpc::Status GrpcCommandTransaction::finish(
             : dispatch_started_ ? safety::CommandLifecycle::Failed
                                 : safety::CommandLifecycle::RejectedBeforeDispatch);
     const std::string detail = success
-        ? std::string{}
+        ? feedback && !feedback->error_message().empty()
+              ? feedback->error_message()
+              : std::string{}
         : feedback && !feedback->error_message().empty()
               ? feedback->error_message()
               : operation_status.error_message();
@@ -1134,7 +1136,7 @@ void GrpcCommandTransaction::populateFeedback_(
         feedback->set_error_message(
             detail.empty() ? safety::toString(reason) : detail);
     }
-    if (success) {
+    if (success && feedback->error_message().empty()) {
         feedback->clear_error_message();
     }
     if (!feedback->has_timestamp()) {

cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp

@@ -1,5 +1,6 @@
 #include "service/grpc/server/include/grpc_arm_service.h"
 
+#include <atomic>
 #include <chrono>
 #include <cstdio>
 #include <condition_variable>
@@ -48,7 +49,16 @@ public:
 
     device::RobotModel getRobotModel() const override { return {}; }
     std::size_t getDof() const override { return 0U; }
-    device::ArmState getRobotState() const override { return {}; }
+    device::ArmState getRobotState() const override
+    {
+        device::ArmState state;
+        state.connected = true;
+        state.powered_on = powered_on_.load();
+        state.robot_mode = state.powered_on
+            ? device::RobotMode::Running
+            : device::RobotMode::PowerOff;
+        return state;
+    }
     device::JointGroupState getJointState() const override { return {}; }
     device::CartesianPose getTcpPose(
         device::FrameType = device::FrameType::Base) const override
@@ -325,6 +335,11 @@ public:
         return torque_on_calls_;
     }
 
+    void setPoweredOn(const bool powered_on)
+    {
+        powered_on_.store(powered_on);
+    }
+
     bool lastTorqueOnHadCancellation() const
     {
         std::lock_guard lock(motion_mutex_);
@@ -505,6 +520,7 @@ private:
     bool last_torque_on_cancellation_requested_{false};
     bool last_motion_had_cancellation_{false};
     bool last_motion_cancellation_requested_{false};
+    std::atomic<bool> powered_on_{false};
 };
 
 class JsonCommandNonArmDevice final : public device::AbstractDevice {
@@ -641,13 +657,20 @@ protected:
         return service_->torqueOff(&context, &request, &response);
     }
 
-    MoveOutcome torqueOn(const std::string& device_id)
+    grpc::Status torqueOn(
+        const std::string& device_id,
+        api::CommandHeader_Feedback& response)
     {
         api::CommandHeader_Request request;
         request.set_device_id(device_id);
-        api::CommandHeader_Feedback response;
         grpc::ServerContext context;
-        auto status = service_->torqueOn(&context, &request, &response);
+        return service_->torqueOn(&context, &request, &response);
+    }
+
+    MoveOutcome torqueOn(const std::string& device_id)
+    {
+        api::CommandHeader_Feedback response;
+        auto status = torqueOn(device_id, response);
         return {
             std::move(status),
             response.success(),
@@ -752,6 +775,23 @@ TEST_F(GrpcArmServiceTest,
     EXPECT_EQ(left_arm_->execute_calls, 0);
 }
 
+TEST_F(GrpcArmServiceTest,
+       TorqueOnAlreadyEnabledArmReturnsOkWithoutDispatchingBackend)
+{
+    aubo_arm_->setPoweredOn(true);
+
+    api::CommandHeader_Feedback response;
+    const auto status = torqueOn("aubo_arm", response);
+
+    ASSERT_TRUE(status.ok()) << status.error_message();
+    EXPECT_TRUE(response.success());
+    EXPECT_EQ(response.error_message(), "RobotArm is already enabled");
+    EXPECT_EQ(
+        response.reason_code(),
+        api::COMMAND_REASON_CODE_NONE);
+    EXPECT_EQ(aubo_arm_->torqueOnCalls(), 0);
+}
+
 TEST_F(GrpcArmServiceTest,
        StopMotionRevokesBlockedMoveJLeaseBeforeMoveLReturns)
 {