From 09dad196dba7a9f1e4cbf9b645ae4779a79f783f Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 22 Sep 2026 15:48:33 +0800 Subject: [PATCH 1/4] fix system status device metadata --- .../service/grpc/src/grpc_system_service.cpp | 111 +++++++++++++----- protos/cmvr/api/system_command.proto | 31 +++++ 2 files changed, 111 insertions(+), 31 deletions(-) diff --git a/cmvr-es/service/grpc/src/grpc_system_service.cpp b/cmvr-es/service/grpc/src/grpc_system_service.cpp index b79c036f..69c9002b 100644 --- a/cmvr-es/service/grpc/src/grpc_system_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_system_service.cpp @@ -11,6 +11,79 @@ using namespace cmvr::device; using namespace cmvr::service; +namespace { + +namespace device = cmvr::device; + +cmvr::api::DeviceType toApiDeviceType(device::DeviceKind kind) +{ + switch (kind) { + case device::DeviceKind::AGV: return cmvr::api::DeviceType::AGV; + case device::DeviceKind::Arm: + case device::DeviceKind::Robot: return cmvr::api::DeviceType::Robot; + case device::DeviceKind::Battery: return cmvr::api::DeviceType::Battery; + case device::DeviceKind::BioHead: return cmvr::api::DeviceType::BioHead; + case device::DeviceKind::Camera: return cmvr::api::DeviceType::Camera; + case device::DeviceKind::CanBus: return cmvr::api::DeviceType::CanBus; + case device::DeviceKind::DexHand: return cmvr::api::DeviceType::DexHand; + case device::DeviceKind::Gripper: return cmvr::api::DeviceType::Gripper; + case device::DeviceKind::Microphone: return cmvr::api::DeviceType::Microphone; + case device::DeviceKind::Motor: return cmvr::api::DeviceType::Motor; + case device::DeviceKind::MotorSystem: return cmvr::api::DeviceType::MotorSystem; + case device::DeviceKind::MujocoViewer: return cmvr::api::DeviceType::MujocoViewer; + case device::DeviceKind::MujocoWorld: return cmvr::api::DeviceType::MujocoWorld; + case device::DeviceKind::Speaker: return cmvr::api::DeviceType::Speaker; + case device::DeviceKind::Unknown: + default: return cmvr::api::DeviceType::Unknown; + } +} + +cmvr::api::DeviceKind toApiDeviceKind(device::DeviceKind kind) +{ + switch (kind) { + case device::DeviceKind::AGV: return cmvr::api::DEVICE_KIND_AGV; + case device::DeviceKind::Arm: return cmvr::api::DEVICE_KIND_ARM; + case device::DeviceKind::Battery: return cmvr::api::DEVICE_KIND_BATTERY; + case device::DeviceKind::BioHead: return cmvr::api::DEVICE_KIND_BIO_HEAD; + case device::DeviceKind::Camera: return cmvr::api::DEVICE_KIND_CAMERA; + case device::DeviceKind::CanBus: return cmvr::api::DEVICE_KIND_CAN_BUS; + case device::DeviceKind::DexHand: return cmvr::api::DEVICE_KIND_DEX_HAND; + case device::DeviceKind::Gripper: return cmvr::api::DEVICE_KIND_GRIPPER; + case device::DeviceKind::Microphone: return cmvr::api::DEVICE_KIND_MICROPHONE; + case device::DeviceKind::Motor: return cmvr::api::DEVICE_KIND_MOTOR; + case device::DeviceKind::MotorSystem: return cmvr::api::DEVICE_KIND_MOTOR_SYSTEM; + case device::DeviceKind::MujocoViewer: return cmvr::api::DEVICE_KIND_MUJOCO_VIEWER; + case device::DeviceKind::MujocoWorld: return cmvr::api::DEVICE_KIND_MUJOCO_WORLD; + case device::DeviceKind::Robot: return cmvr::api::DEVICE_KIND_ROBOT; + case device::DeviceKind::Speaker: return cmvr::api::DEVICE_KIND_SPEAKER; + case device::DeviceKind::Unknown: + default: return cmvr::api::DEVICE_KIND_UNKNOWN; + } +} + +std::string vendorForDriverType(const std::string& driver_type) +{ + if (driver_type == "AuboARM") return "AUBO"; + if (driver_type == "HuayanRobot") return "HUAYAN"; + if (driver_type == "SeerRobokitAgv") return "SEER"; + if (driver_type == "HikvisionCamera") return "HIKVISION"; + if (driver_type == "RealSenseCamera") return "Intel RealSense"; + if (driver_type == "MechmindCamera") return "Mech-Mind"; + if (driver_type == "RH56DFTPDexhand") return "Inspire Robots"; + if (driver_type == "ZeroSimTouchDexHand") return "ZeroSim"; + if (driver_type == "EyouMotor") return "EYOU"; + if (driver_type == "Ti5Motor") return "TI5"; + if (driver_type == "MujocoMotor" || driver_type == "MujocoCamera") return "MuJoCo"; + if (driver_type == "MyAgv" || driver_type == "MotorRobotArm" + || driver_type == "BioHeadRobot") return "CMVR"; + if (driver_type == "FFMpegMicroPhone" || driver_type == "FFMpegSpeaker") return "FFmpeg"; + if (driver_type == "UVCCamera") return "UVC"; + if (driver_type == "PX6AXGen3") return "PX6AX"; + return {}; +} + +} // namespace + gRPCSystemServiceImpl::gRPCSystemServiceImpl() : dmgr_(DeviceManager::getInstance()), action_queue_(std::make_unique( @@ -56,38 +129,14 @@ grpc::Status gRPCSystemServiceImpl::GetSystemStatus(grpc::ServerContext* context const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) { try { - std::list> dev_list; - dmgr_.getDeviceList(dev_list); - for (auto &pair: dev_list) { + const auto snapshot = dmgr_.snapshot(); + for (const auto& device : snapshot.devices) { auto* dev = response->add_device_list(); - dev->set_device_id(pair.first); - if (pair.second == "AGV") { - dev->set_device_type(api::DeviceType::AGV); - } - else if (pair.second == "Battery") { - dev->set_device_type(api::DeviceType::Battery); - } - else if (pair.second == "Camera") { - dev->set_device_type(api::DeviceType::Camera); - } - else if (pair.second == "DexHand") { - dev->set_device_type(api::DeviceType::DexHand); - } - else if (pair.second == "Gripper") { - dev->set_device_type(api::DeviceType::Gripper); - } - else if (pair.second == "Microphone") { - dev->set_device_type(api::DeviceType::Microphone); - } - else if (pair.second == "Robot") { - dev->set_device_type(api::DeviceType::Robot); - } - else if (pair.second == "Speaker") { - dev->set_device_type(api::DeviceType::Speaker); - } - else if (pair.second == "Unknown") { - dev->set_device_type(api::DeviceType::Unknown); - } + dev->set_device_id(device.id); + dev->set_device_type(toApiDeviceType(device.kind)); + dev->set_device_kind(toApiDeviceKind(device.kind)); + dev->set_driver_type(device.type_name); + dev->set_vendor(vendorForDriverType(device.type_name)); } response->mutable_header()->set_success(true); setCurrentTimestamp(response->mutable_header()->mutable_timestamp()); diff --git a/protos/cmvr/api/system_command.proto b/protos/cmvr/api/system_command.proto index ac91ae03..bd35ae70 100644 --- a/protos/cmvr/api/system_command.proto +++ b/protos/cmvr/api/system_command.proto @@ -14,12 +14,43 @@ enum DeviceType { Microphone = 5; Robot = 6; Speaker = 7; + BioHead = 8; + Motor = 9; + MotorSystem = 10; + MujocoWorld = 11; + MujocoViewer = 12; + CanBus = 13; Unknown = 20; } +// 设备在 cmvr-es 运行时中的稳定大类，与具体厂商驱动无关。 +enum DeviceKind { + DEVICE_KIND_UNKNOWN = 0; + DEVICE_KIND_AGV = 1; + DEVICE_KIND_ARM = 2; + DEVICE_KIND_BATTERY = 3; + DEVICE_KIND_BIO_HEAD = 4; + DEVICE_KIND_CAMERA = 5; + DEVICE_KIND_CAN_BUS = 6; + DEVICE_KIND_DEX_HAND = 7; + DEVICE_KIND_GRIPPER = 8; + DEVICE_KIND_MICROPHONE = 9; + DEVICE_KIND_MOTOR = 10; + DEVICE_KIND_MOTOR_SYSTEM = 11; + DEVICE_KIND_MUJOCO_VIEWER = 12; + DEVICE_KIND_MUJOCO_WORLD = 13; + DEVICE_KIND_ROBOT = 14; + DEVICE_KIND_SPEAKER = 15; +} + message DeviceList { string device_id = 1; DeviceType device_type = 2; + DeviceKind device_kind = 3; + // 具体运行时驱动类型，例如 AuboARM、HuayanRobot、SeerRobokitAgv。 + string driver_type = 4; + // 厂商或驱动生态，例如 AUBO、HUAYAN、SEER。 + string vendor = 5; } message GetSystemInfoCommand { 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 2/4] 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); // 对指定关节执行零位标定；关节名称无单位。 From 51bdbb9776ea6401609a37c190faa2f31b3f3517 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Tue, 22 Sep 2026 16:37:26 +0800 Subject: [PATCH 3/4] fix include aubo tcp pose in robot state --- cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 1 + 1 file changed, 1 insertion(+) 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 f354c92a..f6645c3d 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -638,6 +638,7 @@ ArmState AuboArm::getRobotState() const state.speed_scaling = speed_scaling_; state.actual_joint_state = getJointState(); state.target_joint_state = state.actual_joint_state; + state.actual_tcp_pose = getTcpPose(FrameType::Base); return state; } From 9b2f5773e692706e4c618e23889befc18cbbc579 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Wed, 23 Sep 2026 16:04:01 +0800 Subject: [PATCH 4/4] fix report arm freedrive control mode --- .../devices/arm/aubo_arm/include/aubo_arm.h | 2 +- cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 33 +++++++++++++++++++ cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp | 28 ++++++++++++++++ cmvr-es/devices/arm/huayan_arm/huayan_arm.h | 2 +- 4 files changed, 63 insertions(+), 2 deletions(-) diff --git a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h index fc2d7a71..97270398 100644 --- a/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h +++ b/cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h @@ -34,7 +34,7 @@ public: CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override { return ControlMode::Position; } + ControlMode getControlMode() const override; bool supportsActionQueueMotion() const noexcept override { return true; } Result listBaseFrame(std::vector& frame_names) const override; Result listTCPFrame(std::vector& frame_names) const override; 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 f6645c3d..f19621f3 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -733,6 +733,39 @@ SafetyMode AuboArm::getSafetyMode() const return static_cast(hardware_safety_mode_.load()); } +ControlMode AuboArm::getControlMode() const +{ + if (!connected_.load()) { + return ControlMode::None; + } + +#if defined(CMVR_HAS_AUBO_SDK) + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + return ControlMode::Position; + } + const auto robot_interface = + sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface) { + return ControlMode::Position; + } + + const auto robot_state = robot_interface->getRobotState(); + if (robot_state && + robot_state->getRobotModeType() == + arcs::common_interface::RobotModeType::BackDrive) { + return ControlMode::Freedrive; + } + } catch (const std::exception&) { + // Keep telemetry available when a controller version does not support + // one of the optional hand-guiding status queries. + } +#endif + + return ControlMode::Position; +} + bool AuboArm::isEmergencyStopped() const { return emergency_stopped_.load() || hardware_emergency_stopped_.load(); diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp index 77613b38..8c64de69 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.cpp @@ -283,6 +283,34 @@ Result HuayanRobot::setSpeedScaling(const double scaling) return Result::success(); } +ControlMode HuayanRobot::getControlMode() const +{ + if (!isConnected()) { + return ControlMode::None; + } + + int tri_stage_enabled = 0; + int tri_stage_mode = 1; + int force_control_state = 0; + int tri_stage_result = -1; + int force_state_result = -1; + { + std::lock_guard lock(mutex_); + tri_stage_result = HRIF_ReadTriStageSwitch( + box_id_, robot_id_, tri_stage_enabled, tri_stage_mode); + force_state_result = HRIF_ReadForceControlState( + box_id_, robot_id_, force_control_state); + } + const bool tri_stage_freedrive = + tri_stage_result == 0 && tri_stage_enabled != 0 && tri_stage_mode == 0; + const bool force_freedrive = + force_state_result == 0 && force_control_state == 3; + if (tri_stage_freedrive || force_freedrive) { + return ControlMode::Freedrive; + } + return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; +} + bool HuayanRobot::isProtectiveStopped() const { const auto state = readHrState_(); diff --git a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h index 850f22b9..0ebccc17 100644 --- a/cmvr-es/devices/arm/huayan_arm/huayan_arm.h +++ b/cmvr-es/devices/arm/huayan_arm/huayan_arm.h @@ -38,7 +38,7 @@ public: CartesianPose getTcpPose(FrameType frame = FrameType::Base) const override; RobotMode getRobotMode() const override; SafetyMode getSafetyMode() const override; - ControlMode getControlMode() const override { return servo_mode_.load() ? ControlMode::Servo : ControlMode::Position; } + ControlMode getControlMode() const override; bool supportsActionQueueMotion() const noexcept override { return true; } Result listBaseFrame(std::vector& frame_names) const override; Result listTCPFrame(std::vector& frame_names) const override;