From e28a0a43818e4896e8a61e13ec893f059099339a Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 18 Aug 2026 10:46:19 +0800 Subject: [PATCH] skip duplicate torqueOn when already enabled --- .../grpc/server/src/grpc_arm_service.cpp | 18 +++++++ .../server/src/grpc_command_transaction.cpp | 6 ++- .../server/tests/grpc_arm_service_test.cpp | 48 +++++++++++++++++-- 3 files changed, 66 insertions(+), 6 deletions(-) diff --git a/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp index 3c04f608..c1cb01f4 100644 --- a/cmvr-es/service/grpc/server/src/grpc_arm_service.cpp +++ b/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()) { diff --git a/cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp b/cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp index bba5b75e..16549ed3 100644 --- a/cmvr-es/service/grpc/server/src/grpc_command_transaction.cpp +++ b/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()) { diff --git a/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp b/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp index 980d5b7f..f9720de4 100644 --- a/cmvr-es/service/grpc/server/tests/grpc_arm_service_test.cpp +++ b/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 #include #include #include @@ -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 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) {