SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat(grpc): expose arm robot state

Browse Source
This commit is contained in:
linbo 2026-08-03 14:43:12 +08:00
parent 7fb7c4dcbe
commit f57807372d
5 changed files with 252 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
cmvr-es/service/grpc/include/grpc_arm_service.h
View File

@ -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;

25
cmvr-es/service/grpc/src/grpc_arm_client_test.cpp
View File

@ -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();
}
}

156
cmvr-es/service/grpc/src/grpc_arm_service.cpp
View File

@ -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::RobotArm>(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)

73
protos/cmvr/api/arm_command.proto
View File

@ -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;

1
protos/cmvr/api/arm_service.proto
View File

@ -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);