From 32fe139015896a68112c95ed3b53f0280539f0c1 Mon Sep 17 00:00:00 2001 From: lgv Date: Tue, 30 Jun 2026 16:05:47 +0800 Subject: [PATCH] refactor(devices): remove legacy robot stack and stale tests --- cmvr-es/devices/CMakeLists.txt | 1 - cmvr-es/devices/device_types.h | 6 + .../dexhand/px_6ax_gen3/CMakeLists.txt | 14 - .../px_6ax_gen3/src/px_6ax_gen3_test.cpp | 163 ------- .../dexhand/rh56dftp_dexhand/CMakeLists.txt | 14 - .../src/rh56dftp_dexhand_test.cpp | 135 ------ cmvr-es/devices/robot/CMakeLists.txt | 3 - cmvr-es/devices/robot/abstract_robot.h | 170 -------- .../devices/robot/aubo_robot/CMakeLists.txt | 35 -- .../robot/aubo_robot/include/aubo_robot.h | 51 --- .../robot/aubo_robot/src/aubo_robot.cpp | 272 ------------ cmvr-es/devices/robot/c701/CMakeLists.txt | 50 --- .../robot/c701/motor/motor_controller.cpp | 406 ------------------ .../robot/c701/motor/motor_controller.h | 101 ----- .../c701/motor/motor_controller_test.cpp | 87 ---- .../c701/motor/protocol/ti5_motor_rpdo1.cpp | 27 -- .../c701/motor/protocol/ti5_motor_rpdo1.h | 60 --- .../motor/protocol/ti5_motor_sdo_response.cpp | 40 -- .../motor/protocol/ti5_motor_sdo_response.h | 26 -- .../c701/motor/protocol/ti5_motor_tpdo1.cpp | 36 -- .../c701/motor/protocol/ti5_motor_tpdo1.h | 32 -- .../c701/motor/protocol/ti5_motor_tpdo2.cpp | 26 -- .../c701/motor/protocol/ti5_motor_tpdo2.h | 32 -- 23 files changed, 6 insertions(+), 1781 deletions(-) delete mode 100644 cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp delete mode 100644 cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp delete mode 100644 cmvr-es/devices/robot/CMakeLists.txt delete mode 100644 cmvr-es/devices/robot/abstract_robot.h delete mode 100644 cmvr-es/devices/robot/aubo_robot/CMakeLists.txt delete mode 100644 cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h delete mode 100644 cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp delete mode 100644 cmvr-es/devices/robot/c701/CMakeLists.txt delete mode 100644 cmvr-es/devices/robot/c701/motor/motor_controller.cpp delete mode 100644 cmvr-es/devices/robot/c701/motor/motor_controller.h delete mode 100644 cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp delete mode 100644 cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h diff --git a/cmvr-es/devices/CMakeLists.txt b/cmvr-es/devices/CMakeLists.txt index 0fcfbade..9d1ff6e8 100644 --- a/cmvr-es/devices/CMakeLists.txt +++ b/cmvr-es/devices/CMakeLists.txt @@ -5,6 +5,5 @@ add_subdirectory(microphone) add_subdirectory(dexhand) add_subdirectory(biohead) add_subdirectory(arm) -add_subdirectory(robot) add_subdirectory(canbus) add_subdirectory(motor) diff --git a/cmvr-es/devices/device_types.h b/cmvr-es/devices/device_types.h index c5c08b9e..38b8d5b3 100644 --- a/cmvr-es/devices/device_types.h +++ b/cmvr-es/devices/device_types.h @@ -18,6 +18,8 @@ namespace cmvr::device { Microphone, Motor, MotorSystem, + MujocoViewer, + MujocoWorld, Robot, Speaker, }; @@ -46,6 +48,10 @@ namespace cmvr::device { return "Motor"; case DeviceKind::MotorSystem: return "MotorSystem"; + case DeviceKind::MujocoViewer: + return "MujocoViewer"; + case DeviceKind::MujocoWorld: + return "MujocoWorld"; case DeviceKind::Robot: return "Robot"; case DeviceKind::Speaker: diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt b/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt index 599da518..b6a30201 100644 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/px_6ax_gen3/CMakeLists.txt @@ -12,17 +12,3 @@ target_link_libraries(px_6ax_gen3 ) install(TARGETS px_6ax_gen3 LIBRARY DESTINATION lib) - -add_executable(px_6ax_gen3_test - src/px_6ax_gen3_test.cpp -) - -target_link_libraries(px_6ax_gen3_test PRIVATE - cmvr_es::device::px_6ax_gen3 - cmvr_es::common - cmvr_es::proto - glog - gtest - gtest_main - pthread -) diff --git a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp b/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp deleted file mode 100644 index bf0b57b3..00000000 --- a/cmvr-es/devices/dexhand/px_6ax_gen3/src/px_6ax_gen3_test.cpp +++ /dev/null @@ -1,163 +0,0 @@ -#include "gtest/gtest.h" - -#include "../include/px_6ax_gen3.h" -#include "cmvr/config/dexhand_config/dexhand_config.pb.h" -#include "common/config/config_files.h" - -#include -#include -#include -#include -#include -#include -#include -#include - -namespace { - -using DexHand = cmvr::device::AbstractDexHand; -using PX6AXGen3 = cmvr::device::PX6AXGen3; - -DexHand::FingerType parseFingerType(const std::string& value) { - if (value == "PINKY") { - return DexHand::FingerType::PINKY; - } - if (value == "RING") { - return DexHand::FingerType::RING; - } - if (value == "MIDDLE" || value == "MIDDLE_FINGER") { - return DexHand::FingerType::MIDDLE; - } - if (value == "THUMB") { - return DexHand::FingerType::THUMB; - } - if (value == "PALM") { - return DexHand::FingerType::PALM; - } - return DexHand::FingerType::INDEX; -} - -DexHand::TactileRegion parseTactileRegion(const std::string& value) { - if (value == "FINGER") { - return DexHand::TactileRegion::FINGER; - } - if (value == "PAD") { - return DexHand::TactileRegion::PAD; - } - if (value == "THUMB_MIDDLE") { - return DexHand::TactileRegion::THUMB_MIDDLE; - } - if (value == "PALM_PAD") { - return DexHand::TactileRegion::PALM_PAD; - } - return DexHand::TactileRegion::TIP; -} - -struct StopGuard { - std::shared_ptr hand; - - ~StopGuard() { - if (!hand) { - return; - } - try { - hand->stop(); - } catch (...) { - } - } -}; - -DexHand::ResultantForce readFirstValidResultantForce(PX6AXGen3& hand, - const DexHand::FingerType finger, - const DexHand::TactileRegion region, - const int max_attempts, - const std::chrono::milliseconds retry_interval) { - std::string last_error; - - for (int attempt = 0; attempt < max_attempts; ++attempt) { - try { - return hand.getResultantForce(finger, region); - } catch (const std::exception& ex) { - last_error = ex.what(); - } - - if (attempt + 1 < max_attempts) { - std::this_thread::sleep_for(retry_interval); - } - } - - throw std::runtime_error("Failed to read PX6AXGen3 resultant force: " + last_error); -} - -DexHand::ResultantForce readStateResultantForce(PX6AXGen3& hand) { - cmvr::device::DexHandState state; - hand.getState(state); - return DexHand::ResultantForce{0, 0, state.hands[0].force}; -} - -} // namespace - -TEST(PX6AXGen3Test, PrintResultantForceOnly) { - cmvr::config::DexHandRootConfig root_config; - ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config)); - const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr; - for (const auto& cfg : root_config.dexhand().dexhands()) { - if (cfg.id() == "paxini_tip_1" && cfg.has_px_6ax_gen3()) { - device_config_ptr = &cfg; - break; - } - } - ASSERT_NE(device_config_ptr, nullptr); - auto test_config = device_config_ptr->px_6ax_gen3(); - test_config.set_id(device_config_ptr->id()); - ASSERT_FALSE(test_config.serial_port().empty()); - - auto hand = std::make_shared(test_config); - ASSERT_NO_THROW(hand->init()); - ASSERT_NO_THROW(hand->start()); - StopGuard stop_guard{hand}; - - EXPECT_EQ(hand->state(), DexHand::Status::STREAMING); - - const DexHand::FingerType finger = parseFingerType(test_config.tactile_finger()); - const DexHand::TactileRegion region = parseTactileRegion(test_config.tactile_region()); - const int warmup_ms = test_config.poll_interval_ms() > 0 - ? test_config.poll_interval_ms() * 5 - : 200; - const int iterations = 1000000; - const int read_interval_ms = test_config.poll_interval_ms() > 0 - ? test_config.poll_interval_ms() - : 10; - if (warmup_ms > 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms)); - } - - bool read_once = false; - for (int iteration = 0; iteration < iterations; ++iteration) { - DexHand::ResultantForce resultant_force{}; - ASSERT_NO_THROW(resultant_force = readFirstValidResultantForce( - *hand, - finger, - region, - 10, - std::chrono::milliseconds(100))); - - DexHand::ResultantForce state_force{}; - ASSERT_NO_THROW(state_force = readStateResultantForce(*hand)); - - std::cout << "[PX6AXGen3Test] iter=" << (iteration + 1) - << " resultant_fx=" << resultant_force.fx - << " resultant_fy=" << resultant_force.fy - << " resultant_fz=" << resultant_force.fz - << " state_resultant_fz=" << state_force.fz - << std::endl; - - read_once = true; - - if (read_interval_ms > 0 && iteration + 1 < iterations) { - std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms)); - } - } - - EXPECT_TRUE(read_once); -} diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt index 8d2c82bc..1e522d43 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt @@ -7,17 +7,3 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand) target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware cmvr_es::proto -lmodbus) install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib) - -add_executable(rh56dftp_dexhand_test - src/rh56dftp_dexhand_test.cpp -) - -target_link_libraries(rh56dftp_dexhand_test PRIVATE - cmvr_es::device::rh56dftp_dexhand - cmvr_es::common - cmvr_es::proto - glog - gtest - gtest_main - pthread -) diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp deleted file mode 100644 index 3469e22f..00000000 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp +++ /dev/null @@ -1,135 +0,0 @@ -#include "gtest/gtest.h" - -#include "../include/rh56dftp_dexhand.h" -#include "cmvr/config/dexhand_config/dexhand_config.pb.h" -#include "common/config/config_files.h" - -#include -#include -#include -#include -#include -#include -#include - -namespace { - -using DexHand = cmvr::device::AbstractDexHand; -using RH56DexHand = cmvr::device::RH56DFTPDexhand; - -const char* tactileRegionToString(const DexHand::TactileRegion region) { - switch (region) { - case DexHand::TactileRegion::TIP: return "TIP"; - case DexHand::TactileRegion::FINGER: return "FINGER"; - case DexHand::TactileRegion::PAD: return "PAD"; - case DexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE"; - case DexHand::TactileRegion::PALM_PAD: return "PALM_PAD"; - } - return "UNKNOWN"; -} - -struct StopGuard { - std::shared_ptr hand; - - ~StopGuard() { - if (!hand) { - return; - } - try { - hand->stop(); - } catch (...) { - } - } -}; - -} // namespace - -TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) { - cmvr::config::DexHandRootConfig root_config; - ASSERT_TRUE(cmvr::ConfigHelper::loadConfigFile("devices/dexhand/dexhand.pb.txt", root_config)); - const cmvr::config::DexHandDeviceConfig* device_config_ptr = nullptr; - for (const auto& cfg : root_config.dexhand().dexhands()) { - if (cfg.id() == "hand2" && cfg.has_rh56dftp()) { - device_config_ptr = &cfg; - break; - } - } - ASSERT_NE(device_config_ptr, nullptr); - auto hand_config = device_config_ptr->rh56dftp(); - hand_config.set_id(device_config_ptr->id()); - ASSERT_FALSE(hand_config.ip().empty()); - - const auto finger = DexHand::FingerType::RING; - const auto region = DexHand::TactileRegion::TIP; - const int iterations = 20000; - const int warmup_ms = 200; - const int read_interval_ms = 10; - - auto hand = std::make_shared(hand_config); - ASSERT_NO_THROW(hand->init()); - ASSERT_NO_THROW(hand->start()); - StopGuard stop_guard{hand}; - - EXPECT_EQ(hand->state(), DexHand::Status::STREAMING); - ASSERT_NO_THROW(hand->setTactilePollingRegion(finger, region)); - - if (warmup_ms > 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms)); - } - - double last_pressure_sum = 0.0; - double last_pressure_peak = 0.0; - int point_count = 0; - std::string sensor_name; - - for (int i = 0; i < iterations; ++i) { - const auto region_data = hand->getSensorData(finger, region); - - ASSERT_TRUE(region_data.valid()); - EXPECT_EQ(region_data.finger, finger); - EXPECT_EQ(region_data.region, region); - - if (point_count == 0) { - point_count = region_data.view.pointCount(); - sensor_name = region_data.name == nullptr ? "" : region_data.name; - } - - last_pressure_sum = 0.0; - last_pressure_peak = 0.0; - for (int index = 0; index < region_data.view.pointCount(); ++index) { - const double pressure = static_cast(region_data.view.data[index].fz); - last_pressure_sum += pressure; - if (pressure > last_pressure_peak) { - last_pressure_peak = pressure; - } - } - - std::cout << std::fixed << std::setprecision(3) - << "[RH56DFTPDexhandLatencyTest] iter=" << (i + 1) - << "/" << iterations - << " sensor=" << sensor_name - << " pressure_sum=" << last_pressure_sum - << " pressure_peak=" << last_pressure_peak - << std::endl; - - if (read_interval_ms > 0) { - std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms)); - } - } - - std::cout << std::fixed << std::setprecision(3) - << "[RH56DFTPDexhandLatencyTest] id=" << hand_config.id() - << " ip=" << hand_config.ip() - << " port=" << hand_config.port() - << " finger=INDEX" - << " region=" << tactileRegionToString(region) - << " sensor=" << sensor_name - << " iterations=" << iterations - << " points=" << point_count - << " last_pressure_sum=" << last_pressure_sum - << " last_pressure_peak=" << last_pressure_peak - << " read_interval_ms=" << read_interval_ms - << "\n"; - - EXPECT_GT(point_count, 0); -} diff --git a/cmvr-es/devices/robot/CMakeLists.txt b/cmvr-es/devices/robot/CMakeLists.txt deleted file mode 100644 index db04a476..00000000 --- a/cmvr-es/devices/robot/CMakeLists.txt +++ /dev/null @@ -1,3 +0,0 @@ -#add_subdirectory(ti5_robot) -#add_subdirectory(c701) -#add_subdirectory(aubo_robot) diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h deleted file mode 100644 index fc127847..00000000 --- a/cmvr-es/devices/robot/abstract_robot.h +++ /dev/null @@ -1,170 +0,0 @@ -// -// Created by xtkuang on 2025/5/8. -// - -#ifndef CMVR_ES_ABSTRACT_ROBOT_H -#define CMVR_ES_ABSTRACT_ROBOT_H -#pragma once - -#include "json/json.h" -#include "../abstract_device.h" -#include "common/types/arm/arm_types.h" -#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" -#include "cmvr/common/geometry.pb.h" -#include "cmvr/msgs/motor.pb.h" -#include "common/base/logging/logger.h" -#include - -namespace cmvr::device{ - - class AbstractRobot: public AbstractDevice { - public: - AbstractRobot() = default; - ~AbstractRobot() override=default; - - DeviceKind kind() const noexcept override { return DeviceKind::Robot; } - - virtual int getDOF() { CMVR_LOG(ERROR) << "[AbstractRobot] getDOF is not implemented"; return 0; } - - virtual std::vector getJointNames() { CMVR_LOG(ERROR) << "[AbstractRobot] getJointNames is not implemented"; return {}; } - - virtual std::unordered_map getJointQ() const { CMVR_LOG(ERROR) << "[AbstractRobot] getJointQ is not implemented"; return {}; } - - /** - * - * @param joint_qs 函数会根据 joint_qs 的 joint name 去获取值 - */ - virtual void getJointQ(std::unordered_map &joint_qs) const { CMVR_LOG(ERROR) << "[AbstractRobot] getJointQ(out) is not implemented"; joint_qs.clear(); } - virtual std::vector getLinkNames() { CMVR_LOG(ERROR) << "[AbstractRobot] getLinkNames is not implemented"; return {}; } - - virtual void getJointsState(std::vector& states) = 0; - virtual void getState(RobotState &state) { CMVR_LOG(ERROR) << "[AbstractRobot] getState is not implemented"; state = RobotState{}; } - - virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) { CMVR_LOG(ERROR) << "[AbstractRobot] getTransform is not implemented"; return {}; } - - virtual void torqueOn() { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOn is not implemented"; } - virtual void torqueOn(const std::string &joint_name) { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOn(joint) is not implemented: " << joint_name; } - - virtual void torqueOff() { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOff is not implemented"; } - virtual void torqueOff(const std::string &joint_name) { CMVR_LOG(ERROR) << "[AbstractRobot] torqueOff(joint) is not implemented: " << joint_name; } - - virtual void eStop() { CMVR_LOG(ERROR) << "[AbstractRobot] eStop is not implemented"; } - - /** - * @brief 关节空间 point-to-point 运动（内部轨迹规划）。 - * @details 控制器会在内部生成满足速度/加速度约束的关节轨迹，并做多关节时间同步，使关节协调到达目标点。 - * @param joints 目标关节角，单位 rad。 - * @param max_vel 主导轴最大速度，单位 rad/s。 - * @param max_acc 主导轴最大加速度，单位 rad/s^2。 - */ - virtual void moveJ(std::vector &joints, double max_vel=0.5, double max_acc=0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(vector) is not implemented"; } - - /** - * @brief 按关节命令执行关节空间运动（内部轨迹规划）。 - * @param cmd 目标关节命令（关节名 + 角度，`vel` 字段可由具体实现决定是否使用）。 - * @param vel 主导轴最大速度，单位 rad/s。 - * @param acc 主导轴最大加速度，单位 rad/s^2。 - */ - virtual void moveJ(std::vector &cmd, double vel=0.5, double acc=0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(vector) is not implemented"; } - virtual void speedJ(std::vector &cmd) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(commands) is not implemented"; } - virtual void speedJ(double vel) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(double) is not implemented"; } - /** - * @brief 末端位姿目标的关节空间运动。 - * @details 典型实现为先做 IK 求解关节目标，再按 `moveJ` 语义执行（关节空间规划与同步）。 - * @param base_link 基坐标系 link 名称。 - * @param ee_link 末端 link 名称。 - * @param pose 目标末端位姿。 - * @param vel 主导轴最大速度，单位 rad/s。 - * @param acc 主导轴最大加速度，单位 rad/s^2。 - */ - virtual void moveJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d pose,double vel = 0.5, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ(pose) is not implemented"; } - virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveDeltaJ is not implemented"; } - - virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] moveJ_IK is not implemented"; } - - virtual bool moveL(const std::vector &pose, - double speed = 0.25, - double acceleration = 1.2, - double jerk = 5.0, - const std::vector &qd_max = std::vector(7, 2.5), - bool asynchronous = false) { - CMVR_LOG(ERROR) << "[AbstractRobot] moveL is not implemented"; - return false; - } - virtual bool speedL(const std::vector &xd, - double acceleration = 0.25, - double time = 0.0, - cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) { - CMVR_LOG(ERROR) << "[AbstractRobot] speedL is not implemented"; - return false; - } - virtual Eigen::Matrix getSpeedLCommandTwistBase() { - return Eigen::Matrix::Zero(); - } - virtual void stopSpeedL() { CMVR_LOG(ERROR) << "[AbstractRobot] stopSpeedL is not implemented"; } - - virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { CMVR_LOG(ERROR) << "[AbstractRobot] speedJ(joint) is not implemented: " << joint_name; } - - virtual void followJointTrajectory(std::vector> &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followJointTrajectory(double) is not implemented"; } - - virtual void followJointTrajectory(std::vector> &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followJointTrajectory(JointPoint) is not implemented"; } - - virtual void followPoseTrajectory(std::vector &traj, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] followPoseTrajectory is not implemented"; } - - /** - * @brief 关节空间周期伺服（无整段轨迹规划）。 - * @details 每次调用只在一个控制周期内跟踪当前目标，需外部循环持续下发；是否稳定由外部更新频率和目标序列决定。 - * @param joints 当前周期目标关节角，单位 rad。 - * @param dt 当前伺服命令生效时长（控制周期），单位 s。 - */ - virtual void servoJ(std::vector &joints, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector) is not implemented"; } - - /** - * @brief 按关节命令执行周期伺服（无整段轨迹规划）。 - * @param joints 当前周期关节目标命令。 - * @param dt 当前伺服命令生效时长（控制周期），单位 s。 - */ - virtual void servoJ(std::vector &joints, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector) is not implemented"; } - - /** - * @brief 带速度参数的关节空间周期伺服。 - * @param joints 当前周期关节目标命令。 - * @param vel 速度参数（语义由具体驱动实现决定）。 - * @param dt 当前伺服命令生效时长（控制周期），单位 s。 - */ - virtual void servoJ(std::vector &joints, double vel, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(vector, vel) is not implemented"; } - - /** - * @brief 基于末端位姿目标的周期伺服。 - * @details 典型实现为每周期执行 IK 并下发关节伺服目标；属于实时闭环接口，不等价于一次性到位的 `moveJ`。 - * @param base_link 基坐标系 link 名称。 - * @param ee_link 末端 link 名称。 - * @param pose 当前周期目标末端位姿。 - * @param vel 速度参数（语义由具体驱动实现决定）。 - * @param acc 加速度参数（语义由具体驱动实现决定）。 - */ - virtual void servoJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d pose,double vel = 0.1, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] servoJ(pose) is not implemented"; } - virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, cmvr::common::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { CMVR_LOG(ERROR) << "[AbstractRobot] servoDeltaJ is not implemented"; } - - - virtual void servoL(math::Pose3d &pose, double dt) { CMVR_LOG(ERROR) << "[AbstractRobot] servoL is not implemented"; } - - virtual void calibrateZeroQ(const std::string &joint_name) = 0; - - void setToolFrame(const std::string& toolFrame) - { - toolFrame_ = toolFrame; - } - virtual std::string getToolFrame() const { return toolFrame_; } - - virtual cmvr::common::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0; - virtual cmvr::common::Pose3d fk(bool is_tcp = true) = 0; - virtual std::vector ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose) = 0; - protected: - int dof_{}; - RobotState state_{}; - std::string toolFrame_; - }; -} - -#endif //CMVR_ES_ABSTRACT_ROBOT_H diff --git a/cmvr-es/devices/robot/aubo_robot/CMakeLists.txt b/cmvr-es/devices/robot/aubo_robot/CMakeLists.txt deleted file mode 100644 index a70b9487..00000000 --- a/cmvr-es/devices/robot/aubo_robot/CMakeLists.txt +++ /dev/null @@ -1,35 +0,0 @@ -# 第一步：先定义 Aubo SDK 的路径（必须在 add_library 之前！） -if(CMAKE_SYSTEM_NAME STREQUAL "Linux") - set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include) - set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib) -elseif(CMAKE_SYSTEM_NAME STREQUAL "Windows") - set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/win/include) - set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/win/lib) -endif() - -# 第二步：创建 aubo_robot 库（此时 SDK 路径已经定义好了） -add_library(aubo_robot SHARED src/aubo_robot.cpp) - -# 第三步：给 aubo_robot 配置头文件路径（合并你原来的 ${CMAKE_CURRENT_SOURCE_DIR} 和 SDK 的 include 目录） -target_include_directories(aubo_robot - PUBLIC - ${CMAKE_CURRENT_SOURCE_DIR} # 你原来的路径（如果需要的话） - ${AUBO_SDK_INCLUDE_DIR} # SDK 头文件路径（关键！） -) - -# 第四步：给 aubo_robot 配置库文件路径（精准绑定到目标，比 link_directories 靠谱） -target_link_directories(aubo_robot - PRIVATE - ${AUBO_SDK_LIB_DIR} # SDK 库文件路径（关键！） -) - -# 第五步：链接 SDK 库（和你原来一致，但现在路径已经生效） -target_link_libraries(aubo_robot - PRIVATE - aubo_sdk - robot_proxy - cmvr_es::proto -) - -# 别名（不变） -add_library(cmvr_es::device::aubo_robot ALIAS aubo_robot) diff --git a/cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h b/cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h deleted file mode 100644 index 9277e2de..00000000 --- a/cmvr-es/devices/robot/aubo_robot/include/aubo_robot.h +++ /dev/null @@ -1,51 +0,0 @@ -// -// Created by linbo on 2025/11/7. -// - -#ifndef CMVR_ES_AUBO_ROBOT_H -#define CMVR_ES_AUBO_ROBOT_H -#include "devices/robot/abstract_robot.h" -#include "aubo_sdk/rpc.h" -#include "cmvr/config/aubo_robot_config/aubo_robot_config.pb.h" -#include "cmvr/msgs/robot_detail.pb.h" -using namespace arcs::common_interface; -using namespace arcs::aubo_sdk; -namespace cmvr::device -{ - template - class AuboRobot: public AbstractRobot { - public: - explicit AuboRobot(const config::AuboRobotConfig& cfg); - ~AuboRobot(); - - std::string typeName() const override { return "AuboRobot"; } - bool init () override; - - void torqueOn() override; - void torqueOff() override; - - void getJointsState(std::vector& states) override; - - void moveJ(std::vector& cmd, double vel, double acc) override; - - void moveL(math::Pose3d& pose, double vel, double acc) override; - - void calibrateZeroQ(const std::string& joint_name) override; - cmvr::common::Pose3d fk(const std::string &base_link, const std::string &ee_link) override; - std::vector ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose) override; - private: - static void waitForRobotMode(const RobotInterfacePtr& robot_interface, - const RobotModeType& target_mode); - static int waitArrival(const RobotInterfacePtr& impl); - private: - std::string ip_; - int port_; - std::string username_; - std::string password_; - std::shared_ptr rpc_cli_; - }; -} - - - -#endif //CMVR_ES_AUBO_ROBOT_H diff --git a/cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp b/cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp deleted file mode 100644 index f8c0e4b6..00000000 --- a/cmvr-es/devices/robot/aubo_robot/src/aubo_robot.cpp +++ /dev/null @@ -1,272 +0,0 @@ -#include "common/base/logging/logger.h" -// -// Created by linbo on 2025/11/7. -// - -#include "devices/robot/aubo_robot/include/aubo_robot.h" -using namespace std; -using namespace cmvr::device; - -template -AuboRobot::AuboRobot(const config::AuboRobotConfig &cfg) -{ - try { - id_ = cfg.id(); - ip_ = cfg.ip(); - port_ = cfg.port() > 0 ? cfg.port() : 30004; - username_ = cfg.username(); - password_ = cfg.password(); - } catch (std::exception &e) { - CMVR_LOG(ERROR) << "[AuboRobot] Failed to parse config: " << e.what(); - } -} - - -template -AuboRobot::~AuboRobot() -{ - if (rpc_cli_) - { - // 接口调用: 退出登录 - rpc_cli_->logout(); - // 接口调用: 断开连接 - rpc_cli_->disconnect(); - } -} - -template -bool AuboRobot::init () -{ - try { - //初始化AuboSDK - rpc_cli_ = std::make_shared(); - - // 接口调用: 设置 RPC 超时 - rpc_cli_->setRequestTimeout(1000); - // 接口调用: 连接到 RPC 服务 - rpc_cli_->connect(ip_, port_); - // 接口调用: 登录 - rpc_cli_->login(username_, password_); - return true; - } catch (const std::exception& e) { - CMVR_LOG(ERROR) << "[AuboRobot] init failed: " << e.what(); - rpc_cli_.reset(); - return false; - } -} -template -void AuboRobot::waitForRobotMode(const RobotInterfacePtr& robot_interface, - const RobotModeType& target_mode) -{ - // 接口调用: 获取当前机械臂的模式 - auto current_mode = robot_interface->getRobotState()->getRobotModeType(); - - while (current_mode != target_mode) { - CMVR_LOG(INFO) << "机械臂当前模式:" << current_mode; - std::this_thread::sleep_for(std::chrono::seconds(1)); - current_mode = robot_interface->getRobotState()->getRobotModeType(); - } -} -template -void AuboRobot::torqueOn() -{ - // 接口调用: 获取机器人的名字 - auto robot_name = rpc_cli_->getRobotNames().front(); - - auto robot_interface = rpc_cli_->getRobotInterface(robot_name); - - // 接口调用: 设置负载 - double mass = 0.0; - std::vector cog(3, 0.0); - std::vector aom(3, 0.0); - std::vector inertia(6, 0.0); - robot_interface->getRobotConfig()->setPayload(mass, cog, aom, inertia); - - // 接口调用: 获取机械臂当前模式 - auto robot_mode = robot_interface->getRobotState()->getRobotModeType(); - - if (robot_mode == RobotModeType::Running) { - CMVR_LOG(INFO) << "机械臂已松刹车，处于运行模式"; - - } else { - // 接口调用: 机械臂发起上电请求 - robot_interface->getRobotManage()->poweron(); - - // 等待机械臂进入空闲模式 - waitForRobotMode(robot_interface, RobotModeType::Idle); - - CMVR_LOG(INFO) << "机械臂上电成功，当前模式:" - << robot_interface->getRobotState()->getRobotModeType() - ; - - // 接口调用: 机械臂发起松刹车请求 - rpc_cli_->getRobotInterface(robot_name)->getRobotManage()->startup(); - - // 等待机械臂进入运行模式 - waitForRobotMode(robot_interface, RobotModeType::Running); - - CMVR_LOG(INFO) << "机械臂松刹车成功，当前模式:" - << robot_interface->getRobotState()->getRobotModeType() - ; - } -} -template -void AuboRobot::getJointsState(std::vector& states) -{ - try { - // 接口调用: 获取机器人的名字 - auto robot_name = rpc_cli_->getRobotNames().front(); - - auto robot_interface = rpc_cli_->getRobotInterface(robot_name); - auto rebotState = robot_interface->getRobotState(); - states.clear(); - JointState state; - //获取关节位置和速度 - for (int i = 0; i < 6; i++) { - state.velocity = rebotState->getJointSpeeds()[i]; - state.position = rebotState->getJointPositions()[i]; - states.push_back(state); - } - } catch (exception &e) { - CMVR_LOG(ERROR) << "[AuboRobot] getJointsState failed: " << e.what(); - states.clear(); - } -} - - -template -void AuboRobot::torqueOff() -{ - // 接口调用: 获取机器人的名字 - auto robot_name = rpc_cli_->getRobotNames().front(); - - auto robot_interface = rpc_cli_->getRobotInterface(robot_name); - - // 接口调用: 机械臂断电 - robot_interface->getRobotManage()->poweroff(); - - // 等待机械臂进入断电模式 - waitForRobotMode(robot_interface, RobotModeType::PowerOff); - - CMVR_LOG(INFO) << "机械臂断电成功，当前模式:" - << robot_interface->getRobotState()->getRobotModeType() - ; -} -template -void AuboRobot::moveJ(std::vector& cmd, double vel, double acc) -{ - try - { - //检查参数 - if (cmd.size() != dof_) - { - CMVR_LOG(ERROR) << "[AuboRobot](moveJ)Wrong number of cmd"; - return; - } - //调用aubo sdk接口实现moveJ - - // 接口调用: 获取机器人的名字 - auto robot_name = rpc_cli_->getRobotNames().front(); - - auto robot_interface = rpc_cli_->getRobotInterface(robot_name); - - // 接口调用: 设置机械臂的速度比率 - robot_interface->getMotionControl()->setSpeedFraction(0.3); - // 接口调用: 关节运动 - // 关节角，单位: 弧度 (此处为6关节) - std::vector joint_angle; - for (int i = 0; i < cmd.size(); i++) - { - joint_angle.emplace_back(cmd[i].rad); - } - robot_interface->getMotionControl()->moveJoint( - joint_angle, acc, vel, 0, 0); - } - catch (std::exception& e) - { - CMVR_LOG(ERROR) << "[AuboRobot] moveJ failed: " << e.what(); - } -} - -template -void AuboRobot::calibrateZeroQ(const std::string& joint_name) -{ - -} - -template -cmvr::common::Pose3d AuboRobot::fk(const std::string &base_link, const std::string &ee_link) -{ - cmvr::common::Pose3d pose; - - - return pose; -} - -template -std::vector AuboRobot::ik(const std::string &base_link, const std::string &ee_link,cmvr::common::Pose3d pose) -{ - std::vector ik; - - - return ik; -} -template -int AuboRobot::waitArrival(const RobotInterfacePtr& impl) { - const int max_retry_count = 5; - int cnt = 0; - - // 接口调用: 获取当前的运动指令 ID - int exec_id = impl->getMotionControl()->getExecId(); - - // 等待机械臂开始运动 - while (exec_id == -1) { - if (cnt++ > max_retry_count) { - return -1; - } - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - exec_id = impl->getMotionControl()->getExecId(); - } - - // 等待机械臂动作完成 - while (impl->getMotionControl()->getExecId() != -1) { - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - - return 0; -} -template -void AuboRobot::moveL(math::Pose3d& pose, double vel, double acc) -{ - // 接口调用: 获取机器人的名字 - auto robot_name = rpc_cli_->getRobotNames().front(); - - auto robot_interface = rpc_cli_->getRobotInterface(robot_name); - - // 接口调用: 设置机械臂的速度比率 - robot_interface->getMotionControl()->setSpeedFraction(0.75); - - // 接口调用: 设置工具中心点（TCP相对于法兰盘中心的偏移） - std::vector tcp_offset(6, 0.0); - robot_interface->getRobotConfig()->setTcpOffset(tcp_offset); - - // 接口调用: 直线运动到位置 - std::vector pose1; - pose1.emplace_back(pose.position.x); - pose1.emplace_back(pose.position.y); - pose1.emplace_back(pose.position.z); - pose1.emplace_back(pose.euler.rx); - pose1.emplace_back(pose.euler.ry); - pose1.emplace_back(pose.euler.rz); - robot_interface->getMotionControl()->moveLine(pose1, acc, vel, 0.025, 0); - // 阻塞 - auto ret = waitArrival(robot_interface); - if (ret == 0) { - CMVR_LOG(INFO)<<"直线运动到位置成功！"; - } else { - CMVR_LOG(INFO)<<"直线运动到位置失败！"; - } -} - - -template class cmvr::device::AuboRobot<6>; diff --git a/cmvr-es/devices/robot/c701/CMakeLists.txt b/cmvr-es/devices/robot/c701/CMakeLists.txt deleted file mode 100644 index eccc7569..00000000 --- a/cmvr-es/devices/robot/c701/CMakeLists.txt +++ /dev/null @@ -1,50 +0,0 @@ -add_library(c701 SHARED - ${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_sdo_response.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo1.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_tpdo2.cpp - ${CMAKE_CURRENT_SOURCE_DIR}/motor/protocol/ti5_motor_rpdo1.cpp) - - -target_include_directories(c701 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) - -add_library(cmvr_es::robot::c701 ALIAS c701) - -target_link_libraries(c701 - PRIVATE - cmvr_es::device::canbus - protobuf - glog -) - - -# -------------------------------------------------------- -# Unit test -# -------------------------------------------------------- -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(motor_controller_test - ${CMAKE_CURRENT_SOURCE_DIR}/motor/motor_controller_test.cpp -) - - -target_link_libraries(motor_controller_test - PRIVATE - cmvr_es::device::canbus - cmvr_es::robot::c701 - c701 - gtest - gtest_main - pthread - glog - cmvr_es::proto -) - - diff --git a/cmvr-es/devices/robot/c701/motor/motor_controller.cpp b/cmvr-es/devices/robot/c701/motor/motor_controller.cpp deleted file mode 100644 index fa113c64..00000000 --- a/cmvr-es/devices/robot/c701/motor/motor_controller.cpp +++ /dev/null @@ -1,406 +0,0 @@ -#include "common/base/logging/logger.h" -// -// Created by lgv on 2025/7/17. -// - -#include "robot/c701/motor/motor_controller.h" -#include "robot/c701/motor/protocol/ti5_motor_sdo_response.h" -#include "canbus/canopen/register.h" -#include "robot/c701/motor/protocol/ti5_motor_tpdo1.h" -#include "robot/c701/motor/protocol/ti5_motor_tpdo2.h" - - - -using namespace cmvr::robot::c701; -using namespace cmvr::robot::motor; -using namespace cmvr::device; -using namespace cmvr::msgs; -using namespace cmvr::msgs; - -ErrorCode MotorController::Init(cmvr::device::AbstractCanbus *can_client, bool enable_log) { - if (!can_client->init()) { - CMVR_LOG(ERROR) << "[MotorController] CAN client init failed"; - return ErrorCode::CAN_CLIENT_ERROR_BASE; - } - if (!can_client->start()) { - CMVR_LOG(ERROR) << "[MotorController] CAN client start failed"; - return ErrorCode::CAN_CLIENT_ERROR_BASE; - } - - - auto ret = ErrorCode::OK; - // 初始化 message_manager_ - message_manager_ = std::make_shared >(); - - // NMT - message_manager_->AddSendProtocolData, false>(); - - //sync - message_manager_->AddSendProtocolData, false>(); - for (const auto node_id: node_ids_) { - //nmt - message_manager_->AddRecvProtocolData, false>(node_id); - - //sdo - message_manager_->AddSendProtocolData, false>(node_id); - message_manager_->AddRecvProtocolData(node_id); - - //TPDO - message_manager_->AddRecvProtocolData(node_id); - message_manager_->AddRecvProtocolData(node_id); - - //RPDO - message_manager_->AddSendProtocolData(node_id); - } - - // 初始化 sender - can_sender_ = std::make_shared >(); - ret = can_sender_->Init(can_client, enable_log); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to init can sender."; - return ret; - } - - - // 初始化 receiver - can_receiver_ = std::make_shared >(); - ret = can_receiver_->Init(can_client, message_manager_.get(), enable_log); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to init can receiver."; - return ret; - } - - - - - // nmt - nmt_command_ = dynamic_cast *>( - message_manager_->GetMutableProtocolDataById(NmtRequestProtocol::ID)); - - - if (nmt_command_ == nullptr) { - CMVR_LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; - } - can_sender_->AddMessage(nmt_command_->ID, nmt_command_, true); - - // sync - sync_command_ = dynamic_cast *>( - message_manager_->GetMutableProtocolDataById(SyncProtocol::ID)); - - - if (sync_command_ == nullptr) { - CMVR_LOG(ERROR) << "Ti5 Motor NMT Request Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; - } - // can_sender_->AddMessage(sync_command_->ID, sync_command_, false); - - - // sdo - for (auto node_id: node_ids_) { - sdo_commands_[node_id] = dynamic_cast *>( - message_manager_->GetMutableProtocolDataById(SdoRequestProtocol::ID(node_id))); - - if (sdo_commands_[node_id] == nullptr) { - CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; - } - can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true); - } - - // pdo1 - for (auto node_id: node_ids_) { - rpdo1_commands_[node_id] = dynamic_cast( - message_manager_->GetMutableProtocolDataById(Ti5MotorRPDO1::ID(node_id))); - - if (rpdo1_commands_[node_id] == nullptr) { - CMVR_LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!"; - return ErrorCode::CANBUS_ERROR; - } - can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true); - } - - - - // need sleep to ensure all messages received - CMVR_LOG(INFO) << "Motor Controller is initialized."; - - - return ErrorCode::OK; -} - -ErrorCode MotorController::Start() { - auto ret = ErrorCode::OK; - ret = can_sender_->Start(); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to start can sender."; - return ret; - } - - ret = can_receiver_->Start(); - if (ret != ErrorCode::OK) { - CMVR_LOG(ERROR) << "Failed to start can receiver."; - return ret; - } - return ret; -} - - -void MotorController::SetMode(uint8_t node_id, msgs::RunMode mode) { - - cur_mode_ = mode; - // 1 : 先设置模式 - auto data = static_cast(mode); - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,OPERATION_MODE_6060,SUB_INDEX_0,data); - - - - // 2 : 状态机步进 —— Shutdown（0x06） - controlword_t cw = {}; - cw.quick_stop = 1; - cw.enable_voltage = 1; - SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20); - - - // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F） - cw.switch_on = 1; - cw.enable_operation = 1; - SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value,20); - - - - switch (mode) { - case RUN_MODE_PROFILE_POSITION: { - - // 4 : 设置目标位置（为当前位置） - auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos); - - - // 5 : 触发位置运动（new_set_point 翻转） - cw.new_set_point = 1; - cw.change_set_immediately = 1; - SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); - - // 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标） - cw.new_set_point = 0; - SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); - break; - } - - case RUN_MODE_CYCLIC_SYNC_POSITION: { - // 设置目标位置为当前位置 - auto cur_pos = GetRobotDetail()->motors().at(node_id).position(); - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TARGET_POSITION_607A,SUB_INDEX_0,cur_pos); - - //3 : 使能 15 - cw.enable_operation = 1; - cw.switch_on = 1; - SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CONTROL_WORD_6040,SUB_INDEX_0,cw.value); - break; - } - - - - default: - // TODO: Handle unspecified or unknown mode - break; - } -} - -void MotorController::ConfigTPDO1(uint8_t node_id) { - //TDPO1 配置 状态字 和 控制字 - // 1: 失能 pdo - uint32_t cob_id = TPDO1_BASE_ID_180 + node_id; - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (1U << 31)); - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,0); - - // 2: 配置为异步 - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_COMM_1800,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC); - - // 3：配置约束时间 unit:0.1ms - SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_3,1); - - // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO1_COMM_1800,SUB_INDEX_5,1000); - - // 5 ：映射控制字 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_1,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); - - //6 : 映射状态字 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_2,STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16); - - //7 : 映射模式 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_3,MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8); - - //8 映射错误码 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_MAP_1A00,SUB_INDEX_4,ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16); - - //9 写入该PDO映射对象总个数 - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO1_MAP_1A00,SUB_INDEX_0,4); - - //10 使能 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO1_COMM_1800,SUB_INDEX_1,cob_id | (0U << 31)); - -} - - -void MotorController::ConfigTPDO2(uint8_t node_id) { - // 1: 失能 pdo - uint32_t cob_id = TPDO2_BASE_ID_280 + node_id; - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (1U << 31)); - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,0); - - // 2: 配置为异步 - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_COMM_1801,SUB_INDEX_2,ASYNC_MANUFACTURER_SPECIFIC); - - // 3：配置约束时间 unit:0.1ms - SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_3,10000); - - // 4 : 配置周期发送时间 unit : ms - SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,TPDO2_COMM_1801,SUB_INDEX_5,1000); - - // 5 ：映射当前位置 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_1,ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32); - - //6 : 映射当前速度 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_MAP_1A01,SUB_INDEX_2,ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32); - - //9 写入该PDO映射对象总个数 - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,TPDO2_MAP_1A01,SUB_INDEX_0,2); - - //10 使能 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,TPDO2_COMM_1801,SUB_INDEX_1,cob_id | (0U << 31)); - -} - -void MotorController::ConfigRPDO1(uint8_t node_id,bool start) { - - // 1: 失能 pdo - uint32_t cob_id = RPDO1_BASE_ID_200 + node_id; - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (1U << 31)); - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,0); - - // 2: 配置为 - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_COMM_1400,SUB_INDEX_2,SYNC_EVENT_DRIVEN); - - // // 3：配置约束时间 unit:0.1ms - // SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10); - // - // // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送 - // SeedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0); - - // 5 ：映射位置 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_1,TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32); - - //6 : 映射控制字 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_MAP_1600,SUB_INDEX_2,CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16); - - - if (start) { - //7 写入该PDO映射对象总个数 - SeedSdoRequest(node_id,CS_WRITE_ONE_BYTE,RPDO1_MAP_1600,SUB_INDEX_0,2); - - //8 使能 - SeedSdoRequest(node_id,CS_WRITE_FOUR_BYTES,RPDO1_COMM_1400,SUB_INDEX_1,cob_id | (0U << 31)); - - } - - - -} - -void MotorController::SetPPTargetPosBySdo(uint8_t node_id, int32_t pos) { - - controlword_t cw = {}; - cw.switch_on = 1; - cw.enable_voltage = 1; - cw.enable_operation = 1; - cw.quick_stop = 1; - cw.change_set_immediately = 1; - - // 1. 设置目标位置 - SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos); - - - // 2. 设置触发位（bit4 = 1） - cw.new_set_point = 1; - SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); - - - // 3. 清除触发位（bit4 = 0），准备下一次触发 - cw.new_set_point = 0; - SeedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value); -} - -void MotorController::SetPPTargetPosByPdo(uint8_t node_id, int32_t pos) { - - // 触发目标位置运动 - controlword_t cw; - cw.value = 0x0F; - cw.new_set_point = 1; - cw.change_set_immediately = 1; - - rpdo1_commands_[node_id]->SetTargetPos(pos); - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); - - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - - cw.new_set_point = 0; - rpdo1_commands_[node_id]->SetCtrlWord(cw.value); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); - -} - - -void MotorController::SetTargetPosition(uint8_t node_id, double angle_rad) { - - auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - - switch (cur_mode_) { - case RUN_MODE_CYCLIC_SYNC_POSITION: - SetCSPTargetPosByPdo(node_id,static_cast(cmd)); - break; - case RUN_MODE_PROFILE_POSITION: - SetPPTargetPosByPdo(node_id,static_cast(cmd)); - // SetPPTargetPosBySdo(node_id,static_cast(cmd)); - break; - - } -} - - - - - - - - -void MotorController::ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) { - - SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed); - SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel); - SeedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel); -} - -void MotorController::SetCSPTargetPosByPdo(uint8_t node_id, int32_t pos) { - rpdo1_commands_[node_id]->SetTargetPos(pos); - rpdo1_commands_[node_id]->SetCtrlWord(0x0F); - can_sender_->Update(rpdo1_commands_[node_id]->ID()); -} - -void MotorController::SeedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) { - nmt_command_->RequestService(node_id,command); - can_sender_->Update(nmt_command_->ID); - std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); -} - - -void MotorController::SeedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index, msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms) { - sdo_commands_[node_id]->SetFrameData(cs,index,sub_index,data); - can_sender_->Update(sdo_commands_[node_id]->ID()); - std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms)); -} - - diff --git a/cmvr-es/devices/robot/c701/motor/motor_controller.h b/cmvr-es/devices/robot/c701/motor/motor_controller.h deleted file mode 100644 index ba8551be..00000000 --- a/cmvr-es/devices/robot/c701/motor/motor_controller.h +++ /dev/null @@ -1,101 +0,0 @@ -// -// Created by lgv on 2025/7/17. -// - -#pragma once - -#include - -#include "canbus/can_client/socket/socket_can_client_raw.h" -#include "can_comm/can_sender.h" -#include "canbus/can_comm/can_receiver.h" -#include "canbus/can_comm/can_sender.h" -#include "canbus/can_comm/message_manager.h" -#include "cmvr/msgs/error_code.pb.h" -#include "cmvr/msgs/robot_detail.pb.h" -#include "canbus/canopen/sdo_request_protocol.h" -#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h" -#include "canbus/canopen/sync_protocol.h" - -#include "canbus/canopen/nmt_response_protocol.h" -#include "canopen/nmt_request_protocol.h" -namespace cmvr { - namespace robot { - namespace c701 { - class MotorController { - public: - MotorController(std::vector node_ids):node_ids_(node_ids) {} - - MotorController(const MotorController &) = delete; - - MotorController &operator=(const MotorController &) = delete; - - /** - * @brief Destructor. - */ - virtual ~MotorController() = default; - - msgs::ErrorCode Init(cmvr::device::AbstractCanbus *can_client,bool enable_log); - msgs::ErrorCode Start(); - - std::unique_ptr GetRobotDetail() { - auto data_ptr = std::make_unique(); - message_manager_->GetSensorData(data_ptr.get()); - return data_ptr; - } - - - void SeedNmtRequest(uint8_t node_id,msgs::NmtCommand command,uint32_t delay_ms = 10); - - void SeedSdoRequest(uint8_t node_id,msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data,uint32_t delay_ms = 10); - - - // 设置软件限位（上限和下限） - void SetPositionLimits(uint8_t node_id, int32_t lower_limit, int32_t upper_limit); - void ConfigProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel); - void ConfigTPDO1(uint8_t node_id); - void ConfigTPDO2(uint8_t node_id); - - // 目标位置 607A + 控制字 6040 - void ConfigRPDO1(uint8_t node_id,bool start); - void SetTargetPosition(uint8_t node_id,double angle_rad); - void SetMode(uint8_t node_id,msgs::RunMode mode); - - - - - private: - static constexpr double GearRatio = 101.0; // 电机减速比 - static constexpr double RADTODEG = 180.0 / 3.1415926; - std::vector node_ids_{}; // 要控制的电机 id - // std::map control_commands_{}; - - - // nmt - device::NmtRequestProtocol* nmt_command_{nullptr}; - - //sync - device::SyncProtocol* sync_command_{nullptr}; - - // sdo - std::map*> sdo_commands_{}; - - // rpdo1 - std::map rpdo1_commands_{}; - - - std::shared_ptr> can_receiver_{nullptr}; - std::shared_ptr> can_sender_{nullptr}; - std::shared_ptr> message_manager_{nullptr}; - - msgs::RunMode cur_mode_{msgs::RunMode::RUN_MODE_UNSPECIFIED}; - - - void SetPPTargetPosBySdo(uint8_t node_id,int32_t pos); - void SetPPTargetPosByPdo(uint8_t node_id,int32_t pos); - void SetCSPTargetPosByPdo(uint8_t node_id,int32_t pos); - - }; - } - } -} diff --git a/cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp b/cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp deleted file mode 100644 index f060c132..00000000 --- a/cmvr-es/devices/robot/c701/motor/motor_controller_test.cpp +++ /dev/null @@ -1,87 +0,0 @@ -// -// Created by lgv on 2025/7/18. -// - - -#include - -#include "robot/c701/motor/motor_controller.h" -#include "canbus/can_comm/can_sender.h" -#include "canbus/can_comm/message_manager.h" - -#include "cmvr/msgs/robot_detail.pb.h" -#include "canbus/canopen/sdo_request_protocol.h" -#include "canbus/can_client/socket/socket_can_client_raw.h" -// #include "cmvr/msgs/ti5_motor.pb.h" - -using cmvr::msgs::RobotDetail; -using cmvr::msgs::ErrorCode; -using cmvr::device::CanSender; -using cmvr::device::MessageManager; - -using cmvr::robot::c701::MotorController; -using cmvr::device::SdoRequestProtocol; -using cmvr::device::SocketCanClientRaw; - -using namespace cmvr::msgs; - - -TEST(MotorControllerTest, MotorControllerCmdTest) { - - - std::vector node_ids = {0x03}; - MotorController controller(node_ids); - - - - - cmvr::config::SocketCanConfig can_config; - can_config.set_channel_id(0); - SocketCanClientRaw can_client(can_config); - can_client.start(); - - - auto result = controller.Init(&can_client, false); - controller.Start(); - - for (uint8_t node_id: node_ids) { - - - - controller.ConfigTPDO1(node_id); - controller.ConfigTPDO2(node_id); - controller.ConfigRPDO1(node_id,true); - controller.ConfigProfile(node_id,1000,1000,1000); - - - - controller.SeedNmtRequest(node_id,NMT_ENTER_PRE_OPERATIONAL); - - controller.SeedNmtRequest(node_id,NMT_START_REMOTE_NODE); - - std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - } - - for (uint8_t node_id: node_ids) { - controller.SetMode(node_id,RUN_MODE_CYCLIC_SYNC_POSITION); - - - - controller.SetTargetPosition(node_id,3.14); - // controller.SyncStart(); - // controller.SetTarget(node_id,3.14,1000); - } - - while (true) { - - - // controller.SetNmtRequest(test_id,NMT_RESET_NODE); - // controller.SeedSdoRequest(test_id,CS_READ_REQUEST,ACTUAL_POSITION_6064,SUB_INDEX_0,0); - - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - - } - - - EXPECT_EQ(result, ErrorCode::CANBUS_ERROR); -} diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp deleted file mode 100644 index e5025791..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.cpp +++ /dev/null @@ -1,27 +0,0 @@ -// -// Created by lgv on 2025/7/28. -// - -#include "robot/c701/motor/protocol/ti5_motor_rpdo1.h" -#include "common/base/logging/logger.h" - -using namespace cmvr::msgs; -using namespace cmvr::device; -using namespace cmvr::robot::motor; - - -void Ti5MotorRPDO1::UpdateData(uint8_t *data) { - std::lock_guard lock(mutex_); - data[0] = target_pos_ & 0xFF; - data[1] = target_pos_ >> 8 & 0xFF; - data[2] = target_pos_ >> 16 & 0xFF; - data[3] = target_pos_ >> 24 & 0xFF; - - data[4] = ctrl_word_ & 0xFF; - data[5] = ctrl_word_ >> 8 & 0xFF; - - // data[4] = target_vel_ & 0xFF; - // data[5] = target_vel_ >> 8 & 0xFF; - // data[6] = target_vel_ >> 16 & 0xFF; - // data[7] = target_vel_ >> 24 & 0xFF; -} diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h deleted file mode 100644 index 770dc77b..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_rpdo1.h +++ /dev/null @@ -1,60 +0,0 @@ -// -// Created by lgv on 2025/7/28. -// -#pragma once -#include "canbus/can_comm/protocol_data.h" -#include "cmvr/msgs/robot_detail.pb.h" -#include - -namespace cmvr { - namespace robot { - namespace motor { - class Ti5MotorRPDO1 : public device::ProtocolData { - public: - static constexpr uint32_t BASE_ID = msgs::RPDO1_BASE_ID_200; - - static uint32_t ID(uint8_t node_id) { - return BASE_ID + node_id; - } - uint32_t ID() const{ - return BASE_ID + node_id_; - } - - explicit Ti5MotorRPDO1(uint8_t node_id) : node_id_(node_id) {} - - void UpdateData(uint8_t *data) override; - - int32_t GetLength() const override { - return 0x06; - } - - uint32_t GetPeriod() const override { - return 1000 * 1; // 5 ms - } - - void SetTargetPos(int32_t position) { - std::lock_guard lock(mutex_); - target_pos_ = position; - } - - void SetTargetVel(int32_t velocity) { - std::lock_guard lock(mutex_); - target_vel_ = velocity; - } - - void SetCtrlWord(uint16_t ctrl_word) { - std::lock_guard lock(mutex_); - ctrl_word_ = ctrl_word; - } - - private: - mutable std::mutex mutex_; - uint8_t node_id_{0}; - int32_t target_pos_{0}; - int32_t target_vel_{0}; - - uint16_t ctrl_word_{0}; - }; - } - } -} \ No newline at end of file diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp deleted file mode 100644 index fc9e020a..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "common/base/logging/logger.h" -// -// Created by lgv on 2025/7/24. -// - -#include "ti5_motor_sdo_response.h" - - -using namespace cmvr::robot::motor; -using namespace cmvr::msgs; - -void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response, - cmvr::msgs::RobotDetail *sensor_data) const { - // 通过 node_id 获取对应的电机状态（Ti5_MotorStatus） - auto &motors_map = *sensor_data->mutable_motors(); - auto *motor_status = &motors_map[this->node_id_]; - - // 先把基础的 sdo_response 拷贝进去 - motor_status->mutable_sdo_response()->CopyFrom(sdo_response); - - - switch (sdo_response.index()) { - case msgs::CONTROL_WORD_6040: - motor_status->set_ctrl_word(sdo_response.data()); - break; - case msgs::STATUS_WORD_6041: - motor_status->set_status_word(sdo_response.data()); - break; - case msgs::ACTUAL_POSITION_6064: - motor_status->set_position(static_cast(sdo_response.data())); - CMVR_LOG(INFO) << "pos = " << motor_status->position(); - break; - } - - // CMVR_LOG(INFO) << "Parsed motor SDO for node " << int(this->node_id_) - // << ": command=" << int(sdo_response.cs()) - // << ", index=" << sdo_response.index() - // << ", subindex=" << int(sdo_response.sub_index()) - // << ", data=" << sdo_response.data(); -} diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h deleted file mode 100644 index a19b3773..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_sdo_response.h +++ /dev/null @@ -1,26 +0,0 @@ -// -// Created by lgv on 2025/7/24. -// - - -#pragma once - -#include "canbus/canopen/sdo_response_protocol.h" -#include "cmvr/msgs/robot_detail.pb.h" - -namespace cmvr { - namespace robot { - namespace motor { - class Ti5MotorSdoResponse : public device::SdoResponseProtocol { - public: - explicit Ti5MotorSdoResponse(uint8_t node_id) - : SdoResponseProtocol(node_id) { - } - - protected: - void ParseSdoData(const msgs::SdoFrame &sdo_response, - cmvr::msgs::RobotDetail *sensor_data) const override; - }; - } - } -} diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp deleted file mode 100644 index 0d2bcef0..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.cpp +++ /dev/null @@ -1,36 +0,0 @@ -// -// Created by lgv on 2025/7/25. -// - -#include "ti5_motor_tpdo1.h" -#include "common/base/logging/logger.h" -#include "canbus/canopen/register.h" -using namespace cmvr::msgs; -using namespace cmvr::device; -using namespace cmvr::robot::motor; - - -void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const { - if (length < 7) { - CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; - return; - } - - auto &motors_map = *sensor_data->mutable_motors(); - auto *motor_status = &motors_map[this->node_id_]; - - motor_status->set_ctrl_word(bytes[1] << 8 | bytes[0]); - motor_status->set_status_word(bytes[3] << 8 | bytes[2]); - motor_status->set_run_mode(static_cast(bytes[4])); - motor_status->set_error_state(bytes[6] << 8 | bytes[5]); - - statusword_t st{}; - st.value = motor_status->status_word(); - - if (st.op_mode_specific > 0) { - CMVR_LOG(INFO) << st.op_mode_specific ; - } - - - -} diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h deleted file mode 100644 index c8e68471..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo1.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by lgv on 2025/7/25. -// -#pragma once - -#include "canbus/can_comm/protocol_data.h" -#include "cmvr/msgs/robot_detail.pb.h" - -namespace cmvr { - namespace robot { - namespace motor { - class Ti5MotorTPDO1 : public device::ProtocolData { - public: - static constexpr uint32_t BASE_ID = msgs::TPDO1_BASE_ID_180; - - static uint32_t ID(uint8_t node_id) { - return BASE_ID + node_id; - } - uint32_t ID() const{ - return BASE_ID + node_id_; - } - - explicit Ti5MotorTPDO1(uint8_t node_id) : node_id_(node_id) {} - - void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override; - - private: - uint8_t node_id_{0}; - }; - } - } -} \ No newline at end of file diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp deleted file mode 100644 index 3716f96c..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.cpp +++ /dev/null @@ -1,26 +0,0 @@ -// -// Created by lgv on 2025/7/25. -// - -#include "ti5_motor_tpdo2.h" -#include "common/base/logging/logger.h" - -using namespace cmvr::msgs; -using namespace cmvr::device; -using namespace cmvr::robot::motor; - - -void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const { - if (length < 8) { - CMVR_LOG(WARNING) << "Motor TPDO1 Response Protocol: data length too short: " << length; - return; - } - - auto &motors_map = *sensor_data->mutable_motors(); - auto *motor_status = &motors_map[this->node_id_]; - - motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]); - motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]); - - CMVR_LOG(INFO) << "Motor ID " << this->node_id_ << "pos = " << motor_status->position() << " speed = " << motor_status->speed(); -} diff --git a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h b/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h deleted file mode 100644 index 9eee93d2..00000000 --- a/cmvr-es/devices/robot/c701/motor/protocol/ti5_motor_tpdo2.h +++ /dev/null @@ -1,32 +0,0 @@ -// -// Created by lgv on 2025/7/25. -// -#pragma once - -#include "canbus/can_comm/protocol_data.h" -#include "cmvr/msgs/robot_detail.pb.h" - -namespace cmvr { - namespace robot { - namespace motor { - class Ti5MotorTPDO2 : public device::ProtocolData { - public: - static constexpr uint32_t BASE_ID = msgs::TPDO2_BASE_ID_280; - - static uint32_t ID(uint8_t node_id) { - return BASE_ID + node_id; - } - uint32_t ID() const{ - return BASE_ID + node_id_; - } - - explicit Ti5MotorTPDO2(uint8_t node_id) : node_id_(node_id) {} - - void Parse(const std::uint8_t *bytes, int32_t length, msgs::RobotDetail *sensor_data) const override; - - private: - uint8_t node_id_{0}; - }; - } - } -} \ No newline at end of file