feat expose sdk detected robot arm metadata

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<std::string> joint_names;
     std::vector<JointLimit> joint_limits;

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<std::size_t>(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;
         {

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<std::size_t>(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);

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;

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<std::string>& 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<api::ArmRobotMode>(static_cast<int>(src.robot_mode)));
+    dst->set_safety_mode(static_cast<api::ArmSafetyMode>(static_cast<int>(src.safety_mode)));
+    dst->set_control_mode(static_cast<api::ArmControlMode>(static_cast<int>(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::RobotArm>(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<std::uint32_t>(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::RobotArm>(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)

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 {
   // 位姿查询请求。

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);
   // 对指定关节执行零位标定；关节名称无单位。