From f5b765af3d23090feee2c7c9504f23d785978ac1 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Thu, 6 Nov 2025 15:58:32 +0800 Subject: [PATCH] update robot controller --- config/dev_config.xml | 28 ++- include/controller/touch_controller.h | 114 ------------- include/hardware/can/can_group.h | 2 + .../can/motor_protocol/motorprotocolmanager.h | 3 +- .../service/grpc_service/grpc_hlc_service.h | 25 --- src/controller/CMakeLists.txt | 58 ------- src/controller/touch_controller.cpp | 159 ------------------ src/controller/touch_controller_test.cpp | 71 -------- .../robot/controller/abstractcontroller.h | 7 +- .../robot/controller/controller_manager.cpp | 4 +- .../controller/jointpositioncontroller.cpp | 32 +++- .../robot/humanoid_robot/CMakeLists.txt | 1 + .../robot/humanoid_robot/humanoid_robot.cpp | 24 +++ .../robot/humanoid_robot/humanoid_robot.h | 5 +- src/hardware/can/can_group.cpp | 9 + .../motor_protocol/motorprotocolmanager.cpp | 24 ++- src/hardware_manager/hardware_manager.cpp | 1 + src/main.cpp | 3 - src/service/grpc_service/CMakeLists.txt | 25 --- .../grpc_service/grpc_hlc_client_test.cpp | 46 ----- src/service/grpc_service/grpc_hlc_service.cpp | 40 ----- 21 files changed, 114 insertions(+), 567 deletions(-) delete mode 100644 include/controller/touch_controller.h delete mode 100644 include/service/grpc_service/grpc_hlc_service.h delete mode 100644 src/controller/CMakeLists.txt delete mode 100644 src/controller/touch_controller.cpp delete mode 100644 src/controller/touch_controller_test.cpp delete mode 100644 src/service/grpc_service/grpc_hlc_client_test.cpp delete mode 100644 src/service/grpc_service/grpc_hlc_service.cpp diff --git a/config/dev_config.xml b/config/dev_config.xml index 4feb5aa1..4aad70b6 100644 --- a/config/dev_config.xml +++ b/config/dev_config.xml @@ -69,21 +69,19 @@ - - - - - - - - - - - - - - - + + + + + + + + + + + + + diff --git a/include/controller/touch_controller.h b/include/controller/touch_controller.h deleted file mode 100644 index a92f687a..00000000 --- a/include/controller/touch_controller.h +++ /dev/null @@ -1,114 +0,0 @@ -// -// Created by lgv on 2025/8/24. -// - -#pragma once - -#include - -#include "devices/abstract_camera.h" -#include "devices/abstract_dexhand.h" -#include "devices/abstract_robot.h" -#include "cmvr/msgs/geometry.pb.h" -#include "librealsense2/rs.h" -#include "librealsense2/h/rs_frame.h" -namespace cmvr { - namespace ctrl { - - // 简易版 PID 控制器，带死区、积分限幅、输出限幅和输出斜率限制 - class PID { - public: - PID(double kp, double ki, double kd, - double i_max, - double output_max_pos, double output_max_neg, - double delta_max = 0.0) // 输出变化最大值，0 表示不限制 - : kp_(kp), ki_(ki), kd_(kd), - i_max_(i_max), - output_max_pos_(output_max_pos), - output_max_neg_(output_max_neg), - delta_max_(delta_max), - prev_error_(0), integral_(0), prev_output_(0) {} - - double compute(double target, double current, double dt, double deadband = 0.0) { - double error = target - current; - - // 死区处理 - if (fabs(error) <= deadband) { - error = 0.0; - } - - // 积分累加限幅 - integral_ += error * dt; - if (integral_ > i_max_) integral_ = i_max_; - if (integral_ < -i_max_) integral_ = -i_max_; - - // 微分 - double derivative = (error - prev_error_) / dt; - prev_error_ = error; - - // PID 输出 - double output = kp_ * error + ki_ * integral_ + kd_ * derivative; - - // 输出限幅 - if (output > output_max_pos_) output = output_max_pos_; - if (output < -output_max_neg_) output = -output_max_neg_; - - // 输出斜率限制 - if (delta_max_ > 0.0) { - double delta = output - prev_output_; - if (delta > delta_max_) output = prev_output_ + delta_max_; - else if (delta < -delta_max_) output = prev_output_ - delta_max_; - } - - prev_output_ = output; - return output; - } - - private: - double kp_, ki_, kd_; - double prev_error_; - double integral_; - double i_max_; // 积分限幅 - double output_max_pos_; // 向下按的最大输出 - double output_max_neg_; // 向上抬的最大输出 - double delta_max_; // 输出斜率限制 - double prev_output_; - }; - - - - - class TouchController { - public: - TouchController() {}; - TouchController(std::shared_ptr robot,std::shared_ptr hand,std::shared_ptr cam) - :robot_(std::move(robot)),hand_(std::move(hand)),cam_(std::move(cam)), - pid_(std::make_shared(0.005, 0.001, 0.001, 5000.0, 0.5, 1.0)){} - ~TouchController()=default; - - - bool isArrive(double max_force); - - void touch(int u,int v ,double max_force); - - void touch(std::shared_ptr robot,const msgs::Pose3d pose,const msgs::Pose3d offset); - void touch( msgs::Pose3d pose, msgs::Pose3d offset,double max_force); - - private: - std::shared_ptr robot_{nullptr}; - std::shared_ptr hand_{nullptr}; - std::shared_ptr cam_{nullptr}; - - std::shared_ptr pid_{nullptr}; - - const double touch_threshold_ = 5.0; // 触控判定阈值 - - - // 从压阻矩阵提取触控点与压力 - bool extractTouch(const std::vector>& matrix,double& force, int& x, int& y); - - - - }; - } -} diff --git a/include/hardware/can/can_group.h b/include/hardware/can/can_group.h index f356795c..97e6e856 100644 --- a/include/hardware/can/can_group.h +++ b/include/hardware/can/can_group.h @@ -16,6 +16,8 @@ namespace cmvr::hardware void init(); + std::shared_ptr getMotorProtocol(const std::string &protocolType); + uint8_t getNodeId(const std::string& joint_name); private: XmlNode cfg_; std::string id_; diff --git a/include/hardware/can/motor_protocol/motorprotocolmanager.h b/include/hardware/can/motor_protocol/motorprotocolmanager.h index 2880b08d..2e79f64a 100644 --- a/include/hardware/can/motor_protocol/motorprotocolmanager.h +++ b/include/hardware/can/motor_protocol/motorprotocolmanager.h @@ -21,7 +21,7 @@ namespace cmvr::hardware public: struct MotorInfo { - std::string node_id; //电机ID + uint8_t node_id; //电机ID std::string jointName; //关节名称 double limitQLb; //逆时针限位 double limitQUb; //顺时针限位 @@ -32,6 +32,7 @@ namespace cmvr::hardware std::shared_ptr> manager); ~MotorProtocolManager() = default; + uint8_t getNodeId(const std::string& joint_name); //根据协议类型获取协议实例对象 std::shared_ptr getMotorProtocol(const std::string &protocolType); private: diff --git a/include/service/grpc_service/grpc_hlc_service.h b/include/service/grpc_service/grpc_hlc_service.h deleted file mode 100644 index ecdd5c11..00000000 --- a/include/service/grpc_service/grpc_hlc_service.h +++ /dev/null @@ -1,25 +0,0 @@ -// -// Created by lgv on 2025/8/25. -// -#pragma once - -#include "device_manager/device_manager.h" -#include "cmvr/api/hlc_service.grpc.pb.h" - -namespace cmvr { - namespace service { - class gRPCHlcServiceImpl final : public api::HlcService::Service { - public: - gRPCHlcServiceImpl(); - ~gRPCHlcServiceImpl() = default; - grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; - - private: - device::DeviceManager& dmgr_; - }; - - - - } - -} diff --git a/src/controller/CMakeLists.txt b/src/controller/CMakeLists.txt deleted file mode 100644 index c995c881..00000000 --- a/src/controller/CMakeLists.txt +++ /dev/null @@ -1,58 +0,0 @@ -find_package(glog REQUIRED) -find_package(protobuf REQUIRED) - -add_library(controller SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/touch_controller.cpp -) - -target_include_directories(controller PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -target_link_libraries(controller PRIVATE - protobuf::libprotobuf - glog::glog - cmvr_es::device::humanoid_robot - -) - -add_library(cmvr_es::ctrl::controller ALIAS controller) - - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- -find_package(glog REQUIRED) -find_package(protobuf REQUIRED) -find_package(PkgConfig REQUIRED) -find_package(fcl REQUIRED) -find_package(OpenCV REQUIRED) - -include_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/include -) - -link_directories( - ${CMAKE_SOURCE_DIR}/third_party/gtest/1.17.0/lib -) - - -add_executable(touch_controller_test - ${CMAKE_CURRENT_SOURCE_DIR}/touch_controller_test.cpp -) - - -target_link_libraries(touch_controller_test - PRIVATE - protobuf::libprotobuf - glog::glog - cmvr_es::device::humanoid_robot - cmvr_es::ctrl::controller - gtest - gtest_main - pthread - glog::glog - proto-objects - ccd - fcl - cmvr_es::device_manager - ${OpenCV_LIBS} -) diff --git a/src/controller/touch_controller.cpp b/src/controller/touch_controller.cpp deleted file mode 100644 index c4d60872..00000000 --- a/src/controller/touch_controller.cpp +++ /dev/null @@ -1,159 +0,0 @@ -// -// Created by lgv on 2025/8/24. -// - -#include "controller/touch_controller.h" - -using namespace cmvr::ctrl; -using namespace cmvr::msgs; - -void TouchController::touch(std::shared_ptr robot, const msgs::Pose3d pose, const msgs::Pose3d offset) { - - msgs::Position target_position; - target_position.set_x(pose.position().x() - offset.position().x()); - target_position.set_y(pose.position().y() - offset.position().y()); - target_position.set_z(pose.position().z() - offset.position().z()); - - - msgs::Euler target_euler; - target_euler.set_rx(pose.euler().rx() - offset.euler().rx()); - target_euler.set_ry(pose.euler().ry() - offset.euler().ry()); - target_euler.set_rz(pose.euler().rz() - offset.euler().rz()); - - - msgs::Pose3d target_pose; - *target_pose.mutable_position() = target_position; - *target_pose.mutable_euler() = target_euler; - - robot->moveJ("PELVIS_S","R_WRIST_R_S",target_pose); -} - -void TouchController::touch(msgs::Pose3d pose, msgs::Pose3d offset, double max_force) { - auto hand_data = hand_->getSensorData(); - double force = 0; - int x ,y; - extractTouch(hand_data.index.tip.data,force,x,y); - - double dz = pid_->compute(max_force, force, 0.01,10); - - std::cout << dz << std::endl; - - // LOG(INFO) << "Force : " << force << " dz : " << dz; - - // // 输出 3x3 数组 - // LOG(INFO) << "Tip data (3x3):"; - // for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) { - // std::stringstream ss; - // for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) { - // ss << hand_data.index.tip.data[i][j] << "\t"; - // } - // LOG(INFO) << ss.str(); - // } - - - - - - - -} - - -bool TouchController::extractTouch(const std::vector > &matrix, double &force, int &x, int &y) { - - - int rows = matrix.size(); - int cols = matrix[0].size(); - double maxVal = 0; - int maxX = -1, maxY = -1; - double total = 0; - - for (int i = 0; i < rows; i++) { - for (int j = 0; j < cols; j++) { - double val = matrix[i][j]; - total += val; - if (val > maxVal) { - maxVal = val; - maxX = i; - maxY = j; - } - } - } - - if (maxVal > touch_threshold_) { - force = total; - x = maxX; - y = maxY; - return true; - } - return false; - -} - - -bool TouchController::isArrive(double max_force) { - const auto& hand_data = hand_->getSensorData(); - - double force = 0.0; - int x = 0, y = 0; - extractTouch(hand_data.index.tip.data, force, x, y); - - LOG(INFO) << "Force : " << force ; - - // 输出 3x3 数组 - LOG(INFO) << "Tip data (3x3):"; - for (size_t i = 0; i < hand_data.index.tip.data.size(); ++i) { - std::stringstream ss; - for (size_t j = 0; j < hand_data.index.tip.data[i].size(); ++j) { - ss << hand_data.index.tip.data[i][j] << "\t"; - } - LOG(INFO) << ss.str(); - } - - return force > max_force; -} - -void TouchController::touch(int u, int v, double max_force) { - LOG(INFO) << "Touch request at pixel (" << u << ", " << v << ") with max_force=" << max_force; - - // 获取目标点和当前位姿 - auto target_pose = cam_->get3DPointFromPixel(u, v); - auto cur_pose = robot_->fk("PELVIS_S", "R_FINGER_TIP"); - - LOG(INFO) << "Target 3D Pose: " << target_pose; - - // 定义关键点位 - auto pre_touch_pose = cur_pose; - pre_touch_pose.mutable_position()->set_x(target_pose[0] - 0.05); - pre_touch_pose.mutable_position()->set_y(target_pose[1]); - pre_touch_pose.mutable_position()->set_z(target_pose[2]); - - auto touch_pose = pre_touch_pose; - touch_pose.mutable_position()->set_x(target_pose[0]); - - const auto& retreat_pose = cur_pose; - // retreat_pose.mutable_position()->set_x(target_pose[0] - 0.20); - - // 1. 移动到预接触位置 - robot_->moveJ("PELVIS_S", "R_FINGER_TIP", pre_touch_pose); - - // 2. 向前接触并开启压力监测 - robot_->servoJ("PELVIS_S", "R_FINGER_TIP", touch_pose); - - // 3. 等待达到最大压力 - constexpr int k_sleep_ms = 5; - constexpr int k_timeout_ms = 5000; - int elapsed_ms = 0; - - while (!isArrive(max_force) && elapsed_ms < k_timeout_ms) { - std::this_thread::sleep_for(std::chrono::milliseconds(k_sleep_ms)); - elapsed_ms += k_sleep_ms; - } - - if (elapsed_ms >= k_timeout_ms) { - LOG(WARNING) << "Timeout waiting for force feedback!"; - } else { - LOG(INFO) << "Max force reached, retreating."; - robot_->moveJ("PELVIS_S", "R_FINGER_TIP", retreat_pose); - } -} diff --git a/src/controller/touch_controller_test.cpp b/src/controller/touch_controller_test.cpp deleted file mode 100644 index baa283bf..00000000 --- a/src/controller/touch_controller_test.cpp +++ /dev/null @@ -1,71 +0,0 @@ -// -// Created by lgv on 2025/8/27. -// - - -#include "device_manager/device_manager.h" -#include "gtest/gtest.h" -#include "controller/touch_controller.h" -#include "cmvr/msgs/geometry.pb.h" - - -using namespace cmvr::device; -using namespace cmvr::msgs; -using namespace cmvr::ctrl; - -TEST(TouchControllerTest,MyTest) { - // - std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; - const XmlNode config(config_path); - - if (!config.hasChild("DeviceManager")){ - LOG(ERROR) << "Device Manager node not found"; - } - auto dmgr_cfg = config.getChild("DeviceManager"); - auto &dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("hc01"); - auto hand = dmgr.getDevice("hand1"); - auto cam = dmgr.getDevice("cam4"); - - cmvr::msgs::Pose3d pose; - - pose.mutable_position()->set_x( 1.49969573e-01); - pose.mutable_position()->set_y(-4.00100001e-01); - pose.mutable_position()->set_z(-1.00102800e-01); - - pose.mutable_euler()->set_rx(0); - pose.mutable_euler()->set_ry(0); - pose.mutable_euler()->set_rz(1.57); - - robot->moveJ("PELVIS_S","R_WRIST_R_S",pose); - // robot->seJ("PELVIS_S","R_WRIST_R_S",pose); - // robot->servoJ("PELVIS_S","R_WRIST_R_S",pose,0.5); - - cmvr::msgs::Pose3d delta_pose; - delta_pose.mutable_position()->set_x( 0.01); - delta_pose.mutable_position()->set_y(0); - delta_pose.mutable_position()->set_z(0); - - delta_pose.mutable_euler()->set_rx(0); - delta_pose.mutable_euler()->set_ry(0); - delta_pose.mutable_euler()->set_rz(0); - - - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.02); - - Pose3d offset; - - TouchController controller(robot, hand,cam); - - while (true) { - // controller.touch(pose,offset,500); - if (controller.isArrive(500)) { - delta_pose.mutable_position()->set_x( -0.01); - robot->servoDeltaJ("PELVIS_S","R_FINGER_TIP",delta_pose,0.05); - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - -} \ No newline at end of file diff --git a/src/devices/robot/controller/abstractcontroller.h b/src/devices/robot/controller/abstractcontroller.h index a9815d01..ab371361 100644 --- a/src/devices/robot/controller/abstractcontroller.h +++ b/src/devices/robot/controller/abstractcontroller.h @@ -19,15 +19,16 @@ namespace cmvr::device class AbstractController { public: - explicit AbstractController(const XmlNode& cfg); - virtual ~AbstractController(); + explicit AbstractController(const XmlNode& cfg){} + virtual ~AbstractController(){} [[nodiscard]] ControllerState getState() const {return state_;} //此处的Json中应该包含目标电机信息，电机id，canGroupId等其他必要参数，用以确定是调用哪个can实例发送消息 // 还要包含操作内容，比如要执行的是直接控制每个电机位置，还是指定末端关节位置 /* - {“canGroupId”:"",motors:[{"joint_name":"","id":""},{"joint_name":"","id":""}]} + "params":{“canGroupId”:"",motors:[{"joint_name":"","id":""},{"joint_name":"","id":""}]}, + "operate":{} **/ virtual void call(const Json::Value& json) = 0; diff --git a/src/devices/robot/controller/controller_manager.cpp b/src/devices/robot/controller/controller_manager.cpp index 6470287d..ac53ade7 100644 --- a/src/devices/robot/controller/controller_manager.cpp +++ b/src/devices/robot/controller/controller_manager.cpp @@ -11,7 +11,7 @@ using namespace cmvr::device; ControllerManager::ControllerManager(const XmlNode& cfg):state_(ControlManagerState_Idle) { - + create(cfg); } void ControllerManager::clearError() @@ -92,8 +92,6 @@ void ControllerManager::switchMode(ControlManagerState state) break; } state_ = state; - - } ComponentGroup& ControllerManager::getComponentGroup(const std::string& id) diff --git a/src/devices/robot/controller/jointpositioncontroller.cpp b/src/devices/robot/controller/jointpositioncontroller.cpp index cc61b178..531427a7 100644 --- a/src/devices/robot/controller/jointpositioncontroller.cpp +++ b/src/devices/robot/controller/jointpositioncontroller.cpp @@ -21,11 +21,41 @@ void JointPositionController::call(const Json::Value& json) //解析jason，执行算法 + /* + "params":{ + “canGroupId”:"leftArm", + "protocolType":"Ti5MotorProtocol", + "motors":[{"joint_name":""},{"joint_name":""}] + }, + "operate":{} + **/ + std::string canGroupId; + std::string protocolType; + if (json.isMember("params")) + { + Json::Value params = json["params"]; + if (params.isMember("canGroupId")) + canGroupId = params["canGroupId"].asString(); + if (params.isMember("protocolType")) + protocolType = params["protocolType"].asString(); + if (params.isMember("motors")) + { + for (int i = 0; i < params["motors"].size(); i++) + { + std::string joint_name = params["motors"][i]["joint_name"].asString(); + } + } + } + if (json.isMember("operate")) + { + + } //调用canmanager执行电机指令 - HardWareManager::getInstance(); + auto protocol = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorProtocol(protocolType); + //这里通过joint_name获取node_id? } void JointPositionController::interrupt() diff --git a/src/devices/robot/humanoid_robot/CMakeLists.txt b/src/devices/robot/humanoid_robot/CMakeLists.txt index fb62ba24..2f03eced 100644 --- a/src/devices/robot/humanoid_robot/CMakeLists.txt +++ b/src/devices/robot/humanoid_robot/CMakeLists.txt @@ -11,6 +11,7 @@ target_link_libraries(humanoid_robot PRIVATE cmvr_es::device::canbus cmvr_es::device::ti5motor protobuf::libprotobuf + cmvr_es::device::controller_manager ) diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 54a66a79..8fa69f44 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -92,6 +92,11 @@ HumanoidRobot::HumanoidRobot(const XmlNode &cfg) : AbstractRobot(cfg) { upd_timer_->start(chrono::nanoseconds(1000 / upd_freq_ * 1000), [this] { update_state_(); }); rsm_.store(ROBOT_READY); + + + auto controllerManagerCfg = cfg.getChild("ControllerManager"); + controller_manager_ = std::make_shared(controllerManagerCfg); + } catch (exception &e) { LOG(ERROR) << "HumanoidRobot init failed, id=" << id_; throw runtime_error(e.what()); @@ -306,6 +311,25 @@ void HumanoidRobot::eStop() { template void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { try { + auto state = controller_manager_->getSate(); + if (state == ControlManagerState_Teach + || state == ControlManagerState_EStop + || state == ControlManagerState_MajorFault) + { + throw runtime_error("Controller state error"); + } + else if (state == ControlManagerState_Command) + { + //当前有命令正在执行，判断优先级？ + //先获取当前正在执行的控制器 + auto controller = controller_manager_->getActiveController(); + Json::Value callJson; + Json::Value paramsJson; + Json::Value operateJson; + //拆分cmd，构建json内容 + + } + for (const auto &j: cmd) { auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.h b/src/devices/robot/humanoid_robot/humanoid_robot.h index f24eaf4a..3933a541 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/humanoid_robot.h @@ -31,6 +31,7 @@ #include #include +#include "../controller/controller_manager.h" namespace cmvr::device{ @@ -221,8 +222,8 @@ namespace cmvr::device{ std::shared_ptr motor_manager_{nullptr}; - - + private: + std::shared_ptr controller_manager_{nullptr}; }; diff --git a/src/hardware/can/can_group.cpp b/src/hardware/can/can_group.cpp index 8510db40..64e3de73 100644 --- a/src/hardware/can/can_group.cpp +++ b/src/hardware/can/can_group.cpp @@ -51,4 +51,13 @@ void CanGroup::init() } +std::shared_ptr CanGroup::getMotorProtocol(const std::string &protocolType) +{ + return motor_protocol_manager->getMotorProtocol(protocolType); +} + +uint8_t CanGroup::getNodeId(const std::string& joint_name) +{ + return motor_protocol_manager->getNodeId(joint_name); +} diff --git a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp index 6ad98dd9..e4f2cfd9 100644 --- a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp +++ b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp @@ -23,7 +23,11 @@ MotorProtocolManager::MotorProtocolManager(const XmlNode &cfg,std::shared_ptr MotorProtocolManager::getMotorProtocol(co return motor_protocols_[protocolType]; return nullptr; } + +uint8_t MotorProtocolManager::getNodeId(const std::string& joint_name) +{ + uint8_t node_id = 0; + if (motors_.count("Ti5MotorProtocol")) + { + const auto motors = motors_["Ti5MotorProtocol"]; + for (auto &motor: motors) + { + if (motor.jointName == joint_name) + { + node_id = motor.node_id; + break; + } + } + } + return node_id; +} \ No newline at end of file diff --git a/src/hardware_manager/hardware_manager.cpp b/src/hardware_manager/hardware_manager.cpp index 27a95c4a..9f35fc7f 100644 --- a/src/hardware_manager/hardware_manager.cpp +++ b/src/hardware_manager/hardware_manager.cpp @@ -6,6 +6,7 @@ using namespace std; using namespace cmvr::hardware; std::shared_ptr HardWareManager::instance_ = nullptr; +std::once_flag HardWareManager::init_flag_; HardWareManager::HardWareManager(const XmlNode& cfg) { try { diff --git a/src/main.cpp b/src/main.cpp index 2753330a..a041e5ef 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -18,7 +18,6 @@ #include "utils/base/logger.h" #include "service/grpc_service/grpc_head_service.h" #include "service/grpc_service/grpc_humanoid_robot_service.h" -#include "service/grpc_service/grpc_hlc_service.h" #include "service/http_service/httpclient.h" #include "json/json.h" @@ -55,7 +54,6 @@ void runServer(const XmlNode &cfg){ auto dexhand_service = gRPCDexHandServiceImpl(); auto biohand_service = gRPCMBioHeadServiceImpl(); auto humanoid_robot_service = gRPCHumanoidRobotServiceImpl(); - auto hlc_service = gRPCHlcServiceImpl(); grpc::ServerBuilder builder; builder.AddListeningPort(address, grpc::InsecureServerCredentials()); @@ -66,7 +64,6 @@ void runServer(const XmlNode &cfg){ builder.RegisterService(&dexhand_service); builder.RegisterService(&biohand_service); builder.RegisterService(&humanoid_robot_service); - builder.RegisterService(&hlc_service); // 🔥 关键！启用反射 //grpc::reflection::InitProtoReflectionServerBuilderPlugin(); diff --git a/src/service/grpc_service/CMakeLists.txt b/src/service/grpc_service/CMakeLists.txt index c0449d14..fc32fc54 100644 --- a/src/service/grpc_service/CMakeLists.txt +++ b/src/service/grpc_service/CMakeLists.txt @@ -6,7 +6,6 @@ add_library(service grpc_head_service.cpp grpc_dexhand_service.cpp grpc_humanoid_robot_service.cpp - grpc_hlc_service.cpp ) target_include_directories(service PUBLIC ${PROJECT_SOURCE_DIR}/include) @@ -61,27 +60,3 @@ target_link_libraries(grpc_humanoid_robot_client_test ${OpenCV_LIBS} ) - - - -add_executable(grpc_hlc_client_test - grpc_hlc_client_test.cpp -) - - -target_link_libraries(grpc_hlc_client_test - PRIVATE - cmvr_es::device::canbus - cmvr_es::device::ti5motor - cmvr_es::device::humanoid_robot - gtest - gtest_main - pthread - glog::glog - proto-objects - ccd - fcl - cmvr_es::device_manager - ${OpenCV_LIBS} -) - diff --git a/src/service/grpc_service/grpc_hlc_client_test.cpp b/src/service/grpc_service/grpc_hlc_client_test.cpp deleted file mode 100644 index 9024f292..00000000 --- a/src/service/grpc_service/grpc_hlc_client_test.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// -// Created by lgv on 2025/8/25. -// -#include "gtest/gtest.h" -#include -#include -#include "../../../include/service/grpc_service/grpc_hlc_service.h" -#include "google/protobuf/timestamp.pb.h" -#include -#include - -using namespace cmvr::api; - -TEST(GrpcHlcClientTest, MyTest) { - // 连接服务端 - auto channel = grpc::CreateChannel("0.0.0.0:50052", grpc::InsecureChannelCredentials()); - auto stub = cmvr::api::HlcService::NewStub(channel); - - - grpc::ClientContext context; - cmvr::api::Touch_Request request; - cmvr::api::Touch_Response response; - - - request.mutable_header()->set_device_id("hc01"); - *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); - - - request.set_u(12); - request.set_v(13); - request.set_max_force(1300); - - - - // 调用 - grpc::Status status = stub->touch(&context, request, &response); - - if (status.ok()) { - LOG(INFO) << "Touch RPC succeeded." << std::endl; - LOG(INFO) << "Success: " << response.mutable_header()->success() << std::endl; - LOG(INFO) << "Error message: " << response.mutable_header()->error_message() << std::endl; - LOG(INFO) << "Timestamp: " << response.mutable_header()->timestamp().seconds() << std::endl; - } else { - LOG(ERROR) << "Touch RPC failed: " << status.error_message() << std::endl; - } -} \ No newline at end of file diff --git a/src/service/grpc_service/grpc_hlc_service.cpp b/src/service/grpc_service/grpc_hlc_service.cpp deleted file mode 100644 index cc8dd780..00000000 --- a/src/service/grpc_service/grpc_hlc_service.cpp +++ /dev/null @@ -1,40 +0,0 @@ -// -// Created by lgv on 2025/8/25. -// - - -#include "service/grpc_service/grpc_hlc_service.h" -#include -#include "robot/humanoid_robot/humanoid_robot.h" -#include "controller/touch_controller.h" - - -using namespace cmvr::service; -using namespace cmvr::device; -using namespace cmvr::api; -using google::protobuf::util::TimeUtil; - -gRPCHlcServiceImpl::gRPCHlcServiceImpl():dmgr_(DeviceManager::getInstance()){} - -grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { - - grpc::Status ret = grpc::Status::OK; - - try { - std::shared_ptr robot = nullptr; - auto cam = dmgr_.getDevice("cam4"); - auto hand = dmgr_.getDevice("hand1"); - ctrl::TouchController touch_controller(robot,hand,cam);; - // touch_controller.touch(request->u(),request->v(),request->max_force()); - response->mutable_header()->set_success(true); - response->mutable_header()->set_error_message(""); - }catch (const std::exception& e) { - response->mutable_header()->set_success(false); - response->mutable_header()->set_error_message(e.what()); - ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - } - *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; -} - -