From f57807372d4c6f79b7ed50d68082e2af1fa43fed Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Mon, 3 Aug 2026 14:43:12 +0800 Subject: [PATCH] feat(grpc): expose arm robot state --- .../service/grpc/include/grpc_arm_service.h | 3 + .../service/grpc/src/grpc_arm_client_test.cpp | 25 +++ cmvr-es/service/grpc/src/grpc_arm_service.cpp | 156 +++++++++++++++++- protos/cmvr/api/arm_command.proto | 73 ++++++++ protos/cmvr/api/arm_service.proto | 1 + 5 files changed, 252 insertions(+), 6 deletions(-) diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h index 07a2dc30..3133cc0c 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ b/cmvr-es/service/grpc/include/grpc_arm_service.h @@ -38,6 +38,9 @@ public: grpc::Status getJointState(grpc::ServerContext* context, const api::JointRequest* request, api::JointResponse* response) override; + grpc::Status getRobotState(grpc::ServerContext* context, + const api::GetRobotState_Request* request, + api::GetRobotState_Response* response) override; grpc::Status getPose(grpc::ServerContext* context, const api::GetPose_Request* request, api::GetPose_Response* response) override; diff --git a/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp b/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp index ff1af4d8..6559a1e2 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_client_test.cpp @@ -82,3 +82,28 @@ TEST(GrpcArmClientTest, GetJointState) CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); } } + +TEST(GrpcArmClientTest, GetRobotState) +{ + auto stub = makeStub(); + cmvr::api::GetRobotState_Request request; + fillHeader(request.mutable_header()); + + cmvr::api::GetRobotState_Response response; + grpc::ClientContext context; + const grpc::Status status = stub->getRobotState(&context, request, &response); + + if (status.ok()) { + const auto& state = response.state(); + CMVR_LOG(INFO) << "Robot state: connected=" << state.connected() + << ", powered_on=" << state.powered_on() + << ", moving=" << state.moving() + << ", fault=" << state.fault() + << ", robot_mode=" << state.robot_mode() + << ", safety_mode=" << state.safety_mode() + << ", control_mode=" << state.control_mode() + << ", joints=" << state.actual_joint_state().name_size(); + } else { + CMVR_LOG(ERROR) << "RPC failed: " << status.error_message(); + } +} diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index 1ae88019..60dc3a26 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -92,6 +92,131 @@ api::CartesianPose toApiCartesianPose(const device::CartesianPose& src) return dst; } +api::CartesianVelocity toApiCartesianVelocity(const device::CartesianVelocity& src) +{ + api::CartesianVelocity dst; + dst.set_vx(src.vx); + dst.set_vy(src.vy); + dst.set_vz(src.vz); + dst.set_wx(src.wx); + dst.set_wy(src.wy); + dst.set_wz(src.wz); + return dst; +} + +api::CartesianWrench toApiCartesianWrench(const device::CartesianWrench& src) +{ + api::CartesianWrench dst; + dst.set_fx(src.fx); + dst.set_fy(src.fy); + dst.set_fz(src.fz); + dst.set_tx(src.tx); + dst.set_ty(src.ty); + dst.set_tz(src.tz); + return dst; +} + +api::ArmRobotMode toApiRobotMode(const device::RobotMode mode) +{ + switch (mode) { + case device::RobotMode::Disconnected: + return api::ARM_ROBOT_MODE_DISCONNECTED; + case device::RobotMode::PowerOff: + return api::ARM_ROBOT_MODE_POWER_OFF; + case device::RobotMode::Idle: + return api::ARM_ROBOT_MODE_IDLE; + case device::RobotMode::Running: + return api::ARM_ROBOT_MODE_RUNNING; + case device::RobotMode::Paused: + return api::ARM_ROBOT_MODE_PAUSED; + case device::RobotMode::Stopped: + return api::ARM_ROBOT_MODE_STOPPED; + case device::RobotMode::Fault: + return api::ARM_ROBOT_MODE_FAULT; + case device::RobotMode::Unknown: + default: + return api::ARM_ROBOT_MODE_UNKNOWN; + } +} + +api::ArmSafetyMode toApiSafetyMode(const device::SafetyMode mode) +{ + switch (mode) { + case device::SafetyMode::Normal: + return api::ARM_SAFETY_MODE_NORMAL; + case device::SafetyMode::Reduced: + return api::ARM_SAFETY_MODE_REDUCED; + case device::SafetyMode::ProtectiveStop: + return api::ARM_SAFETY_MODE_PROTECTIVE_STOP; + case device::SafetyMode::EmergencyStop: + return api::ARM_SAFETY_MODE_EMERGENCY_STOP; + case device::SafetyMode::SafeguardStop: + return api::ARM_SAFETY_MODE_SAFEGUARD_STOP; + case device::SafetyMode::SystemEmergencyStop: + return api::ARM_SAFETY_MODE_SYSTEM_EMERGENCY_STOP; + case device::SafetyMode::Fault: + return api::ARM_SAFETY_MODE_FAULT; + case device::SafetyMode::Unknown: + default: + return api::ARM_SAFETY_MODE_UNKNOWN; + } +} + +api::ArmControlMode toApiControlMode(const device::ControlMode mode) +{ + switch (mode) { + case device::ControlMode::Manual: + return api::ARM_CONTROL_MODE_MANUAL; + case device::ControlMode::Position: + return api::ARM_CONTROL_MODE_POSITION; + case device::ControlMode::Velocity: + return api::ARM_CONTROL_MODE_VELOCITY; + case device::ControlMode::Torque: + return api::ARM_CONTROL_MODE_TORQUE; + case device::ControlMode::Servo: + return api::ARM_CONTROL_MODE_SERVO; + case device::ControlMode::Freedrive: + return api::ARM_CONTROL_MODE_FREEDRIVE; + case device::ControlMode::None: + default: + return api::ARM_CONTROL_MODE_NONE; + } +} + +void fillJointState(const device::RobotModel& model, + const device::JointGroupState& state, + api::JointState* msg) +{ + for (const auto& name : model.joint_names) msg->add_name(name); + for (const double value : state.position) msg->add_position(value); + for (const double value : state.velocity) msg->add_velocity(value); + for (const double value : state.effort) msg->add_effort(value); +} + +void fillRobotState(const device::RobotModel& model, + const device::ArmState& state, + api::RobotState* msg) +{ + msg->set_timestamp(state.timestamp); + msg->set_robot_mode(toApiRobotMode(state.robot_mode)); + msg->set_safety_mode(toApiSafetyMode(state.safety_mode)); + msg->set_control_mode(toApiControlMode(state.control_mode)); + msg->set_connected(state.connected); + msg->set_powered_on(state.powered_on); + msg->set_brake_released(state.brake_released); + msg->set_moving(state.moving); + msg->set_program_running(state.program_running); + msg->set_protective_stopped(state.protective_stopped); + msg->set_emergency_stopped(state.emergency_stopped); + msg->set_fault(state.fault); + msg->set_speed_scaling(state.speed_scaling); + fillJointState(model, state.actual_joint_state, msg->mutable_actual_joint_state()); + fillJointState(model, state.target_joint_state, msg->mutable_target_joint_state()); + *msg->mutable_actual_tcp_pose() = toApiCartesianPose(state.actual_tcp_pose); + *msg->mutable_actual_tcp_velocity() = toApiCartesianVelocity(state.actual_tcp_velocity); + *msg->mutable_actual_tcp_wrench() = toApiCartesianWrench(state.actual_tcp_wrench); +} + device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src) { return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()}; @@ -327,12 +452,7 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*, const auto model = arm->getRobotModel(); const auto state = arm->getJointState(); auto* msg = response->mutable_state(); - for (const auto& name : model.joint_names) { - msg->add_name(name); - } - for (double v : state.position) msg->add_position(v); - for (double v : state.velocity) msg->add_velocity(v); - for (double v : state.effort) msg->add_effort(v); + fillJointState(model, state, msg); fillFeedback(response->mutable_header(), true); // CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (getJointState): success, id=" << device_id // << ", joints=" << msg->name_size() @@ -344,6 +464,30 @@ grpc::Status gRPCArmServiceImpl::getJointState(grpc::ServerContext*, } } +grpc::Status gRPCArmServiceImpl::getRobotState( + grpc::ServerContext*, + const api::GetRobotState_Request* request, + api::GetRobotState_Response* response) +{ + try { + const std::string device_id = request->header().device_id(); + auto arm = dmgr_.getDevice(device_id); + if (!arm) { + return setDeviceNotFound(response, device_id); + } + + const auto model = arm->getRobotModel(); + const auto state = arm->getRobotState(); + fillRobotState(model, state, response->mutable_state()); + fillFeedback(response->mutable_header(), true); + logRpcSuccess("getRobotState", device_id); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillFeedback(response->mutable_header(), false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} + grpc::Status gRPCArmServiceImpl::getPose(grpc::ServerContext*, const api::GetPose_Request* request, api::GetPose_Response* response) diff --git a/protos/cmvr/api/arm_command.proto b/protos/cmvr/api/arm_command.proto index 3c0d7b99..53fbf804 100644 --- a/protos/cmvr/api/arm_command.proto +++ b/protos/cmvr/api/arm_command.proto @@ -11,6 +11,38 @@ enum ArmFrameType { ARM_FRAME_USER = 3; } +enum ArmRobotMode { + ARM_ROBOT_MODE_UNKNOWN = 0; + ARM_ROBOT_MODE_DISCONNECTED = 1; + ARM_ROBOT_MODE_POWER_OFF = 2; + ARM_ROBOT_MODE_IDLE = 3; + ARM_ROBOT_MODE_RUNNING = 4; + ARM_ROBOT_MODE_PAUSED = 5; + ARM_ROBOT_MODE_STOPPED = 6; + ARM_ROBOT_MODE_FAULT = 7; +} + +enum ArmSafetyMode { + ARM_SAFETY_MODE_UNKNOWN = 0; + ARM_SAFETY_MODE_NORMAL = 1; + ARM_SAFETY_MODE_REDUCED = 2; + ARM_SAFETY_MODE_PROTECTIVE_STOP = 3; + ARM_SAFETY_MODE_EMERGENCY_STOP = 4; + ARM_SAFETY_MODE_SAFEGUARD_STOP = 5; + ARM_SAFETY_MODE_SYSTEM_EMERGENCY_STOP = 6; + ARM_SAFETY_MODE_FAULT = 7; +} + +enum ArmControlMode { + ARM_CONTROL_MODE_NONE = 0; + ARM_CONTROL_MODE_MANUAL = 1; + ARM_CONTROL_MODE_POSITION = 2; + ARM_CONTROL_MODE_VELOCITY = 3; + ARM_CONTROL_MODE_TORQUE = 4; + ARM_CONTROL_MODE_SERVO = 5; + ARM_CONTROL_MODE_FREEDRIVE = 6; +} + message JointPositionCommand { repeated double position = 1; } @@ -46,6 +78,15 @@ message CartesianVelocity { double wz = 6; } +message CartesianWrench { + double fx = 1; + double fy = 2; + double fz = 3; + double tx = 4; + double ty = 5; + double tz = 6; +} + message TransformMatrix4x4 { double m00 = 1; double m01 = 2; double m02 = 3; double m03 = 4; double m10 = 5; double m11 = 6; double m12 = 7; double m13 = 8; @@ -133,6 +174,38 @@ message JointResponse { JointState state = 2; } +message RobotState { + double timestamp = 1; + ArmRobotMode robot_mode = 2; + ArmSafetyMode safety_mode = 3; + ArmControlMode control_mode = 4; + bool connected = 5; + bool powered_on = 6; + bool brake_released = 7; + bool moving = 8; + bool program_running = 9; + bool protective_stopped = 10; + bool emergency_stopped = 11; + bool fault = 12; + double speed_scaling = 13; + JointState actual_joint_state = 14; + JointState target_joint_state = 15; + CartesianPose actual_tcp_pose = 16; + CartesianVelocity actual_tcp_velocity = 17; + CartesianWrench actual_tcp_wrench = 18; +} + +message GetRobotState { + message Request { + CommandHeader.Request header = 1; + } + + message Response { + CommandHeader.Feedback header = 1; + RobotState state = 2; + } +} + message GetPose { message Request { CommandHeader.Request header = 1; diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index 92562f4a..a07018f7 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/protos/cmvr/api/arm_service.proto @@ -16,6 +16,7 @@ service ArmService { rpc servoJ(ServoJ.Request) returns (ServoJ.Response); rpc stopMotion(CommandHeader.Request) returns (CommandHeader.Feedback); rpc getJointState(JointRequest) returns (JointResponse); + rpc getRobotState(GetRobotState.Request) returns (GetRobotState.Response); rpc getPose(GetPose.Request) returns (GetPose.Response); rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response); rpc getPoseMatrix(GetPoseMatrix.Request) returns (GetPoseMatrix.Response);