From 061bfc5048b320dceaf82eeb07ae25715e5708c9 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 22 Sep 2026 16:23:50 +0800 Subject: [PATCH] feat expose sdk detected robot arm metadata --- cmvr-es/common/types/arm/arm_types.h | 5 + cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 25 +++++ cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 16 +++ .../service/grpc/include/grpc_arm_service.h | 6 + cmvr-es/service/grpc/src/grpc_arm_service.cpp | 104 ++++++++++++++++++ protos/cmvr/api/arm_command.proto | 95 ++++++++++++++++ protos/cmvr/api/arm_service.proto | 4 + 7 files changed, 255 insertions(+) diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 52b367de..3b53834a 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -106,8 +106,13 @@ struct JointLimit { struct RobotModel { std::string name; + std::string subtype; std::string manufacturer; std::string serial_number; + std::string description_id; + std::string default_base_frame; + std::string default_tcp_frame; + bool detected_from_sdk{false}; std::size_t dof{0}; std::vector joint_names; std::vector joint_limits; diff --git a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp index d850384a..f354c92a 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -389,6 +389,9 @@ AuboArm::AuboArm(const config::RobotArmConfig& cfg) const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; model_.name = vendor_cfg_.model().empty() ? "AuboARM" : vendor_cfg_.model(); model_.manufacturer = vendorBrandName(vendor_cfg_.brand()); + model_.description_id = model_.name; + model_.default_base_frame = vendor_cfg_.base_frame(); + model_.default_tcp_frame = vendor_cfg_.tool_frame(); model_.dof = dof; model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); if (model_.joint_names.empty()) { @@ -1550,6 +1553,28 @@ Result AuboArm::connect(const std::string& ip, const int port) sdk_->rpc_client->setRequestTimeout(1000); sdk_->rpc_client->connect(ip, port > 0 ? port : 30004); sdk_->rpc_client->login(username_, password_); + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (!robot_names.empty()) { + const auto robot_config = + sdk_->rpc_client->getRobotInterface(robot_names.front())->getRobotConfig(); + const std::string sdk_model = robot_config->getRobotType(); + const std::string sdk_subtype = robot_config->getRobotSubType(); + if (!sdk_model.empty()) { + model_.name = sdk_model; + model_.description_id = sdk_model; + model_.subtype = sdk_subtype; + model_.detected_from_sdk = true; + CMVR_LOG(INFO) << "[AuboArm] SDK detected model, id=" << id_ + << ", model=" << model_.name + << ", subtype=" << model_.subtype; + } + } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) << "[AuboArm] SDK model detection failed, id=" << id_ + << ", using configured model=" << model_.name + << ": " << e.what(); + } ip_ = ip; port_ = port > 0 ? port : 30004; { diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 672896a4..77613b38 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -96,6 +96,9 @@ HuayanRobot::HuayanRobot(const config::RobotArmConfig& cfg) const auto dof = vendor_cfg_.dof() > 0 ? static_cast(vendor_cfg_.dof()) : 6U; model_.name = vendor_cfg_.model().empty() ? "HuayanRobot" : vendor_cfg_.model(); model_.manufacturer = "Huayan"; + model_.description_id = model_.name; + model_.default_base_frame = ucs_name_; + model_.default_tcp_frame = tcp_name_; model_.dof = dof; model_.joint_names.assign(vendor_cfg_.joint_names().begin(), vendor_cfg_.joint_names().end()); if (model_.joint_names.empty()) { @@ -613,6 +616,19 @@ Result HuayanRobot::connect(const std::string& ip, const int port) connected_.store(false); return result; } + std::string sdk_model; + const int model_result = HRIF_ReadRobotModel(box_id_, sdk_model); + if (model_result == 0 && !sdk_model.empty()) { + model_.name = sdk_model; + model_.description_id = sdk_model; + model_.detected_from_sdk = true; + CMVR_LOG(INFO) << "[HuayanRobot] SDK detected model, id=" << id_ + << ", model=" << model_.name; + } else { + CMVR_LOG(WARNING) << "[HuayanRobot] SDK model detection failed, id=" << id_ + << ", code=" << model_result + << ", using configured model=" << model_.name; + } ip_ = ip; port_ = use_port; connected_.store(true); diff --git a/cmvr-es/service/grpc/include/grpc_arm_service.h b/cmvr-es/service/grpc/include/grpc_arm_service.h index 76bb44b3..37543af0 100644 --- a/cmvr-es/service/grpc/include/grpc_arm_service.h +++ b/cmvr-es/service/grpc/include/grpc_arm_service.h @@ -44,9 +44,15 @@ public: grpc::Status stopMotion(grpc::ServerContext* context, const api::CommandHeader_Request* request, api::CommandHeader_Feedback* response) override; + grpc::Status GetArmInfo(grpc::ServerContext* context, + const api::GetArmInfo_Request* request, + api::GetArmInfo_Response* response) override; 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_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index c0fe66d9..3dd63f1d 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -93,6 +93,56 @@ api::CartesianPose toApiCartesianPose(const device::CartesianPose& src) return dst; } +void fillJointState(api::JointState* dst, + const device::JointGroupState& src, + const std::vector& joint_names, + const double timestamp) +{ + for (const auto& name : joint_names) dst->add_name(name); + for (const double value : src.position) dst->add_position(value); + for (const double value : src.velocity) dst->add_velocity(value); + for (const double value : src.effort) dst->add_effort(value); + dst->set_timestamp(timestamp); +} + +void fillRobotState(api::RobotState* dst, + const device::ArmState& src, + const device::RobotModel& model) +{ + dst->set_timestamp(src.timestamp); + dst->set_robot_mode(static_cast(static_cast(src.robot_mode))); + dst->set_safety_mode(static_cast(static_cast(src.safety_mode))); + dst->set_control_mode(static_cast(static_cast(src.control_mode))); + dst->set_connected(src.connected); + dst->set_powered_on(src.powered_on); + dst->set_brake_released(src.brake_released); + dst->set_moving(src.moving); + dst->set_program_running(src.program_running); + dst->set_protective_stopped(src.protective_stopped); + dst->set_emergency_stopped(src.emergency_stopped); + dst->set_fault(src.fault); + dst->set_speed_scaling(src.speed_scaling); + fillJointState(dst->mutable_actual_joint_state(), src.actual_joint_state, + model.joint_names, src.timestamp); + fillJointState(dst->mutable_target_joint_state(), src.target_joint_state, + model.joint_names, src.timestamp); + *dst->mutable_actual_tcp_pose() = toApiCartesianPose(src.actual_tcp_pose); + auto* velocity = dst->mutable_actual_tcp_velocity(); + velocity->set_vx(src.actual_tcp_velocity.vx); + velocity->set_vy(src.actual_tcp_velocity.vy); + velocity->set_vz(src.actual_tcp_velocity.vz); + velocity->set_wx(src.actual_tcp_velocity.wx); + velocity->set_wy(src.actual_tcp_velocity.wy); + velocity->set_wz(src.actual_tcp_velocity.wz); + auto* wrench = dst->mutable_actual_tcp_wrench(); + wrench->set_fx(src.actual_tcp_wrench.fx); + wrench->set_fy(src.actual_tcp_wrench.fy); + wrench->set_fz(src.actual_tcp_wrench.fz); + wrench->set_tx(src.actual_tcp_wrench.tx); + wrench->set_ty(src.actual_tcp_wrench.ty); + wrench->set_tz(src.actual_tcp_wrench.tz); +} + device::CartesianVelocity toCartesianVelocity(const api::CartesianVelocity& src) { return {src.vx(), src.vy(), src.vz(), src.wx(), src.wy(), src.wz()}; @@ -490,6 +540,39 @@ grpc::Status gRPCArmServiceImpl::stopMotion(grpc::ServerContext*, } } +grpc::Status gRPCArmServiceImpl::GetArmInfo(grpc::ServerContext*, + const api::GetArmInfo_Request* request, + api::GetArmInfo_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(); + response->set_device_id(device_id); + response->set_manufacturer(model.manufacturer); + response->set_model(model.name); + response->set_subtype(model.subtype); + response->set_dof(static_cast(model.dof)); + for (const auto& name : model.joint_names) response->add_joint_names(name); + response->set_default_base_frame(model.default_base_frame); + response->set_default_tcp_frame(model.default_tcp_frame); + response->set_description_id(model.description_id); + response->set_detected_from_sdk(model.detected_from_sdk); + response->set_driver_type(arm->typeName()); + fillFeedback(response->mutable_header(), true); + CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (GetArmInfo): success, id=" << device_id + << ", manufacturer=" << model.manufacturer + << ", model=" << model.name << ", dof=" << model.dof; + 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::getJointState(grpc::ServerContext*, const api::JointRequest* request, api::JointResponse* response) @@ -520,6 +603,27 @@ 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(response->mutable_state(), state, model); + fillFeedback(response->mutable_header(), true); + 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 5aac4619..024a0700 100644 --- a/protos/cmvr/api/arm_command.proto +++ b/protos/cmvr/api/arm_command.proto @@ -16,6 +16,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 { // 各关节目标角度，单位：rad。 @@ -76,6 +108,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; +} + // 4x4 齐次变换矩阵；前三列为无量纲旋转矩阵，第四列前三项为平移量（m）。 message TransformMatrix4x4 { // 第一行：m00~m02 无单位，m03 单位为 m。 @@ -238,6 +279,60 @@ message JointResponse { JointState state = 2; } +// 机械臂静态能力与 SDK 自动识别信息。 +message GetArmInfo { + message Request { + CommandHeader.Request header = 1; + } + + message Response { + CommandHeader.Feedback header = 1; + string device_id = 2; + string manufacturer = 3; + string model = 4; + string subtype = 5; + uint32 dof = 6; + repeated string joint_names = 7; + string default_base_frame = 8; + string default_tcp_frame = 9; + string description_id = 10; + bool detected_from_sdk = 11; + string driver_type = 12; + } +} + +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; + } +} + // 查询 TCP 位姿。 message GetPose { // 位姿查询请求。 diff --git a/protos/cmvr/api/arm_service.proto b/protos/cmvr/api/arm_service.proto index 64db1d18..ee816fa3 100644 --- a/protos/cmvr/api/arm_service.proto +++ b/protos/cmvr/api/arm_service.proto @@ -29,8 +29,12 @@ service ArmService { rpc servoJ(ServoJ.Request) returns (ServoJ.Response); // 停止当前机械臂运动；不包含数值参数，无单位。 rpc stopMotion(CommandHeader.Request) returns (CommandHeader.Feedback); + // 查询机械臂 SDK 自动识别的型号、自由度、关节名及默认坐标系。 + rpc GetArmInfo(GetArmInfo.Request) returns (GetArmInfo.Response); // 读取关节状态；角度 rad、角速度 rad/s、力矩 N·m。 rpc getJointState(JointRequest) returns (JointResponse); + // 一次性读取完整机械臂运行状态、关节状态及 TCP 位姿。 + rpc getRobotState(GetRobotState.Request) returns (GetRobotState.Response); // 读取 TCP 位姿；位置单位为 m，姿态单位为 rad。 rpc getPose(GetPose.Request) returns (GetPose.Response); // 对指定关节执行零位标定；关节名称无单位。