diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 345c6fe..7924222 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -68,9 +68,9 @@ jobs: - name: Setup LinkerHand SDK run: | - git clone --branch v1.1.7 https://github.com/linker-bot/linkerhand-cpp-sdk.git /tmp/sdk + git clone --depth 1 --branch v2.1.8 https://github.com/linker-bot/linkerhand-cpp-sdk.git /tmp/sdk cd /tmp/sdk && mkdir build && cd build - cmake .. && make -j$(nproc) && make install && ldconfig + cmake .. -DBUILD_EXAMPLES=OFF && make install && ldconfig - name: Build Workspace id: build_step diff --git a/.gitignore b/.gitignore index e8b072e..82063c6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,6 @@ build install -log \ No newline at end of file +log +.vscode/ +src/linker_hand_cpp_ros2/scripts/_* +CLAUDE.md \ No newline at end of file diff --git a/README.md b/README.md index 8ab9554..86b23c9 100644 --- a/README.md +++ b/README.md @@ -15,7 +15,7 @@ LinkerHand-CPP-ROS2 是灵心巧手科技有限公司开发，基于 LinkerHand- - [环境要求](#-环境要求) - [快速开始](#-快速开始) - [话题详情](#-话题详情) -- [使用示例](#-使用示例) +- [控制 GUI](#️-控制-gui) - [许可证](#-许可证) - [联系我们](#-联系我们) - [更新日志](#-更新日志) @@ -24,53 +24,88 @@ LinkerHand-CPP-ROS2 是灵心巧手科技有限公司开发，基于 LinkerHand- - **操作系统**: Linux (Ubuntu 20.04+ 推荐) - **架构**: x86_64 -- **编译器**: GCC 7.0+ 或 Clang 5.0+ +- **编译器**: GCC 7.0+ 或 Clang 6.0+（C++17） - **CMake**: 3.15+ -- **依赖**: ROS2 (推荐 Foxy 或 Humble)、[linkerhand-cpp-sdk](https://github.com/linker-bot/linkerhand-cpp-sdk/releases/tag/v1.1.7) +- **依赖**: ROS2 (推荐 Foxy 或 Humble)、[linkerhand-cpp-sdk v2.1.8](https://github.com/linker-bot/linkerhand-cpp-sdk/releases/tag/v2.1.8)、`nlohmann_json` +- **GUI 可选依赖**: `python3-tk`、`rclpy`（用于 `hand_control_gui.py`） -> 注：本项目仅支持linkerhand-cpp-sdk v1.1.7及以下版本 +> 注：CI 已适配 linkerhand-cpp-sdk v2.1.8；O20 依赖 CAN-FD，当前 ROS2 节点仍以普通 CAN 为主，暂未完成 O20 实机运行适配。Modbus 通信当前仅支持 O6 / L7 / L10。 ## 🚀 快速开始 + #### 下载 + git clone https://github.com/linker-bot/linkerhand-cpp-ros2.git +#### 安装 SDK + +先安装匹配版本的 [linkerhand-cpp-sdk v2.1.8](https://github.com/linker-bot/linkerhand-cpp-sdk/releases/tag/v2.1.8)（见其 README）。本仓库通过 `find_package(linkerhand-cpp-sdk 2.0 CONFIG REQUIRED)` 引用；节点的 RPATH 会指向 `${linkerhand-cpp-sdk_LIBRARY_DIR}`，SDK 位置变更后需要重新 `colcon build`。 + #### 编译 + cd linkerhand-cpp-ros2/ - colcon build + colcon build # 全量构建 + colcon build --packages-select linker_hand_cpp_ros2 # 仅构建核心节点包 -#### 配置XML文件 +#### 启动 CAN 设备 -```bash +节点通过 SocketCAN 打开 `can0` / `can1` 等接口。使用前需先把 CAN 通道拉起（`CAN_BITRATE` 与 launch 参数保持一致）： + + sudo ip link set can0 down 2>/dev/null + sudo ip link set can0 up type can bitrate 1000000 + +> Modbus 通信当前仅支持 O6 / L7 / L10；O20 依赖 CAN-FD，暂未完成 O20 实机运行适配。 + +#### 配置 XML 文件 + +`launch/run.xml` 用于双手同时启动，`launch/run_left.xml` 用于仅启动单手。以 `run.xml` 为例（省略部分与实际默认值对齐）： + +```xml - + - - - - + + + + - - - + + + - - + + + ...... - + ``` +> 手动打开CAN设备后，执行launch会自动检测左手或右手CAN通道。 + #### 运行 + source install/setup.bash - ros2 launch linkerhand_cpp_ros2 run.xml + # 双手（按 run.xml 默认配置启动） + ros2 launch linker_hand_cpp_ros2 run.xml + # 仅左手 + ros2 launch linker_hand_cpp_ros2 run_left.xml + # 覆盖某个参数的示例 + ros2 launch linker_hand_cpp_ros2 run.xml LEFT_JOINTS:=L10 LEFT_CANBUS:=can1 + +#### 测试与静态检查 + + colcon test && colcon test-result --verbose + +项目已禁用 cpplint / uncrustify / copyright / xmllint，主要走 `ament_lint_auto` 剩余检查。 ## 📚 话题详情 @@ -78,15 +113,15 @@ LinkerHand-CPP-ROS2 是灵心巧手科技有限公司开发，基于 LinkerHand- | 话题名称 | I/O | 消息类型 | 描述 | | :--- | :--- | :--- | :--- | | /left_hand_control | Input | sensor_msgs/msg/JointState | 左手控制指令 | -| /left_hand_settings | Input | sensor_msgs/msg/String | 左手设置指令 | +| /left_hand_settings | Input | std_msgs/msg/String | 左手设置指令 | | /left_hand_touch | Output | std_msgs/msg/Float32MultiArray | 左手触觉传感器数据 | | /left_hand_state | Output | sensor_msgs/msg/JointState | 左手关节状态 | | /left_hand_info | Output | std_msgs/msg/String | 左手基本信息 | -| /right_hand_control | Input | sensor_msgs/msg/JointState | 左手控制指令 | -| /right_hand_settings | Input | sensor_msgs/msg/String | 右手设置指令 | -| /right_hand_touch | Output | std_msgs/msg/Float32MultiArray | 左手触觉传感器数据 | -| /right_hand_state | Output | sensor_msgs/msg/JointState | 左手关节状态 | -| /right_hand_info | Output | std_msgs/msg/String | 左手基本信息 | +| /right_hand_control | Input | sensor_msgs/msg/JointState | 右手控制指令 | +| /right_hand_settings | Input | std_msgs/msg/String | 右手设置指令 | +| /right_hand_touch | Output | std_msgs/msg/Float32MultiArray | 右手触觉传感器数据 | +| /right_hand_state | Output | sensor_msgs/msg/JointState | 右手关节状态 | +| /right_hand_info | Output | std_msgs/msg/String | 右手基本信息 | 针对以上话题的具体字段及其详细描述如下表所示： @@ -113,11 +148,15 @@ LinkerHand-CPP-ROS2 是灵心巧手科技有限公司开发，基于 LinkerHand- ```bash L7: ["大拇指弯曲", "大拇指横摆","食指弯曲", "中指弯曲", "无名指弯曲","小拇指弯曲","拇指旋转"] +L6/O6: ["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部", "小指根部"] + L10: ["拇指根部", "拇指侧摆","食指根部", "中指根部", "无名指根部","小指根部","食指侧摆","无名指侧摆","小指侧摆","拇指旋转"] L20: ["拇指根部", "食指根部", "中指根部", "无名指根部","小指根部","拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小指侧摆","拇指横摆","预留","预留","预留","预留","拇指尖部","食指末端","中指末端","无名指末端","小指末端"] -L21: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","预留","预留","预留","预留","大拇指中部","预留","预留","预留","预留","大拇指指尖","食指指尖","中指指尖","无名指指尖","小拇指指尖"] +G20: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","大拇指指尖","食指指尖","中指指尖","无名指指尖","小拇指指尖"] + +L21: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","预留","预留","预留","预留","大拇指中部","预留","预留","预留","预留","大拇指指尖"] L25: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","预留","预留","预留","预留","大拇指中部","食指中部","中指中部","无名指中部","小拇指中部","大拇指指尖","食指指尖","中指指尖","无名指指尖","小拇指指尖"] ``` @@ -130,17 +169,7 @@ L25: ["大拇指根部", "食指根部", "中指根部","无名指根部","小 ```bash # 清除故障码 - $ ros2 topic pub /left_hand_setting std_msgs/msg/String "data: '{\"setting_cmd\": \"clear_faults\"}'" - - # 设置电流 - $ ros2 topic pub /left_hand_setting std_msgs/msg/String "data: '{\"setting_cmd\": \"set_electric_current\", \"params\": {\"electric_current\": 50}}'" - - # 使能 - $ ros2 topic pub /left_hand_setting std_msgs/msg/String "data: '{\"setting_cmd\": \"enable\"}'" - - # 失能 - $ ros2 topic pub /left_hand_setting std_msgs/msg/String "data: '{\"setting_cmd\": \"disable\"}'" - + $ ros2 topic pub /left_hand_settings std_msgs/msg/String "data: '{\"setting_cmd\": \"clear_faults\"}'" ``` @@ -179,7 +208,11 @@ data: 'Hand direction: Left hand ``` - 压感数据 /left_hand_touch - 注意：反馈数据为一个一维数组，长度为360，由每个手指的压感数据组成。单个指头压感数据长度是72（6*12矩阵），每个手指压感数据需要单独拆分。（仅适用点阵式传感器） + 注意：反馈数据为一个一维数组，长度按型号区分： + - 常规型号：长度 360，5 指 × 72（6×12 矩阵）/指； + - O6：长度 200，5 指 × 40（4×10 矩阵）/指。 + + 每个手指压感数据需要自行按指拆分。仅适用点阵式传感器。 ```bash $ ros2 topic echo /left_hand_touch @@ -195,20 +228,34 @@ data: - '...' ``` -## 💡 使用示例 +## 🎛️ 控制 GUI + +`hand_control_gui.py` 提供基于 tkinter 的可视化面板：拖动线性滑块直接下发 `JointState`，同时显示 `state` / `touch` / `info` 反馈（含触觉热力图）。它只依赖标准话题，可与任意 `run.xml` 场景配合。 -| 序号 | 文件名称 | 描述 | -| :--- | :-------- | :--------------------------------------------- | -| 1 | src/examples/src/action_group_show_l7 | L7型号手指舞示例 | -| 2 | src/examples/src/action_group_show_l10 | L10型号手指舞示例 | -| 3 | src/examples/src/action_group_show_l20 | L20型号手指舞示例 | -| 4 | src/examples/src/loop_l20 | L20型号手势张开握拳示例 | -| 5 | src/examples/src/show_ok_l20 | L20型号手势ok示例 | -| 6 | src/examples/src/show_surround_index_finger_l20 | L20型号单指手势示例 | -| 7 | src/examples/src/show_wave_l20 | L20型号wave手势示例 | -| 8 | src/examples/src/action_group_show_l25 | L25型号手指舞示例 | -| 9 | src/examples/src/loop_l25 | L25型号手势张开握拳示例 | +

+ hand_control_gui 截图 +

+```bash +source install/setup.bash +# 自动从 /linker_hand_{side}_node 读取 HAND_JOINTS 型号参数 +ros2 run linker_hand_cpp_ros2 hand_control_gui.py +# 或显式指定型号 +ros2 run linker_hand_cpp_ros2 hand_control_gui.py --side right --model L10 +``` + +功能界面： + +- **顶部**：型号 / 侧别 下拉框；「实时」开关（打开后按 `--rate` 周期 (默认60hz) 把当前滑块值发布出去）。 +- **中部左**：反馈面板 —— `位置` / `速度` / `力矩` 三行数值，下方是 5 指触觉热力图；型号切换会自动重建。 +- **中部右**：控制面板 —— 通过顶部 tab 在 `位置` / `速度` / `力矩` 之间切换；点击或拖动线条即可调整（0–255）；右侧 Spinbox 支持精确输入。 +- **底部右下**：`复制位置值` 链接，把当前所有关节位置以 `"255, 255, 255, ..."` 的形式写入剪贴板。 + +常用参数：`--side left|right`、`--model L6|O6|L7|L10|L20|L21|L25|G20|O20`、`--rate `、`--no-live`（禁用自动发布，仅调试用）。 + +初始位置：`hand_control_gui.py` 中的 `POSITION_DEFAULTS` 按型号定制（默认全 255）；`G20` 使用 `[255, 255, 255, 255, 255, 255, 130, 125, 125, 125, 255, 255, 255, 255, 255, 255]` 作为张开手掌的安全初值。 + +依赖 `python3-tk` 与 `rclpy`。 ## 📄 许可证 @@ -224,8 +271,12 @@ Copyright (c) 2026 灵心巧手（北京）科技有限公司 ## 📝 更新日志 -详细的版本更新记录请参考 [CHANGELOG.md](CHANGELOG.md)（待创建）。 +详细的版本更新记录请查看 [`git log`](https://github.com/linker-bot/linkerhand-cpp-ros2/commits/main) 或本地执行： + +```bash +git log --oneline --decorate +``` --- -**注意**: 使用前请确保设备已正确连接并配置好通信接口。 \ No newline at end of file +**注意**: 使用前请确保设备已正确连接并配置好通信接口。 diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt deleted file mode 100644 index 57efa31..0000000 --- a/src/examples/CMakeLists.txt +++ /dev/null @@ -1,137 +0,0 @@ -cmake_minimum_required(VERSION 3.5) -project(examples) - -# Default to C99 -if(NOT CMAKE_C_STANDARD) - set(CMAKE_C_STANDARD 99) -endif() - -# Default to C++14 -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() - -# Enable additional compiler warnings -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() - -if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") - set(LIB_SUBDIR "x86_64") -elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|arm64") - set(LIB_SUBDIR "aarch64") -else() - message(WARNING "Unknown architecture, defaulting to x86_64") - set(LIB_SUBDIR "x86_64") -endif() - -# Find dependencies -find_package(ament_cmake REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(sensor_msgs REQUIRED) - -# Define common include directories -set(COMMON_INCLUDE_DIRS - /usr/local/linker_hand_cpp_sdk/include - /usr/local/linker_hand_cpp_sdk/third_party/Robotic_Arm/include - include -) - -#----------------------------------------------------------------------------- -# LINKER_HAND_CPP_SDK -#----------------------------------------------------------------------------- -find_library(LINKER_HAND_LIB - NAMES linkerhand_cpp_sdk linkerhand_cpp - PATHS ${CMAKE_CURRENT_SOURCE_DIR}/lib/${LIB_SUBDIR} - /usr/local/lib/linkerhand-cpp-sdk/${LIB_SUBDIR} - /usr/lib/linkerhand-cpp-sdk/${LIB_SUBDIR} - ${CMAKE_INSTALL_PREFIX}/lib/linkerhand-cpp-sdk/${LIB_SUBDIR} - NO_DEFAULT_PATH -) - -set(LINKER_HAND_INCLUDE_DIR - ${CMAKE_CURRENT_SOURCE_DIR}/include - /usr/local/include/linkerhand-cpp-sdk - /usr/include/linkerhand-cpp-sdk - ${CMAKE_INSTALL_PREFIX}/include/linkerhand-cpp-sdk -) - -if(NOT LINKER_HAND_LIB) - message(FATAL_ERROR "linkerhand_cpp_sdk library not found!") -endif() - -if(NOT LINKER_HAND_INCLUDE_DIR) - message(FATAL_ERROR "LinkerHand headers not found!") -endif() - -message(STATUS "Found linkerhand_cpp_sdk library: ${LINKER_HAND_LIB}") -message(STATUS "Found LinkerHand headers: ${LINKER_HAND_INCLUDE_DIR}") - -#----------------------------------------------------------------------------- -# INCLUDE_DIRECTORIES -#----------------------------------------------------------------------------- -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${LINKER_HAND_INCLUDE_DIR} -) - -# ---------------------------------------------------------------------- - -# Define common link libraries -set(COMMON_LIBS ${LINKER_HAND_LIB} pthread) - -# Define source files -set(SOURCE_FILES - action_group_show_l7 - action_group_show_l10 - action_group_show_l20 - action_group_show_l25 - loop_l20 - loop_l25 - show_surround_index_finger_l20 - show_ok_l20 - show_wave_l20 -) - -# Create and name a shared library -foreach(SOURCE_FILE ${SOURCE_FILES}) - add_executable(${SOURCE_FILE} src/${SOURCE_FILE}.cpp) - target_include_directories(${SOURCE_FILE} PUBLIC - $ - $ - ) - ament_target_dependencies(${SOURCE_FILE} - rclcpp - std_msgs - sensor_msgs - ) - target_link_libraries(${SOURCE_FILE} ${COMMON_LIBS}) - install(TARGETS ${SOURCE_FILE} - DESTINATION lib/${PROJECT_NAME} - ) -endforeach() - -# Install launch files -# install(DIRECTORY launch -# DESTINATION share/${PROJECT_NAME}/ -# ) - -# Install configuration files -# install(DIRECTORY config -# DESTINATION share/${PROJECT_NAME}/ -# ) - -# Install settings file -# install(FILES config/setting.yaml -# DESTINATION config -# ) - -# If BUILD_TESTING is enabled, include test-related dependencies -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -# Package information -ament_package() diff --git a/src/examples/package.xml b/src/examples/package.xml deleted file mode 100644 index 6ff7971..0000000 --- a/src/examples/package.xml +++ /dev/null @@ -1,21 +0,0 @@ - - - - examples - 0.0.0 - TODO: Package description - lst - Apache2.0 - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - rclcpp - std_msgs - sensor_msgs - - diff --git a/src/examples/src/action_group_show_l10.cpp b/src/examples/src/action_group_show_l10.cpp deleted file mode 100644 index 3b53fee..0000000 --- a/src/examples/src/action_group_show_l10.cpp +++ /dev/null @@ -1,454 +0,0 @@ -// 本demo为适配球形拇指根部关节版本L10灵巧手，默认支持右手，如需左手，请将 HAND_TYPE::RIGHT 改为 HAND_TYPE::LEFT - -#include -#include -#include -#include -#include - -using namespace std; - -bool running = true; - -vector joint_order = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10"}; - -map hand = { - {"joint1", 255}, //拇指根部弯曲 - {"joint2", 128}, //拇指侧摆 - {"joint3", 255}, //食指根部弯曲 - {"joint4", 255}, //中指根部弯曲 - {"joint5", 255}, //无名指根部弯曲 - {"joint6", 255}, //小指根部弯曲 - {"joint7", 128}, //食指侧摆 - {"joint8", 128}, //中指侧摆 - {"joint9", 128}, //无名指侧摆 - {"joint10", 255} //拇指旋转 -}; - -int show_count = 0; -int show_count_obj = 0; -int show_step = 0; - -vector showLeft() { - static vector position; - show_count++; - if (show_count >= show_count_obj) { - show_count = 0; - switch (show_step) { - case 0: - show_step++; - show_count_obj = 100; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 128; - hand["joint8"] = 128; - hand["joint9"] = 128; - hand["joint10"] = 250; - break; - case 1: - show_step++; - show_count_obj = 10; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint5"] = 0; - hand["joint6"] = 0; - hand["joint10"] = 250; - break; - case 2: - show_step++; - show_count_obj = 30; - hand["joint1"] = 40; - hand["joint2"] = 240; - hand["joint10"] = 80; - break; - case 3: - show_step++; - show_count_obj = 10; - hand["joint7"] = 200; - break; - case 4: - show_step++; - show_count_obj = 10; - hand["joint7"] = 50; - break; - case 5: - show_step++; - show_count_obj = 15; - hand["joint7"] = 128; - break; - case 6: - show_step++; - show_count_obj = 2; - hand["joint7"] = 50; - break; - case 7: - show_step++; - show_count_obj = 10; - hand["joint7"] = 128; - break; - case 8: - show_step++; - show_count_obj = 10; - hand["joint7"] = 50; - break; - case 9: - show_step++; - show_count_obj = 10; - hand["joint7"] = 128; - break; - case 10: - show_step++; - show_count_obj = 15; - hand["joint3"] = 100; - hand["joint4"] = 100; - break; - case 11: - show_step++; - show_count_obj = 15; - hand["joint3"] = 250; - hand["joint4"] = 250; - break; - case 12: - show_step++; - show_count_obj = 15; - hand["joint3"] = 100; - hand["joint4"] = 100; - break; - case 13: - show_step++; - show_count_obj = 15; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 128; - hand["joint8"] = 128; - hand["joint9"] = 128; - hand["joint10"] = 250; - break; - case 14: - show_step++; - show_count_obj = 40; - hand["joint1"] = 40; - hand["joint2"] = 240; - hand["joint10"] = 80; - break; - case 15: - show_step++; - break; - case 16: - show_step++; - show_count_obj = 30; - hand["joint3"] = 10; - hand["joint4"] = 10; - hand["joint5"] = 10; - hand["joint6"] = 10; - break; - case 17: - show_step++; - show_count_obj = 15; - hand["joint6"] = 250; - break; - case 18: - show_step++; - show_count_obj = 15; - hand["joint5"] = 250; - break; - case 19: - show_step++; - show_count_obj = 15; - hand["joint4"] = 250; - break; - case 20: - show_step++; - show_count_obj = 15; - hand["joint3"] = 250; - break; - case 21: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint2"] = 110; - hand["joint10"] = 240; - break; - case 22: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint2"] = 10; - hand["joint10"] = 110; - break; - case 23: - show_step++; - show_count_obj = 40; - hand["joint1"] = 0; - hand["joint2"] = 10; - hand["joint10"] = 110; - break; - case 24: - show_step++; - show_count_obj = 30; - hand["joint1"] = 0; - hand["joint2"] = 240; - hand["joint10"] = 110; - break; - case 25: - show_step++; - show_count_obj = 50; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint10"] = 110; - break; - case 26: - show_step++; - show_count_obj = 10; - hand["joint7"] = 200; - hand["joint8"] = 200; - hand["joint9"] = 200; - break; - case 27: - show_step++; - show_count_obj = 15; - hand["joint7"] = 80; - hand["joint8"] = 80; - hand["joint9"] = 80; - break; - case 28: - show_step++; - show_count_obj = 20; - hand["joint7"] = 128; - hand["joint8"] = 128; - hand["joint9"] = 128; - break; - case 29: - show_step++; - show_count_obj = 15; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint10"] = 250; - break; - case 30: - show_step++; - break; - case 31: - show_step++; - break; - case 32: - show_step++; - break; - case 33: - show_step++; - show_count_obj = 15; - hand["joint3"] = 0; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint10"] = 250; - break; - case 34: - show_step++; - show_count_obj = 15; - hand["joint4"] = 0; - break; - case 35: - show_step++; - show_count_obj = 15; - hand["joint5"] = 0; - break; - case 36: - show_step++; - show_count_obj = 15; - hand["joint6"] = 0; - break; - case 37: - show_step++; - show_count_obj = 40; - hand["joint1"] = 0; - break; - case 38: - show_step++; - show_count_obj = 40; - hand["joint1"] = 250; - hand["joint2"] = 230; - hand["joint10"] = 250; - break; - case 39: - show_step++; - show_count_obj = 30; - hand["joint3"] = 250; - hand["joint6"] = 250; - break; - case 40: - show_step++; - show_count_obj = 40; - hand["joint1"] = 10; - hand["joint2"] = 40; - hand["joint10"] = 60; - break; - case 41: - show_step++; - show_count_obj = 15; - hand["joint7"] = 80; - hand["joint9"] = 200; - break; - case 42: - show_step++; - show_count_obj = 15; - hand["joint7"] = 200; - hand["joint9"] = 80; - break; - case 43: - show_step++; - show_count_obj = 15; - hand["joint7"] = 80; - hand["joint9"] = 200; - break; - case 44: - show_step++; - show_count_obj = 15; - hand["joint7"] = 200; - hand["joint9"] = 80; - break; - case 45: - show_step++; - show_count_obj = 15; - hand["joint7"] = 128; - hand["joint9"] = 128; - break; - case 46: - show_step++; - show_count_obj = 50; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 128; - hand["joint8"] = 128; - hand["joint9"] = 128; - hand["joint10"] = 250; - break; - case 47: - show_step++; - show_count_obj = 50; - hand["joint1"] = 130; - hand["joint2"] = 130; - hand["joint3"] = 130; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint10"] = 90; - break; - case 48: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 120; - break; - case 49: - show_step++; - show_count_obj = 35; - hand["joint1"] = 120; - hand["joint4"] = 130; - hand["joint10"] = 60; - break; - case 50: - show_step++; - show_count_obj = 30; - hand["joint1"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 145; - break; - case 51: - show_step++; - show_count_obj = 35; - hand["joint1"] = 113; - hand["joint2"] = 103; - hand["joint5"] = 128; - hand["joint10"] = 42; - break; - case 52: - show_step++; - show_count_obj = 30; - hand["joint1"] = 250; - hand["joint5"] = 250; - break; - case 53: - show_step++; - show_count_obj = 40; - hand["joint1"] = 118; - hand["joint2"] = 103; - hand["joint6"] = 120; - hand["joint10"] = 22; - break; - case 54: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 128; - hand["joint8"] = 128; - hand["joint9"] = 128; - hand["joint10"] = 250; - break; - default: - break; - } - position.clear(); - for (const auto& joint : joint_order) { // 按照固定的顺序提取关节值 - // std::cout << joint << " " << hand[joint] << std::endl; - position.push_back(static_cast(hand[joint])); - } - } - return position; -} - -class JointStatePublisher : public rclcpp::Node { -public: - JointStatePublisher() : Node("action_group_show") { - joint_state_pub = this->create_publisher("/right_hand_control", 10); - timer = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&JointStatePublisher::publishJointState, this)); - } - -private: - void publishJointState() { - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = this->now(); - joint_state.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10"}; - vector position = showLeft(); - joint_state.position = position; - joint_state.velocity.resize(position.size(), 100.0); - joint_state.effort.resize(position.size(), 200.0); - joint_state_pub->publish(joint_state); - } - - rclcpp::Publisher::SharedPtr joint_state_pub; - rclcpp::TimerBase::SharedPtr timer; -}; - -void signalHandler(int signal) { - (void)signal; - running = false; - rclcpp::shutdown(); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - signal(SIGINT, signalHandler); - rclcpp::spin(std::make_shared()); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/action_group_show_l20.cpp b/src/examples/src/action_group_show_l20.cpp deleted file mode 100644 index 7672689..0000000 --- a/src/examples/src/action_group_show_l20.cpp +++ /dev/null @@ -1,385 +0,0 @@ -// 本demo仅支持L20，默认为右手，如需左手，请修改话题名 - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace std::chrono_literals; - -bool running = true; - -int show_count = 0; -int show_count_obj = 0; -int show_step = 0; - -vector show_left() { - vector position; - show_count++; - if (show_count >= show_count_obj) { - show_count = 0; - switch (show_step) { - case 0: - show_step++; - show_count_obj = 50; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 1: - show_step++; - show_count_obj = 10; - position = {250, 250, 250, 0, 0, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 0, 0}; - break; - case 2: - show_step++; - show_count_obj = 30; - position = {100, 250, 250, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 3: - show_step++; - show_count_obj = 10; - position = {100, 250, 250, 0, 0, 180, 200, 200, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 4: - show_step++; - show_count_obj = 10; - position = {100, 250, 250, 0, 0, 180, 50, 50, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 5: - show_step++; - show_count_obj = 15; - position = {100, 250, 250, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 6: - show_step++; - show_count_obj = 10; - position = {100, 250, 250, 0, 0, 180, 58, 200, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 7: - show_step++; - show_count_obj = 10; - position = {100, 250, 250, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 8: - show_step++; - show_count_obj = 10; - position = {100, 250, 250, 0, 0, 180, 58, 200, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 9: - show_step++; - show_count_obj = 10; - position = {100, 250, 250, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 10: - show_step++; - show_count_obj = 15; - position = {100, 100, 100, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 100, 100, 0, 0}; - break; - case 11: - show_step++; - show_count_obj = 15; - position = {100, 250, 250, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 12: - show_step++; - show_count_obj = 15; - position = {100, 100, 100, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 100, 100, 0, 0}; - break; - case 13: - show_step++; - show_count_obj = 15; - position = {100, 250, 250, 0, 0, 180, 128, 128, 128, 128, 200, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 14: - show_step++; - show_count_obj = 40; - position = {250, 250, 250, 0, 0, 150, 128, 128, 128, 128, 250, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 15: - show_step++; - show_count_obj = 10; - position = {250, 250, 250, 0, 0, 5, 128, 128, 128, 128, 250, 0, 0, 0, 0, 0, 250, 250, 0, 0}; - break; - case 16: - show_step++; - show_count_obj = 30; - position = {250, 100, 100, 100, 100, 5, 128, 128, 128, 128, 250, 0, 0, 0, 0, 0, 10, 10, 10, 10}; - break; - case 17: - show_step++; - show_count_obj = 15; - position = {250, 100, 100, 100, 250, 5, 128, 128, 128, 128, 250, 0, 0, 0, 0, 0, 10, 10, 10, 250}; - break; - case 18: - show_step++; - show_count_obj = 15; - position = {250, 100, 100, 250, 250, 5, 128, 128, 128, 128, 250, 0, 0, 0, 0, 0, 10, 10, 250, 250}; - break; - case 19: - show_step++; - show_count_obj = 15; - position = {250, 100, 250, 250, 250, 5, 128, 128, 128, 128, 250, 0, 0, 0, 0, 0, 10, 250, 250, 250}; - break; - case 20: - show_step++; - show_count_obj = 15; - position = {250, 250, 250, 250, 250, 5, 128, 128, 128, 128, 250, 0, 0, 0, 0, 0, 250, 250, 250, 250}; - break; - case 21: - show_step++; - show_count_obj = 10; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 22: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 10, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 23: - show_step++; - show_count_obj = 40; - position = {0, 250, 250, 250, 250, 250, 128, 128, 128, 128, 10, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 24: - show_step++; - show_count_obj = 30; - position = {0, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 25: - show_step++; - show_count_obj = 50; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 26: - show_step++; - show_count_obj = 10; - position = {250, 250, 250, 250, 250, 250, 200, 200, 200, 200, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 27: - show_step++; - show_count_obj = 15; - position = {250, 250, 250, 250, 250, 250, 80, 80, 80, 80, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 28: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 29: - show_step++; - show_count_obj = 15; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 250, 250, 250}; - break; - case 30: - show_step++; - show_count_obj = 15; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 250, 250}; - break; - case 31: - show_step++; - show_count_obj = 15; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 250}; - break; - case 32: - show_step++; - show_count_obj = 15; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 33: - show_step++; - show_count_obj = 15; - position = {250, 0, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 34: - show_step++; - show_count_obj = 15; - position = {250, 0, 0, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 35: - show_step++; - show_count_obj = 15; - position = {250, 0, 0, 0, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 36: - show_step++; - show_count_obj = 15; - position = {250, 0, 0, 0, 0, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 37: - show_step++; - show_count_obj = 40; - position = {0, 0, 0, 0, 0, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 200, 0, 0, 0, 0}; - break; - case 38: - show_step++; - show_count_obj = 40; - position = {250, 0, 0, 0, 0, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 39: - show_step++; - show_count_obj = 30; - position = {250, 250, 0, 0, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 0, 0, 250}; - break; - case 40: - show_step++; - show_count_obj = 40; - position = {250, 250, 0, 0, 250, 200, 128, 128, 128, 128, 100, 0, 0, 0, 0, 100, 250, 0, 0, 250}; - break; - case 41: - show_step++; - show_count_obj = 15; - position = {250, 250, 0, 0, 250, 200, 80, 128, 128, 200, 100, 0, 0, 0, 0, 100, 250, 0, 0, 250}; - break; - case 42: - show_step++; - show_count_obj = 15; - position = {250, 250, 0, 0, 250, 200, 200, 128, 128, 80, 100, 0, 0, 0, 0, 100, 250, 0, 0, 250}; - break; - case 43: - show_step++; - show_count_obj = 15; - position = {250, 250, 0, 0, 250, 200, 80, 128, 128, 200, 100, 0, 0, 0, 0, 100, 250, 0, 0, 250}; - break; - case 44: - show_step++; - show_count_obj = 15; - position = {250, 250, 0, 0, 250, 200, 200, 128, 128, 80, 100, 0, 0, 0, 0, 100, 250, 0, 0, 250}; - break; - case 45: - show_step++; - show_count_obj = 15; - position = {250, 250, 0, 0, 250, 200, 128, 128, 128, 128, 100, 0, 0, 0, 0, 100, 250, 0, 0, 250}; - break; - case 46: - show_step++; - show_count_obj = 50; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 47: - show_step++; - show_count_obj = 50; - position = {55, 0, 250, 250, 250, 170, 128, 128, 128, 128, 70, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 48: - show_step++; - show_count_obj = 20; - position = {250, 250, 20, 250, 250, 220, 128, 128, 128, 128, 100, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 49: - show_step++; - show_count_obj = 35; - position = {55, 250, 0, 250, 250, 140, 128, 128, 128, 128, 60, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 50: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 20, 250, 170, 128, 128, 128, 128, 100, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 51: - show_step++; - show_count_obj = 35; - position = {55, 250, 250, 0, 250, 110, 128, 128, 128, 128, 50, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 52: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 250, 20, 130, 128, 128, 128, 128, 100, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 53: - show_step++; - show_count_obj = 40; - position = {55, 250, 250, 250, 0, 60, 128, 128, 128, 128, 50, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 54: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 250, 20, 130, 128, 128, 128, 128, 100, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 55: - show_step++; - show_count_obj = 40; - position = {160, 250, 250, 250, 160, 60, 128, 128, 128, 128, 50, 0, 0, 0, 0, 100, 250, 250, 250, 80}; - break; - case 56: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 250, 250, 130, 128, 128, 128, 128, 100, 0, 0, 0, 0, 250, 250, 250, 50, 250}; - break; - case 57: - show_step++; - show_count_obj = 35; - position = {160, 250, 250, 150, 250, 100, 128, 128, 128, 128, 50, 0, 0, 0, 0, 100, 250, 250, 80, 250}; - break; - case 58: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 250, 250, 180, 128, 128, 128, 128, 100, 0, 0, 0, 0, 250, 50, 250, 250, 250}; - break; - case 59: - show_step++; - show_count_obj = 35; - position = {160, 250, 150, 250, 250, 135, 128, 128, 128, 128, 70, 0, 0, 0, 0, 100, 250, 85, 250, 250}; - break; - case 60: - show_step++; - show_count_obj = 20; - position = {250, 250, 250, 250, 250, 220, 128, 128, 128, 128, 100, 0, 0, 0, 0, 250, 50, 250, 250, 250}; - break; - case 61: - show_step++; - show_count_obj = 35; - position = {165, 150, 250, 250, 250, 170, 128, 128, 128, 128, 70, 0, 0, 0, 0, 100, 80, 250, 250, 250}; - break; - case 62: - show_step++; - show_count_obj = 60; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - default: - show_step = 0; - position = {250, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - } - } - return position; -} - -class JointStatePublisher : public rclcpp::Node { -public: - JointStatePublisher() : Node("dong_test_sender") { - joint_state_pub = this->create_publisher("/right_hand_control", 10); - timer = this->create_wall_timer(33ms, std::bind(&JointStatePublisher::publishJointState, this)); - } - -private: - void publishJointState() { - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = this->now(); - joint_state.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10", "joint11", "joint12", "joint13", "joint14", "joint15", "joint16", "joint17", "joint18", "joint19", "joint20"}; - vector position = show_left(); - if (!position.empty()) { - joint_state.position = position; - } - joint_state.velocity.resize(joint_state.position.size(), 100.0); - joint_state.effort.resize(joint_state.position.size(), 200.0); - if (joint_state.position.size() > 0) joint_state_pub->publish(joint_state); - } - - rclcpp::Publisher::SharedPtr joint_state_pub; - rclcpp::TimerBase::SharedPtr timer; -}; - -void signalHandler(int sig) { - (void) sig; - running = false; - rclcpp::shutdown(); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - signal(SIGINT, signalHandler); - rclcpp::spin(std::make_shared()); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/action_group_show_l25.cpp b/src/examples/src/action_group_show_l25.cpp deleted file mode 100644 index 393baa7..0000000 --- a/src/examples/src/action_group_show_l25.cpp +++ /dev/null @@ -1,633 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; -using namespace std::placeholders; -using namespace std; - -int show_count = 0; -int show_count_obj = 0; -int show_step = 0; -vector joint_order = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10", "joint11", "joint12", "joint13", "joint14", "joint15", "joint16", "joint17", "joint18", "joint19", "joint20", "joint21", "joint22", "joint23", "joint24", "joint25"}; - -std::map hand = { - {"joint1", 250}, - {"joint2", 250}, - {"joint3", 250}, - {"joint4", 250}, - {"joint5", 250}, - {"joint6", 250}, - {"joint7", 250}, - {"joint8", 250}, - {"joint9", 250}, - {"joint10", 250}, - {"joint11", 250}, - {"joint12", 0}, - {"joint13", 0}, - {"joint14", 0}, - {"joint15", 0}, - {"joint16", 250}, - {"joint17", 250}, - {"joint18", 250}, - {"joint19", 250}, - {"joint20", 250}, - {"joint21", 250}, - {"joint22", 250}, - {"joint23", 250}, - {"joint24", 250}, - {"joint25", 250} -}; - -std::vector show_left() { - static int show_count = 0; - static int show_count_obj = 0; - static int show_step = 0; - - show_count++; - if (show_count >= show_count_obj) { - show_count = 0; - switch (show_step) { - case 0: - show_step++; - show_count_obj = 50; - hand["joint1"] = 75; - hand["joint2"] = 255; - hand["joint3"] = 255; - hand["joint4"] = 255; - hand["joint5"] = 255; - hand["joint6"] = 176; - hand["joint7"] = 51; - hand["joint8"] = 51; - hand["joint9"] = 125; - hand["joint10"] = 202; - hand["joint11"] = 202; - hand["joint12"] = 255; - hand["joint13"] = 255; - hand["joint14"] = 255; - hand["joint15"] = 255; - hand["joint16"] = 255; - hand["joint17"] = 255; - hand["joint18"] = 255; - hand["joint19"] = 255; - hand["joint20"] = 255; - hand["joint21"] = 255; - hand["joint22"] = 255; - hand["joint23"] = 255; - hand["joint24"] = 255; - hand["joint25"] = 255; - break; - case 1: - show_step++; - show_count_obj = 10; - hand["joint4"] = 0; - hand["joint5"] = 0; - hand["joint7"] = 128; - hand["joint8"] = 128; - hand["joint9"] = 128; - hand["joint10"] = 128; - hand["joint11"] = 250; - hand["joint16"] = 250; - hand["joint17"] = 250; - hand["joint18"] = 250; - hand["joint19"] = 0; - hand["joint20"] = 0; - hand["joint21"] = 250; - hand["joint22"] = 250; - hand["joint23"] = 250; - hand["joint24"] = 0; - hand["joint25"] = 0; - break; - case 2: - show_step++; - show_count_obj = 30; - hand["joint1"] = 100; - hand["joint6"] = 180; - hand["joint16"] = 0; - hand["joint21"] = 0; - break; - case 3: - show_step++; - show_count_obj = 10; - hand["joint7"] = 200; - hand["joint8"] = 200; - hand["joint11"] = 200; - break; - case 4: - show_step++; - show_count_obj = 13; - hand["joint7"] = 50; - hand["joint8"] = 50; - break; - case 5: - show_step++; - show_count_obj = 13; - hand["joint7"] = 128; - hand["joint8"] = 128; - break; - case 6: - show_step++; - show_count_obj = 2; - hand["joint7"] = 50; - hand["joint8"] = 200; - break; - case 7: - show_step++; - show_count_obj = 10; - hand["joint7"] = 128; - hand["joint8"] = 128; - break; - case 8: - show_step++; - show_count_obj = 10; - hand["joint7"] = 50; - hand["joint8"] = 200; - break; - case 9: - show_step++; - show_count_obj = 10; - hand["joint7"] = 128; - hand["joint8"] = 128; - break; - case 10: - show_step++; - show_count_obj = 15; - hand["joint2"] = 100; - hand["joint3"] = 100; - hand["joint17"] = 100; - hand["joint18"] = 100; - hand["joint22"] = 100; - hand["joint23"] = 100; - break; - case 11: - show_step++; - show_count_obj = 15; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint17"] = 250; - hand["joint18"] = 250; - hand["joint22"] = 250; - hand["joint23"] = 250; - break; - case 12: - show_step++; - show_count_obj = 15; - hand["joint2"] = 100; - hand["joint3"] = 100; - hand["joint17"] = 100; - hand["joint18"] = 100; - hand["joint22"] = 100; - hand["joint23"] = 100; - break; - case 13: - show_step++; - show_count_obj = 15; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint17"] = 250; - hand["joint18"] = 250; - hand["joint22"] = 250; - hand["joint23"] = 250; - break; - case 14: - show_step++; - show_count_obj = 40; - hand["joint1"] = 250; - hand["joint6"] = 150; - hand["joint11"] = 250; - hand["joint23"] = 250; - break; - case 15: - show_step++; - show_count_obj = 10; - hand["joint6"] = 5; - break; - case 16: - show_step++; - show_count_obj = 30; - hand["joint2"] = 100; - hand["joint3"] = 100; - hand["joint4"] = 100; - hand["joint5"] = 100; - hand["joint17"] = 100; - hand["joint18"] = 100; - hand["joint19"] = 100; - hand["joint20"] = 100; - hand["joint22"] = 100; - hand["joint23"] = 100; - hand["joint24"] = 100; - hand["joint25"] = 100; - break; - case 17: - show_step++; - show_count_obj = 15; - hand["joint5"] = 250; - hand["joint20"] = 250; - hand["joint25"] = 250; - break; - case 18: - show_step++; - show_count_obj = 15; - hand["joint4"] = 250; - hand["joint19"] = 250; - hand["joint24"] = 250; - break; - case 19: - show_step++; - show_count_obj = 15; - hand["joint3"] = 250; - hand["joint18"] = 250; - hand["joint23"] = 250; - break; - case 20: - show_step++; - show_count_obj = 15; - hand["joint2"] = 250; - hand["joint17"] = 250; - hand["joint22"] = 250; - break; - case 21: - show_step++; - show_count_obj = 10; - hand["joint6"] = 250; - hand["joint16"] = 250; - hand["joint21"] = 250; - break; - case 22: - show_step++; - show_count_obj = 20; - hand["joint11"] = 10; - break; - case 23: - show_step++; - show_count_obj = 40; - hand["joint1"] = 0; - break; - case 24: - show_step++; - show_count_obj = 30; - hand["joint11"] = 250; - break; - case 25: - show_step++; - show_count_obj = 50; - break; - case 26: - show_step++; - show_count_obj = 10; - hand["joint7"] = 200; - hand["joint8"] = 200; - hand["joint9"] = 200; - hand["joint10"] = 200; - break; - case 27: - show_step++; - show_count_obj = 15; - hand["joint7"] = 80; - hand["joint8"] = 80; - hand["joint9"] = 80; - hand["joint10"] = 80; - break; - case 28: - show_step++; - show_count_obj = 20; - hand["joint7"] = 128; - hand["joint8"] = 128; - hand["joint9"] = 128; - hand["joint10"] = 128; - break; - case 29: - show_step++; - show_count_obj = 15; - hand["joint17"] = 0; - hand["joint22"] = 0; - break; - case 30: - show_step++; - show_count_obj = 15; - hand["joint18"] = 0; - hand["joint23"] = 0; - break; - case 31: - show_step++; - show_count_obj = 15; - hand["joint19"] = 0; - hand["joint24"] = 0; - break; - case 32: - show_step++; - show_count_obj = 15; - hand["joint20"] = 0; - hand["joint25"] = 0; - break; - case 33: - show_step++; - show_count_obj = 15; - hand["joint2"] = 0; - break; - case 34: - show_step++; - show_count_obj = 15; - hand["joint3"] = 0; - break; - case 35: - show_step++; - show_count_obj = 15; - hand["joint4"] = 0; - break; - case 36: - show_step++; - show_count_obj = 15; - hand["joint5"] = 0; - break; - case 37: - show_step++; - show_count_obj = 40; - hand["joint1"] = 0; - hand["joint16"] = 200; - break; - case 38: - show_step++; - show_count_obj = 40; - hand["joint1"] = 250; - hand["joint16"] = 250; - break; - case 39: - show_step++; - show_count_obj = 30; - hand["joint2"] = 250; - hand["joint5"] = 250; - hand["joint17"] = 250; - hand["joint20"] = 250; - hand["joint22"] = 250; - hand["joint25"] = 250; - break; - case 40: - show_step++; - show_count_obj = 40; - hand["joint1"] = 100; - hand["joint6"] = 200; - hand["joint11"] = 100; - hand["joint16"] = 100; - break; - case 41: - show_step++; - show_count_obj = 15; - hand["joint7"] = 80; - hand["joint10"] = 200; - break; - case 42: - show_step++; - show_count_obj = 15; - hand["joint7"] = 200; - hand["joint10"] = 80; - break; - case 43: - show_step++; - show_count_obj = 15; - hand["joint7"] = 80; - hand["joint10"] = 200; - break; - case 44: - show_step++; - show_count_obj = 15; - hand["joint7"] = 200; - hand["joint10"] = 80; - break; - case 45: - show_step++; - show_count_obj = 15; - hand["joint7"] = 128; - hand["joint10"] = 128; - break; - case 46: - show_step++; - show_count_obj = 50; - hand["joint1"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint6"] = 250; - hand["joint11"] = 250; - hand["joint16"] = 250; - hand["joint18"] = 250; - hand["joint19"] = 250; - hand["joint21"] = 250; - hand["joint23"] = 250; - hand["joint24"] = 250; - break; - case 47: - show_step++; - show_count_obj = 50; - hand["joint1"] = 40; - hand["joint2"] = 0; - hand["joint6"] = 100; - hand["joint11"] = 70; - hand["joint17"] = 240; - hand["joint22"] = 240; - break; - case 48: - show_step++; - show_count_obj = 20; - hand["joint2"] = 250; - hand["joint6"] = 220; - hand["joint11"] = 100; - hand["joint17"] = 250; - hand["joint22"] = 250; - break; - case 49: - show_step++; - show_count_obj = 35; - hand["joint3"] = 0; - hand["joint6"] = 70; - hand["joint11"] = 60; - hand["joint18"] = 220; - hand["joint23"] = 220; - break; - case 50: - show_step++; - show_count_obj = 20; - hand["joint3"] = 250; - hand["joint6"] = 100; - hand["joint11"] = 100; - hand["joint18"] = 250; - hand["joint23"] = 250; - break; - case 51: - show_step++; - show_count_obj = 35; - hand["joint4"] = 0; - hand["joint6"] = 30; - hand["joint11"] = 50; - hand["joint19"] = 220; - hand["joint24"] = 220; - break; - case 52: - show_step++; - show_count_obj = 20; - hand["joint4"] = 250; - hand["joint6"] = 100; - hand["joint11"] = 100; - hand["joint19"] = 250; - hand["joint24"] = 250; - break; - case 53: - show_step++; - show_count_obj = 40; - hand["joint5"] = 0; - hand["joint6"] = 0; - hand["joint11"] = 40; - hand["joint20"] = 230; - hand["joint25"] = 230; - break; - case 54: - show_step++; - show_count_obj = 20; - hand["joint5"] = 20; - hand["joint6"] = 0; - hand["joint11"] = 100; - hand["joint20"] = 250; - hand["joint25"] = 250; - break; - case 55: - show_step++; - show_count_obj = 40; - hand["joint1"] = 175; - hand["joint5"] = 175; - hand["joint6"] = 0; - hand["joint11"] = 0; - hand["joint16"] = 130; - hand["joint20"] = 100; - hand["joint21"] = 130; - hand["joint25"] = 80; - break; - case 56: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 0; - hand["joint11"] = 0; - hand["joint16"] = 250; - hand["joint20"] = 250; - hand["joint21"] = 250; - hand["joint25"] = 250; - break; - case 57: - show_step++; - show_count_obj = 35; - hand["joint1"] = 170; - hand["joint4"] = 170; - hand["joint6"] = 30; - hand["joint11"] = 50; - hand["joint16"] = 130; - hand["joint19"] = 80; - hand["joint21"] = 130; - hand["joint24"] = 80; - break; - case 58: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint4"] = 250; - hand["joint6"] = 30; - hand["joint11"] = 50; - hand["joint16"] = 250; - hand["joint19"] = 250; - hand["joint21"] = 250; - hand["joint24"] = 250; - break; - case 59: - show_step++; - show_count_obj = 35; - hand["joint1"] = 155; - hand["joint3"] = 155; - hand["joint6"] = 70; - hand["joint11"] = 60; - hand["joint16"] = 130; - hand["joint18"] = 90; - hand["joint21"] = 130; - hand["joint24"] = 80; - break; - case 60: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint3"] = 250; - hand["joint6"] = 100; - hand["joint11"] = 100; - hand["joint16"] = 250; - hand["joint18"] = 250; - hand["joint21"] = 250; - hand["joint24"] = 250; - break; - case 61: - show_step++; - show_count_obj = 35; - hand["joint1"] = 165; - hand["joint2"] = 165; - hand["joint6"] = 100; - hand["joint11"] = 70; - hand["joint16"] = 130; - hand["joint18"] = 80; - hand["joint21"] = 130; - hand["joint24"] = 80; - break; - default: - show_step = 0; - break; - } - } - - std::vector positions; - positions.clear(); - for (const auto& joint : joint_order) { // 按照固定的顺序提取关节值 - positions.push_back(static_cast(hand[joint])); - } - return positions; -} - -class HandController : public rclcpp::Node { -public: - HandController() : Node("dong_test_sender") { - publisher_ = this->create_publisher("/right_hand_control", 10); - timer_ = this->create_wall_timer(33ms, std::bind(&HandController::publishJointState, this)); - } - -private: - - void publishJointState() { - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = this->now(); - std::vector joint_names; - for (const auto& entry : hand) { - joint_names.push_back(entry.first); - } - joint_state.name = joint_names; - vector position = show_left(); - joint_state.position = position; - joint_state.velocity.resize(position.size(), 100.0); - joint_state.effort.resize(position.size(), 200.0); - publisher_->publish(joint_state); - } - - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -void signal_handler(int sig) { - (void) sig; - rclcpp::shutdown(); -} - -int main(int argc, char** argv) { - std::signal(SIGINT, signal_handler); - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/action_group_show_l7.cpp b/src/examples/src/action_group_show_l7.cpp deleted file mode 100644 index 19266ba..0000000 --- a/src/examples/src/action_group_show_l7.cpp +++ /dev/null @@ -1,396 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; -using namespace std::placeholders; - -int show_count = 0; -int show_count_obj = 0; -int show_step = 0; - -std::map hand = { - {"joint1", 255}, // 拇指根部弯曲 - {"joint2", 128}, // 拇指侧摆 - {"joint3", 255}, // 食指根部弯曲 - {"joint4", 255}, // 中指根部弯曲 - {"joint5", 255}, // 无名指根部弯曲 - {"joint6", 255}, // 小指根部弯曲 - {"joint7", 255} // 拇指旋转 -}; - -class HandController : public rclcpp::Node { -public: - HandController() : Node("dong_test_sender") { - publisher_ = this->create_publisher("/right_hand_control", 10); - timer_ = this->create_wall_timer(30ms, std::bind(&HandController::timer_callback, this)); - } - -private: - void timer_callback() { - auto message = sensor_msgs::msg::JointState(); - message.header.stamp = this->now(); - message.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7"}; - message.position = show_left(); - message.velocity.resize(message.position.size(), 100.0); - message.effort.resize(message.position.size(), 200.0); - publisher_->publish(message); - } - - std::vector show_left() { - show_count++; - if (show_count >= show_count_obj) { - show_count = 0; - switch (show_step) { - case 0: - show_step++; - show_count_obj = 100; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 250; - break; - case 1: - show_step++; - show_count_obj = 10; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint5"] = 0; - hand["joint6"] = 0; - hand["joint7"] = 250; - break; - case 2: - show_step += 8; - show_count_obj = 30; - hand["joint1"] = 40; - hand["joint2"] = 240; - hand["joint7"] = 80; - break; - case 3: - show_step++; - break; - case 4: - show_step++; - break; - case 5: - show_step++; - break; - case 6: - show_step++; - break; - case 7: - show_step++; - break; - case 8: - show_step++; - break; - case 9: - show_step++; - break; - case 10: - show_step++; - show_count_obj = 15; - hand["joint3"] = 100; - hand["joint4"] = 100; - break; - case 11: - show_step++; - show_count_obj = 15; - hand["joint3"] = 250; - hand["joint4"] = 250; - break; - case 12: - show_step++; - show_count_obj = 15; - hand["joint3"] = 100; - hand["joint4"] = 100; - break; - case 13: - show_step++; - show_count_obj = 15; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 250; - break; - case 14: - show_step++; - show_count_obj = 40; - hand["joint1"] = 40; - hand["joint2"] = 240; - hand["joint7"] = 120; - break; - case 15: - show_step++; - break; - case 16: - show_step++; - show_count_obj = 30; - hand["joint3"] = 10; - hand["joint4"] = 10; - hand["joint5"] = 10; - hand["joint6"] = 10; - break; - case 17: - show_step++; - show_count_obj = 15; - hand["joint6"] = 250; - break; - case 18: - show_step++; - show_count_obj = 15; - hand["joint5"] = 250; - break; - case 19: - show_step++; - show_count_obj = 15; - hand["joint4"] = 250; - break; - case 20: - show_step++; - show_count_obj = 15; - hand["joint3"] = 250; - break; - case 21: - show_step += 4; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint2"] = 110; - hand["joint7"] = 240; - break; - case 22: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint2"] = 10; - hand["joint7"] = 110; - break; - case 23: - show_step++; - show_count_obj = 40; - hand["joint1"] = 0; - hand["joint2"] = 10; - hand["joint7"] = 110; - break; - case 24: - show_step++; - show_count_obj = 30; - hand["joint1"] = 0; - hand["joint2"] = 240; - hand["joint7"] = 110; - break; - case 25: - show_step += 4; - show_count_obj = 50; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint7"] = 110; - break; - case 26: - show_step++; - break; - case 27: - show_step++; - break; - case 28: - show_step++; - break; - case 29: - show_step += 4; - show_count_obj = 15; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint7"] = 250; - break; - case 30: - show_step++; - break; - case 31: - show_step++; - break; - case 32: - show_step++; - break; - case 33: - show_step++; - show_count_obj = 15; - hand["joint3"] = 0; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint7"] = 250; - break; - case 34: - show_step++; - show_count_obj = 15; - hand["joint4"] = 0; - break; - case 35: - show_step++; - show_count_obj = 15; - hand["joint5"] = 0; - break; - case 36: - show_step++; - show_count_obj = 15; - hand["joint6"] = 0; - break; - case 37: - show_step++; - show_count_obj = 40; - hand["joint1"] = 0; - break; - case 38: - show_step++; - show_count_obj = 40; - hand["joint1"] = 250; - hand["joint2"] = 230; - hand["joint7"] = 250; - break; - case 39: - show_step++; - show_count_obj = 30; - hand["joint3"] = 250; - hand["joint6"] = 250; - break; - case 40: - show_step++; - show_count_obj = 40; - hand["joint1"] = 10; - hand["joint2"] = 40; - hand["joint7"] = 60; - break; - case 41: - show_step += 5; - show_count_obj = 5; - hand["joint1"] = 50; - break; - case 42: - show_step++; - break; - case 43: - show_step++; - break; - case 44: - show_step++; - break; - case 45: - show_step++; - break; - case 46: - show_step++; - show_count_obj = 50; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 250; - break; - case 47: - show_step++; - show_count_obj = 50; - hand["joint1"] = 120; - hand["joint2"] = 130; - hand["joint3"] = 155; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 90; - break; - case 48: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint3"] = 250; - break; - case 49: - show_step++; - show_count_obj = 35; - hand["joint1"] = 120; - hand["joint4"] = 140; - hand["joint7"] = 60; - break; - case 50: - show_step++; - show_count_obj = 30; - hand["joint1"] = 250; - hand["joint4"] = 250; - break; - case 51: - show_step++; - show_count_obj = 35; - hand["joint1"] = 120; - hand["joint2"] = 125; - hand["joint5"] = 145; - hand["joint7"] = 40; - break; - case 52: - show_step++; - show_count_obj = 30; - hand["joint1"] = 250; - hand["joint5"] = 250; - break; - case 53: - show_step++; - show_count_obj = 40; - hand["joint1"] = 120; - hand["joint6"] = 135; - hand["joint7"] = 15; - break; - case 54: - show_step++; - show_count_obj = 20; - hand["joint1"] = 250; - hand["joint2"] = 250; - hand["joint3"] = 250; - hand["joint4"] = 250; - hand["joint5"] = 250; - hand["joint6"] = 250; - hand["joint7"] = 250; - break; - default: - show_step = 0; - break; - } - } - - std::vector positions; - for (const auto& entry : hand) { - positions.push_back(static_cast(entry.second)); - } - return positions; - } - - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; -}; - -void signal_handler(int sig) { - (void) sig; - rclcpp::shutdown(); -} - -int main(int argc, char** argv) { - std::signal(SIGINT, signal_handler); - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/linker_hand_node.cpp b/src/examples/src/linker_hand_node.cpp deleted file mode 100644 index 3e61ba2..0000000 --- a/src/examples/src/linker_hand_node.cpp +++ /dev/null @@ -1,437 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -#include "LinkerHandApi.h" - -using namespace std::chrono_literals; - -static std::string can_channel; - -std::string bytesToHex(const std::vector &bytes) -{ - std::string hexStr; - for (uint8_t byte : bytes) - { - hexStr += std::to_string(byte) + " "; - } - return hexStr; -} - -std::string bytesToHex(const std::vector> &bytes) -{ - std::string hexStr; - for (const auto &vec : bytes) - { - for (uint8_t byte : vec) - { - hexStr += std::to_string(byte) + " "; - } - hexStr += "\n"; - } - return hexStr; -} - -class LinkerHand : public rclcpp::Node -{ -public: - LinkerHand() : Node("linker_hand_cpp_ros2") - { - - // 声明并获取话题参数 - this->declare_parameter("VERSION", "sdk-0.0"); - this->declare_parameter("HAND_EXISTS", false); - this->declare_parameter("HAND_TYPE", 0); - this->declare_parameter("HAND_TOUCH", false); - this->declare_parameter("HAND_JOINTS", "L7"); - this->declare_parameter("HAND_SPEED", 50); - this->declare_parameter("HAND_EFFORT", 200); - this->declare_parameter("CAN_CHANNEL", "can0"); - - this->declare_parameter("HAND_SETTING_TOPIC", "/cb_hand_setting_cmd"); - this->declare_parameter("HAND_CONTROL_TOPIC", "/cb_hand_control_cmd"); - this->declare_parameter("HAND_STATE_TOPIC", "/cb_hand_state"); - this->declare_parameter("HAND_TOUCH_TOPIC", "/cb_hand_touch"); - this->declare_parameter("HAND_INFO_TOPIC", "/cb_hand_info"); - // 获取话题参数 - this->get_parameter("HAND_SETTING_TOPIC", hand_setting_topic); - this->get_parameter("HAND_CONTROL_TOPIC", hand_control_topic); - this->get_parameter("HAND_STATE_TOPIC", hand_state_topic); - this->get_parameter("HAND_TOUCH_TOPIC", hand_touch_topic); - this->get_parameter("HAND_INFO_TOPIC", hand_info_topic); - - // LinkerHand - this->get_parameter("VERSION", version); - this->get_parameter("HAND_EXISTS", hand_exists); - this->get_parameter("HAND_TYPE", hand_type); - this->get_parameter("HAND_TOUCH", hand_touch); - this->get_parameter("HAND_JOINTS", hand_joints); - this->get_parameter("HAND_SPEED", hand_speed); - this->get_parameter("HAND_EFFORT", hand_effort); - this->get_parameter("CAN_CHANNEL", can_channel); - - // std::cout << "LinkerHand SDK version: " << version << std::endl; - std::cout << (hand_type == 0 ? "LEFT_HAND" : "RIGHT_HAND") << " HAND_EXISTS:" << hand_exists << " HAND_JOINTS:" << hand_joints << " HAND_TOUCH:" << hand_touch << std::endl; - - if (hand_exists != true) return; - - // LinkerHandType mapping - std::map linker_hand_map = { - {"L7", LINKER_HAND::L7}, - {"L10", LINKER_HAND::L10}, - {"L20", LINKER_HAND::L20}, - {"L21", LINKER_HAND::L21}, - {"L25", LINKER_HAND::L25} - }; - - COMM_TYPE channel; - - (can_channel == "can0") ? channel = COMM_TYPE::COMM_CAN_0 : channel = COMM_TYPE::COMM_CAN_1; - - if (hand_exists) { - auto it = linker_hand_map.find(hand_joints); - if (it != linker_hand_map.end()) { - if (hand_type == 0) { - hand_api = std::make_unique(it->second, HAND_TYPE::LEFT, channel); - } else if (hand_type == 1) { - hand_api = std::make_unique(it->second, HAND_TYPE::RIGHT, channel); - } else { - std::cout << "Invalid hand_type: " << hand_type << std::endl; - } - } else { - std::cout << "Invalid hand: " << hand_joints << std::endl; - } - } - - if (hand_exists) initHand(hand_api, hand_joints); - - auto hand_control_cmd_cb = [this](sensor_msgs::msg::JointState::SharedPtr msg) -> void - { - if (hand_exists) controlHand(hand_api, hand_joints, msg); - }; - - // auto hand_control_cmd_arc_cb = [this](sensor_msgs::msg::JointState::SharedPtr msg) -> void - // { - // if (hand_exists) controlHand(hand_api, hand_joints, msg, true); - // }; - - auto hand_setting_cb = [this](std_msgs::msg::String::SharedPtr msg) -> void - { - // std::cout << "hand_setting_cb: " << msg->data << std::endl; - - // bool setting_left = false, setting_right = false; - try { - nlohmann::json data = nlohmann::json::parse(msg->data); - RCLCPP_INFO(this->get_logger(), "command:%s", data["setting_cmd"].get().c_str()); - RCLCPP_INFO(this->get_logger(), "data:%s", data.dump().c_str()); - - // if (data["params"]["hand_type"] == "left" && hand_exists) { - // setting_left = true; - // } else if (data["params"]["hand_type"] == "right" && right_hand_exists) { - // setting_right = true; - // } else { - // RCLCPP_ERROR(this->get_logger(), "hand type invalid !"); - // return; - // } - - // clearFaultCode - if (data["setting_cmd"] == "clear_faults") { - hand_api->clearFaultCode(); - } - - // setCurrent - if (data["setting_cmd"] == "set_electric_current") { - int tmp = data["params"]["electric_current"].get(); - std::vector e_c(5, tmp); - hand_api->setCurrent(e_c); - } - - // enable - if (data["setting_cmd"] == "enable") { - hand_api->setEnable(); - } - - // disable - if (data["setting_cmd"] == "disable") { - hand_api->setDisable(); - } - } catch (const std::exception &e) { - RCLCPP_ERROR(this->get_logger(), "Command parameter error : %s", e.what()); - } - }; - - sub_settings = this->create_subscription(hand_setting_topic, 10, hand_setting_cb); - sub_hand_control = this->create_subscription(hand_control_topic, 10, hand_control_cmd_cb); - // sub_hand_control_arc = this->create_subscription("/cb_hand_control_cmd_arc", 10, hand_control_cmd_arc_cb); - - pub_hand_touch_ = this->create_publisher(hand_touch_topic, 10); - pub_hand_info_ = this->create_publisher(hand_info_topic, 10); - pub_hand_state_ = this->create_publisher(hand_state_topic, 10); - // pub_hand_state_arc_ = this->create_publisher("/cb_hand_state_arc", 10); - - - // auto start_time = std::chrono::high_resolution_clock::now(); - // auto end_time = std::chrono::high_resolution_clock::now(); - // auto duration = std::chrono::duration_cast>(end_time - start_time); - - // start_time = std::chrono::high_resolution_clock::now(); - - // end_time = std::chrono::high_resolution_clock::now(); - // duration = std::chrono::duration_cast>(end_time - start_time); - // std::cout << "delay time: " << duration.count() << " seconds" << std::endl; - - - if (hand_exists && hand_touch) { - pub_touch_thread = std::thread([this]() { - while (rclcpp::ok()) { - // rclcpp::spin_some(this); - if (pub_hand_touch_->get_subscription_count() > 0) { - publishTouchData(hand_api, *this->pub_hand_touch_); - } - std::this_thread::sleep_for(std::chrono::milliseconds(125)); - } - }); - } - - pub_state_thread = std::thread([this]() { - while (rclcpp::ok()) { - if (pub_hand_state_->get_subscription_count() > 0) { - publishJointState(hand_api, *this->pub_hand_state_); - // publishJointState(hand_api, *this->pub_hand_state_arc_, true); - } - std::this_thread::sleep_for(std::chrono::milliseconds(20)); - } - }); - - pub_info_thread = std::thread([this]() { - while (rclcpp::ok()) { - if (pub_hand_info_->get_subscription_count() > 0) { - publishLinkerHandInfo(hand_api, *this->pub_hand_info_); - } - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - } - }); - } - - ~LinkerHand() - { - // Ensure the release of resources in the destructor - hand_api.reset(); - - pub_touch_thread.join(); - pub_state_thread.join(); - pub_info_thread.join(); - } - - // General function: Publish touch sensor data - template - void publishTouchData(const HandType& hand, rclcpp::Publisher& publisher) - { - auto message = std_msgs::msg::Float32MultiArray(); - - const auto& touch = hand->getForce(); - - for (const auto& vec : touch) { - for (uint8_t byte : vec) { - message.data.push_back(static_cast(byte)); - } - } - publisher.publish(message); - } - - // General function: public joint state - template - void publishJointState(const HandType& hand, rclcpp::Publisher& publisher, const bool is_arc = false) - { - auto message = sensor_msgs::msg::JointState(); - - (is_arc) ? message.position = hand->getStateArc() : message.position = convert(hand->getState()); - message.velocity = convert(hand->getSpeed()); - message.effort = convert(hand->getTorque()); - - publisher.publish(message); - } - - // General function: publish linker hand info - template - void publishLinkerHandInfo(const HandType& hand, rclcpp::Publisher& publisher) - { - auto message = std_msgs::msg::String(); - message.data = hand->getVersion() + "Temperature: " + vectorToString(hand->getTemperature()) + "\nFaultCode: " + vectorToString(hand->getFaultCode()) + "\nCurrent: " + vectorToString(hand->getCurrent()); - publisher.publish(message); - - - } - - // General function: Convert vector - template - std::vector convert(const std::vector& vec) { - std::vector result; - for (const auto& value : vec) { - result.push_back(static_cast(value)); - } - return result; - } - - // General function: subscribe Hand Control Information - template - void controlHand(const HandType& hand, const std::string& hand_name, const sensor_msgs::msg::JointState::SharedPtr msg, const bool is_arc = false) - { - // for (auto &p : msg->position) - // { - // std::cout << p << " "; - // } - // std::cout << std::endl; - - // init hand speed and effort - std::vector speed(5, hand_speed); - std::vector effort(5, hand_effort); - - if (hand_name == "L7" && msg->position.size() == 7) - { - (msg->velocity.size() == 7) ? speed = convert(msg->velocity) : speed = std::vector(7, hand_speed); - (msg->effort.size() == 7) ? effort = convert(msg->effort) : effort = std::vector(7, hand_effort); - - hand->setSpeed(speed); // speed - hand->setTorque(effort); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } - else if (hand_name == "L10" && msg->position.size() == 10) - { - if (msg->velocity.size() == 5) speed = convert(msg->velocity); - - hand->setSpeed(speed); // speed - hand->setTorque(effort); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } - else if (hand_name == "L20" && msg->position.size() == 20) - { - if (msg->velocity.size() == 5) speed = convert(msg->velocity); - - hand->setSpeed(speed); // speed - // hand->setTorque({100, 100, 100, 100, 100}); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } - else if ((hand_name == "L25" || hand_name == "L21") && msg->position.size() == 25) - { - (msg->velocity.size() == 25) ? speed = convert(msg->velocity) : speed = std::vector(25, hand_speed); - (msg->effort.size() == 25) ? effort = convert(msg->effort) : effort = std::vector(25, hand_effort); - - hand->setSpeed(speed); // speed - hand->setTorque(effort); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } - else - { - std::cout << hand_name << " Invalid joint number: " << msg->position.size() << std::endl; - return; - } - } - - // std::vector to std::string - std::string vectorToString(const std::vector& vec) { - std::ostringstream oss; - for (size_t i = 0; i < vec.size(); ++i) { - if (i > 0) { - oss << " "; - } - oss << static_cast(vec[i]); - } - return oss.str(); - } - - template - void initHand(const HandType& hand, const std::string& hand_name) - { - if (hand_name == "L10") - { - hand->setSpeed(std::vector(5, hand_speed)); // speed - hand->setTorque(std::vector(5, hand_effort)); // torque - hand->fingerMove({255, 128, 255, 255, 255, 255, 128, 128, 128, 128}); // joint position - } - } - -private: - rclcpp::Subscription::SharedPtr sub_hand_control; - rclcpp::Subscription::SharedPtr sub_hand_control_arc; - rclcpp::Subscription::SharedPtr sub_settings; - // rclcpp::TimerBase::SharedPtr timer_; - // rclcpp::TimerBase::SharedPtr timer_state_; - // rclcpp::TimerBase::SharedPtr timer_info_; - // rclcpp::TimerBase::SharedPtr timer_touch_; - rclcpp::Publisher::SharedPtr pub_hand_info_; - rclcpp::Publisher::SharedPtr pub_hand_touch_; - rclcpp::Publisher::SharedPtr pub_hand_state_; - rclcpp::Publisher::SharedPtr pub_hand_state_arc_; - - std::unique_ptr hand_api; - - std::string version; - bool hand_exists; - bool hand_touch; - std::string hand_joints; - - int hand_speed; - int hand_effort; - - // 创建线程对象 - std::thread pub_touch_thread; - std::thread pub_state_thread; - std::thread pub_info_thread; - - std::string hand_setting_topic; - std::string hand_state_topic; - std::string hand_state_arc_topic; - std::string hand_info_topic; - std::string hand_touch_topic; - std::string hand_control_topic; - std::string hand_control_arc_topic; - - int hand_type; -}; - -// signal -void signalHandler(int signal) { - if (signal == SIGINT) { - // stop can channel - int result = system(std::string("sudo ip link set " + can_channel + " down").c_str()); - if (result == 0) { - std::cout << ((can_channel == "can0") ? "CAN0" : "CAN1") <<" interface stopped successfully." << std::endl; - } - - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ctrl+C detected. Shutting down..."); - rclcpp::shutdown(); - } -} - -int main(int argc, char *argv[]) -{ - rclcpp::init(argc, argv); - - // register signal - std::signal(SIGINT, signalHandler); - - auto node = std::make_shared(); - - rclcpp::spin(node); - return 0; -} diff --git a/src/examples/src/loop_l20.cpp b/src/examples/src/loop_l20.cpp deleted file mode 100644 index 8758e69..0000000 --- a/src/examples/src/loop_l20.cpp +++ /dev/null @@ -1,73 +0,0 @@ -// 本demo仅支持L20，默认为右手，如需左手，请修改hand_type - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace std::chrono_literals; - -bool running = true; -vector pos1 = {255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255}; -vector pos2 = {69.0, 0.0, 0.0, 0.0, 0.0, 151.0, 10.0, 100.0, 180.0, 240.0, 14.0, 255.0, 255.0, 255.0, 255.0, 109.0, 0.0, 0.0, 0.0, 0.0}; -vector joint_names = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10", "joint11", "joint12", "joint13", "joint14", "joint15", "joint16", "joint17", "joint18", "joint19", "joint20"}; - - -class JointStatePublisher : public rclcpp::Node { -public: - JointStatePublisher() : Node("dong_test_sender") { - string hand_type = "right"; // 控制右手 - string topic_name; - if (hand_type == "left") { - topic_name = "/left_hand_control"; - } else if (hand_type == "right") { - topic_name = "/right_hand_control"; - } - joint_state_pub = this->create_publisher(topic_name, 10); - timer = this->create_wall_timer(33ms, std::bind(&JointStatePublisher::publishJointState, this)); - } - -private: - void publishJointState() { - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = this->now(); - joint_state.name = joint_names; - joint_state.velocity.resize(joint_names.size(), 100.0); - joint_state.effort.resize(joint_names.size(), 200.0); - - static int msg_count = 0; - if (msg_count < 100) { - joint_state.position = pos1; - msg_count++; - } else if (msg_count < 200) { - joint_state.position = pos2; - msg_count++; - } else { - msg_count = 0; - count++; - RCLCPP_INFO(this->get_logger(), "Loop completed %d times", count); - } - if (joint_state.position.size() > 0) - joint_state_pub->publish(joint_state); - } - - rclcpp::Publisher::SharedPtr joint_state_pub; - rclcpp::TimerBase::SharedPtr timer; - int count = 0; -}; - -void signalHandler(int sig) { - (void) sig; - running = false; - rclcpp::shutdown(); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - signal(SIGINT, signalHandler); - rclcpp::spin(std::make_shared()); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/loop_l25.cpp b/src/examples/src/loop_l25.cpp deleted file mode 100644 index 65f5ba5..0000000 --- a/src/examples/src/loop_l25.cpp +++ /dev/null @@ -1,104 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; -using namespace std::placeholders; - -std::string hand_joint = "L25"; // 控制L25版本灵巧手 -std::string hand_type = "right"; // 控制左手 - -class HandController : public rclcpp::Node { -public: - HandController() : Node("dong_test_sender") { - if (hand_type == "left") { - publisher_ = this->create_publisher("/left_hand_control", 10); - } else if (hand_type == "right") { - publisher_ = this->create_publisher("/right_hand_control", 10); - } - - if (hand_joint == "L25") { - pos1_1 = {230, 0, 0, 15, 5, 250, 55, 0, 75, 95, 85, 0, 0, 0, 0, 250, 0, 40, 35, 5, 250, 0, 5, 0, 0}; - pos1_2 = {80, 255, 255, 255, 255, 180, 51, 51, 72, 202, 202, 255.0, 255.0, 255.0, 255.0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255}; - pos2_1 = {230, 0, 0, 15, 5, 250, 55, 0, 75, 95, 85, 0, 0, 0, 0, 80, 0, 40, 35, 5, 250, 0, 5, 0, 0}; - pos2_2 = {230, 0, 0, 15, 5, 42, 55, 0, 75, 95, 85, 0, 0, 0, 0, 90, 0, 40, 35, 5, 120, 0, 5, 0, 0}; - } - - joint_state.name = { - "joint1", "joint2", "joint3", "joint4", "joint5", "joint6", - "joint7", "joint8", "joint9", "joint10", "joint11", "joint12", - "joint13", "joint14", "joint15", "joint16", "joint17", "joint18", - "joint19", "joint20" - }; - - timer_ = this->create_wall_timer(33ms, std::bind(&HandController::timer_callback, this)); - } - -private: - void timer_callback() { - auto message = sensor_msgs::msg::JointState(); - message.header.stamp = this->now(); - - joint_state.position = pos2_1; - // joint_state.velocity.resize(joint_state.position.size(), 0.0); - // joint_state.effort.resize(joint_state.position.size(), 0.0); - message = joint_state; - publisher_->publish(message); - RCLCPP_INFO(this->get_logger(), "Published position 1"); - - std::this_thread::sleep_for(1.3s); - - joint_state.position = pos2_2; - // joint_state.velocity.resize(joint_state.position.size(), 0.0); - // joint_state.effort.resize(joint_state.position.size(), 0.0); - message = joint_state; - publisher_->publish(message); - RCLCPP_INFO(this->get_logger(), "Published position 2"); - - std::this_thread::sleep_for(5s); - - joint_state.position = pos1_1; - // joint_state.velocity.resize(joint_state.position.size(), 0.0); - // joint_state.effort.resize(joint_state.position.size(), 0.0); - message = joint_state; - publisher_->publish(message); - RCLCPP_INFO(this->get_logger(), "Published position 3"); - - std::this_thread::sleep_for(0.5s); - - joint_state.position = pos1_2; - // joint_state.velocity.resize(joint_state.position.size(), 0.0); - // joint_state.effort.resize(joint_state.position.size(), 0.0); - message = joint_state; - publisher_->publish(message); - RCLCPP_INFO(this->get_logger(), "Published position 4"); - - RCLCPP_INFO(this->get_logger(), "Cycle completed %d times", count); - count++; - std::this_thread::sleep_for(3s); - } - - rclcpp::Publisher::SharedPtr publisher_; - rclcpp::TimerBase::SharedPtr timer_; - sensor_msgs::msg::JointState joint_state; - std::vector pos1_1, pos1_2, pos2_1, pos2_2; - int count = 0; -}; - -void signal_handler(int sig) { - (void) sig; - rclcpp::shutdown(); -} - -int main(int argc, char** argv) { - std::signal(SIGINT, signal_handler); - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/show_ok_l20.cpp b/src/examples/src/show_ok_l20.cpp deleted file mode 100644 index 831d27f..0000000 --- a/src/examples/src/show_ok_l20.cpp +++ /dev/null @@ -1,105 +0,0 @@ -// 本demo仅支持L20，默认为右手，如需左手，请修改话题名 - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace std::chrono_literals; - -bool running = true; - -int show_count = 0; -int show_count_obj = 0; -int show_step = 0; - -vector show_left() { - vector position; - show_count++; - if (show_count >= show_count_obj) { - show_count = 0; - switch (show_step) { - case 0: - show_step++; - show_count_obj = 40; - position = {128, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 1: - show_step++; - show_count_obj = 40; - position = {190, 50, 250, 250, 250, 250, 200, 120, 168, 250, 140, 0, 0, 0, 0, 5, 5, 250, 250, 250}; - break; - case 2: - show_step++; - show_count_obj = 33; - position = {190, 50, 250, 250, 250, 250, 200, 120, 168, 250, 140, 0, 0, 0, 0, 5, 5, 5, 5, 5}; - break; - case 3: - show_step++; - show_count_obj = 30; - position = {190, 50, 250, 250, 250, 250, 200, 120, 168, 250, 140, 0, 0, 0, 0, 5, 5, 250, 250, 250}; - break; - case 4: - show_step++; - show_count_obj = 30; - position = {190, 50, 250, 250, 250, 250, 200, 120, 168, 250, 140, 0, 0, 0, 0, 5, 5, 5, 5, 5}; - break; - case 5: - show_step++; - show_count_obj = 15; - position = {190, 50, 250, 250, 250, 250, 200, 120, 168, 250, 140, 0, 0, 0, 0, 5, 5, 250, 250, 250}; - break; - case 6: - show_step++; - show_count_obj = 2; - position = {190, 50, 250, 250, 250, 250, 80, 120, 168, 250, 140, 0, 0, 0, 0, 5, 5, 250, 250, 250}; - break; - default: - show_step = 0; - position = {128, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - } - } - return position; -} - -class JointStatePublisher : public rclcpp::Node { -public: - JointStatePublisher() : Node("dong_test_sender") { - joint_state_pub = this->create_publisher("/right_hand_control", 10); - timer = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&JointStatePublisher::publishJointState, this)); - } - -private: - void publishJointState() { - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = this->now(); - joint_state.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10", "joint11", "joint12", "joint13", "joint14", "joint15", "joint16", "joint17", "joint18", "joint19", "joint20"}; - vector position = show_left(); - if (!position.empty()) { - joint_state.position = position; - } - joint_state.velocity.resize(joint_state.position.size(), 100.0); - joint_state.effort.resize(joint_state.position.size(), 200.0); - if (joint_state.position.size() > 0) joint_state_pub->publish(joint_state); - } - - rclcpp::Publisher::SharedPtr joint_state_pub; - rclcpp::TimerBase::SharedPtr timer; -}; - -void signalHandler(int sig) { - (void) sig; - running = false; - rclcpp::shutdown(); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - signal(SIGINT, signalHandler); - rclcpp::spin(std::make_shared()); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/show_surround_index_finger_l20.cpp b/src/examples/src/show_surround_index_finger_l20.cpp deleted file mode 100644 index cf016cb..0000000 --- a/src/examples/src/show_surround_index_finger_l20.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// 本demo仅支持L20，默认为右手，如需左手，请修改话题名 - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace std::chrono_literals; - -bool running = true; - -int show_count = 0; -int show_count_obj = 0; -int show_step = 0; - -vector show_left() { - vector position; - show_count++; - if (show_count >= show_count_obj) { - show_count = 0; - switch (show_step) { - case 0: - show_step++; - show_count_obj = 50; - position = {128, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 1: - show_step++; - show_count_obj = 40; - position = {190, 250, 5, 5, 5, 120, 128, 128, 128, 120, 180, 0, 0, 0, 0, 5, 250, 5, 5, 5}; - break; - case 2: - show_step++; - show_count_obj = 13; - position = {190, 210, 5, 5, 5, 120, 5, 128, 128, 120, 180, 0, 0, 0, 0, 5, 250, 5, 5, 5}; - break; - case 3: - show_step++; - show_count_obj = 13; - position = {190, 170, 5, 5, 5, 120, 128, 128, 128, 120, 180, 0, 0, 0, 0, 5, 250, 5, 5, 5}; - break; - case 4: - show_step++; - show_count_obj = 13; - position = {190, 210, 5, 5, 5, 120, 250, 128, 128, 120, 180, 0, 0, 0, 0, 5, 250, 5, 5, 5}; - break; - case 5: - show_step++; - show_count_obj = 13; - position = {190, 250, 5, 5, 5, 120, 128, 128, 128, 120, 180, 0, 0, 0, 0, 5, 250, 5, 5, 5}; - break; - default: - show_step = 1; - break; - } - } - return position; -} - -class JointStatePublisher : public rclcpp::Node { -public: - JointStatePublisher() : Node("dong_test_sender") { - joint_state_pub = this->create_publisher("/right_hand_control", 10); - timer = this->create_wall_timer(std::chrono::milliseconds(33), std::bind(&JointStatePublisher::publishJointState, this)); - } - -private: - void publishJointState() { - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = this->now(); - joint_state.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10", "joint11", "joint12", "joint13", "joint14", "joint15", "joint16", "joint17", "joint18", "joint19", "joint20"}; - joint_state.position = show_left(); - joint_state.velocity.resize(joint_state.position.size(), 100.0); - joint_state.effort.resize(joint_state.position.size(), 200.0); - if (joint_state.position.size() > 0) - joint_state_pub->publish(joint_state); - } - - rclcpp::Publisher::SharedPtr joint_state_pub; - rclcpp::TimerBase::SharedPtr timer; -}; - -void signalHandler(int sig) { - (void) sig; - running = false; - rclcpp::shutdown(); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - signal(SIGINT, signalHandler); - rclcpp::spin(std::make_shared()); - return 0; -} \ No newline at end of file diff --git a/src/examples/src/show_wave_l20.cpp b/src/examples/src/show_wave_l20.cpp deleted file mode 100644 index 1c29b5c..0000000 --- a/src/examples/src/show_wave_l20.cpp +++ /dev/null @@ -1,95 +0,0 @@ -// 本demo仅支持L20，默认为右手，如需左手，请修改话题名 - -#include -#include -#include -#include -#include -#include - -using namespace std; -using namespace std::chrono_literals; - -bool running = true; - -int show_count = 0; -int show_count_obj = 0; -int show_step = 0; - -vector show_left() { - vector position; - show_count++; - if (show_count >= show_count_obj) { - show_count = 0; - switch (show_step) { - case 0: - show_step++; - show_count_obj = 40; - position = {128, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 1: - show_step++; - show_count_obj = 30; - position = {128, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 2: - show_step++; - show_count_obj = 33; - position = {128, 0, 0, 0, 0, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 0, 0, 0, 0}; - break; - case 3: - show_step++; - show_count_obj = 30; - position = {128, 0, 0, 0, 0, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - case 4: - show_step++; - show_count_obj = 30; - position = {128, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - default: - show_step = 0; - position = {128, 250, 250, 250, 250, 250, 128, 128, 128, 128, 250, 0, 0, 0, 0, 250, 250, 250, 250, 250}; - break; - } - } - return position; -} - -class JointStatePublisher : public rclcpp::Node { -public: - JointStatePublisher() : Node("dong_test_sender") { - joint_state_pub = this->create_publisher("/right_hand_control", 10); - timer = this->create_wall_timer(33ms, std::bind(&JointStatePublisher::publishJointState, this)); - } - -private: - void publishJointState() { - sensor_msgs::msg::JointState joint_state; - joint_state.header.stamp = this->now(); - joint_state.name = {"joint1", "joint2", "joint3", "joint4", "joint5", "joint6", "joint7", "joint8", "joint9", "joint10", "joint11", "joint12", "joint13", "joint14", "joint15", "joint16", "joint17", "joint18", "joint19", "joint20"}; - vector position = show_left(); - if (!position.empty()) { - joint_state.position = position; - } - joint_state.velocity.resize(joint_state.position.size(), 100.0); - joint_state.effort.resize(joint_state.position.size(), 200.0); - if (joint_state.position.size() > 0) joint_state_pub->publish(joint_state); - } - - rclcpp::Publisher::SharedPtr joint_state_pub; - rclcpp::TimerBase::SharedPtr timer; -}; - -void signalHandler(int sig) { - (void) sig; - running = false; - rclcpp::shutdown(); -} - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - signal(SIGINT, signalHandler); - rclcpp::spin(std::make_shared()); - return 0; -} \ No newline at end of file diff --git a/src/linker_hand_cpp_ros2/CMakeLists.txt b/src/linker_hand_cpp_ros2/CMakeLists.txt index d815ad6..9a66cad 100644 --- a/src/linker_hand_cpp_ros2/CMakeLists.txt +++ b/src/linker_hand_cpp_ros2/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.10) project(linker_hand_cpp_ros2) # Default to C99 @@ -6,69 +6,24 @@ if(NOT CMAKE_C_STANDARD) set(CMAKE_C_STANDARD 99) endif() -# Default to C++14 +# Default to C++17 if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) + set(CMAKE_CXX_STANDARD 17) endif() +set(CMAKE_CXX_STANDARD_REQUIRED ON) +set(CMAKE_CXX_EXTENSIONS OFF) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() -if(CMAKE_SYSTEM_PROCESSOR MATCHES "x86_64") - set(LIB_SUBDIR "x86_64") -elseif(CMAKE_SYSTEM_PROCESSOR MATCHES "aarch64|arm64") - set(LIB_SUBDIR "aarch64") -else() - message(WARNING "Unknown architecture, defaulting to x86_64") - set(LIB_SUBDIR "x86_64") -endif() - # find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(sensor_msgs REQUIRED) - -#----------------------------------------------------------------------------- -# LINKER_HAND_CPP_SDK -#----------------------------------------------------------------------------- -find_library(LINKER_HAND_LIB - NAMES linkerhand_cpp_sdk linkerhand_cpp - PATHS ${CMAKE_CURRENT_SOURCE_DIR}/lib/${LIB_SUBDIR} - /usr/local/lib/linkerhand-cpp-sdk/${LIB_SUBDIR} - /usr/lib/linkerhand-cpp-sdk/${LIB_SUBDIR} - ${CMAKE_INSTALL_PREFIX}/lib/linkerhand-cpp-sdk/${LIB_SUBDIR} - NO_DEFAULT_PATH -) - -set(LINKER_HAND_INCLUDE_DIR - ${CMAKE_CURRENT_SOURCE_DIR}/include - /usr/local/include/linkerhand-cpp-sdk - /usr/include/linkerhand-cpp-sdk - ${CMAKE_INSTALL_PREFIX}/include/linkerhand-cpp-sdk -) - -if(NOT LINKER_HAND_LIB) - message(FATAL_ERROR "linkerhand_cpp_sdk library not found!") -endif() - -if(NOT LINKER_HAND_INCLUDE_DIR) - message(FATAL_ERROR "LinkerHand headers not found!") -endif() - -message(STATUS "Found linkerhand_cpp_sdk library: ${LINKER_HAND_LIB}") -message(STATUS "Found LinkerHand headers: ${LINKER_HAND_INCLUDE_DIR}") - -#----------------------------------------------------------------------------- -# INCLUDE_DIRECTORIES -#----------------------------------------------------------------------------- -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR}/include - ${LINKER_HAND_INCLUDE_DIR} -) - -# ---------------------------------------------------------------------- +find_package(nlohmann_json REQUIRED) +find_package(linkerhand-cpp-sdk 2.0 CONFIG REQUIRED) add_executable(linker_hand_node src/linker_hand_node.cpp) target_include_directories(linker_hand_node PUBLIC @@ -76,17 +31,25 @@ target_include_directories(linker_hand_node PUBLIC $) ament_target_dependencies(linker_hand_node rclcpp std_msgs sensor_msgs) -# 链接库 -target_link_libraries(linker_hand_node ${LINKER_HAND_LIB} pthread) +target_link_libraries(linker_hand_node LinkerHand::linkerhand_cpp_sdk nlohmann_json::nlohmann_json) install(TARGETS linker_hand_node DESTINATION lib/${PROJECT_NAME}) +install(PROGRAMS + scripts/hand_control_gui.py + DESTINATION lib/${PROJECT_NAME} +) + install (DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY assets + DESTINATION share/${PROJECT_NAME} +) + # install(DIRECTORY config # DESTINATION share/${PROJECT_NAME} # ) @@ -96,7 +59,8 @@ install (DIRECTORY launch # ) set_target_properties(linker_hand_node PROPERTIES - INSTALL_RPATH "/usr/local/linker_hand_cpp_sdk/third_party/Robotic_Arm/lib" + BUILD_RPATH "${linkerhand-cpp-sdk_LIBRARY_DIR}" + INSTALL_RPATH "${linkerhand-cpp-sdk_LIBRARY_DIR}" ) if(BUILD_TESTING) diff --git a/src/linker_hand_cpp_ros2/assets/hand_control_gui.png b/src/linker_hand_cpp_ros2/assets/hand_control_gui.png new file mode 100644 index 0000000..97c4085 Binary files /dev/null and b/src/linker_hand_cpp_ros2/assets/hand_control_gui.png differ diff --git a/src/linker_hand_cpp_ros2/assets/logo.png b/src/linker_hand_cpp_ros2/assets/logo.png new file mode 100644 index 0000000..7ef15bd Binary files /dev/null and b/src/linker_hand_cpp_ros2/assets/logo.png differ diff --git a/src/linker_hand_cpp_ros2/launch/run.xml b/src/linker_hand_cpp_ros2/launch/run.xml index dc38de6..db741a9 100644 --- a/src/linker_hand_cpp_ros2/launch/run.xml +++ b/src/linker_hand_cpp_ros2/launch/run.xml @@ -1,24 +1,24 @@ - + - + - - + + - + - - + + - - - + + + @@ -30,7 +30,8 @@ - + + @@ -50,7 +51,8 @@ - + + diff --git a/src/linker_hand_cpp_ros2/launch/run_left.xml b/src/linker_hand_cpp_ros2/launch/run_left.xml new file mode 100644 index 0000000..43f6a47 --- /dev/null +++ b/src/linker_hand_cpp_ros2/launch/run_left.xml @@ -0,0 +1,42 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/linker_hand_cpp_ros2/package.xml b/src/linker_hand_cpp_ros2/package.xml index 8361c57..aefadfe 100644 --- a/src/linker_hand_cpp_ros2/package.xml +++ b/src/linker_hand_cpp_ros2/package.xml @@ -11,6 +11,8 @@ ament_lint_auto ament_lint_common + nlohmann-json-dev + rclpy ament_cmake diff --git a/src/linker_hand_cpp_ros2/scripts/hand_control_gui.py b/src/linker_hand_cpp_ros2/scripts/hand_control_gui.py new file mode 100755 index 0000000..f2fa90b --- /dev/null +++ b/src/linker_hand_cpp_ros2/scripts/hand_control_gui.py @@ -0,0 +1,1084 @@ +#!/usr/bin/env python3 +import argparse +import os +import threading +import tkinter as tk +from tkinter import font as tkfont +from tkinter import ttk + +import rclpy +from rclpy.node import Node +from rclpy.parameter_client import AsyncParameterClient +from sensor_msgs.msg import JointState +from std_msgs.msg import Float32MultiArray +from std_msgs.msg import String + + +JOINT_COUNTS = { + "L6": 6, + "O6": 6, + "L7": 7, + "L10": 10, + "L20": 20, + "L21": 21, + "L25": 25, + "G20": 16, + "O20": 34, +} + +# Per-model position slider defaults (joint order matches README). Length should +# equal JOINT_COUNTS[model]; position_defaults() pads with 255 / truncates as a +# safety net. +POSITION_DEFAULTS = {model: [255] * count for model, count in JOINT_COUNTS.items()} +POSITION_DEFAULTS["G20"] = [255, 255, 255, 255, 255, 255, 130, 125, 125, + 125, 255, 255, 255, 255, 255, 255] + + +def position_defaults(model): + count = JOINT_COUNTS.get(model, 0) + values = POSITION_DEFAULTS.get(model, []) + return [values[i] if i < len(values) else 255 for i in range(count)] + + +FINGER_NAMES = ("拇指", "食指", "中指", "无名指", "小指") +STATE_LABELS = {"position": "位置", "speed": "速度", "torque": "力矩"} + + +class HandControlPublisher(Node): + def __init__(self, topic): + super().__init__("hand_control_gui") + self.topic = None + self.publisher = None + self.state_topic = None + self.touch_topic = None + self.info_topic = None + self.state_subscription = None + self.touch_subscription = None + self.info_subscription = None + self.latest_state = None + self.latest_touch = [] + self.latest_info = "" + self.data_lock = threading.Lock() + self.set_topic(topic) + + def set_topic(self, topic): + if topic == self.topic: + return + if self.publisher is not None: + self.destroy_publisher(self.publisher) + self.topic = topic + self.publisher = self.create_publisher(JointState, topic, 10) + + def set_feedback_topics(self, state_topic, touch_topic, info_topic): + if state_topic != self.state_topic: + if self.state_subscription is not None: + self.destroy_subscription(self.state_subscription) + self.state_topic = state_topic + self.state_subscription = self.create_subscription( + JointState, state_topic, self.state_callback, 10 + ) + if touch_topic != self.touch_topic: + if self.touch_subscription is not None: + self.destroy_subscription(self.touch_subscription) + self.touch_topic = touch_topic + self.touch_subscription = self.create_subscription( + Float32MultiArray, touch_topic, self.touch_callback, 10 + ) + if info_topic != self.info_topic: + if self.info_subscription is not None: + self.destroy_subscription(self.info_subscription) + self.info_topic = info_topic + self.info_subscription = self.create_subscription( + String, info_topic, self.info_callback, 10 + ) + + def state_callback(self, msg): + with self.data_lock: + self.latest_state = msg + + def touch_callback(self, msg): + with self.data_lock: + self.latest_touch = list(msg.data) + + def info_callback(self, msg): + with self.data_lock: + self.latest_info = msg.data + + def get_feedback(self): + with self.data_lock: + return self.latest_state, list(self.latest_touch), self.latest_info + + def publish_command(self, positions, speeds, torques): + msg = JointState() + msg.header.stamp = self.get_clock().now().to_msg() + msg.position = [float(v) for v in positions] + msg.velocity = [float(v) for v in speeds] + msg.effort = [float(v) for v in torques] + self.publisher.publish(msg) + + def get_remote_parameter(self, node_name, parameter_name, timeout_sec=1.0): + client = AsyncParameterClient(self, node_name) + wait_for_service = getattr(client, "wait_for_service", None) + if wait_for_service is None: + wait_for_service = client.wait_for_services + if not wait_for_service(timeout_sec=timeout_sec): + return None + future = client.get_parameters([parameter_name]) + rclpy.spin_until_future_complete(self, future, timeout_sec=timeout_sec) + if not future.done() or future.result() is None: + return None + values = future.result().values + if not values: + return None + return values[0].string_value + + +class HandControlGui: + BG = "#eaedf3" + CARD = "#ffffff" + TEXT = "#111827" + MUTED = "#4b5563" + BORDER = "#c4cad6" + TROUGH = "#d1d5db" + ACCENT = "#0a5fff" + ACCENT_HOVER = "#0846d1" + ACCENT_PRESS = "#0637a3" + GREEN = "#22c55e" + GRAY_DOT = "#9ca3af" + + def __init__(self, node, model, side, topic, live_publish, publish_rate_hz): + self.node = node + self.closed = False + + self.root = tk.Tk() + self.root.title("LinkerHand") + self.root.protocol("WM_DELETE_WINDOW", self.close) + self.root.minsize(1080, 720) + self.root.configure(background=self.BG) + + self.font_family = self._pick_font_family() + self.configure_style() + + self.model_var = tk.StringVar(value=model) + self.side_var = tk.StringVar(value=side) + self.topic_var = tk.StringVar(value=topic) + self.state_topic_var = tk.StringVar() + self.touch_topic_var = tk.StringVar() + self.info_topic_var = tk.StringVar() + self.live_var = tk.BooleanVar(value=live_publish) + self.rate_var = tk.IntVar(value=max(1, int(publish_rate_hz))) + self.joint_count = JOINT_COUNTS[model] + self._current_model = model + self.command_dirty = False + + self.middle_container = None + self.feedback_container = None + self.control_container = None + self.touch_canvases = [] + self.latest_touch_values = [] + + self.position_vars = [tk.IntVar(value=v) for v in position_defaults(model)] + self.speed_vars = [tk.IntVar(value=255) for _ in range(self.joint_count)] + self.torque_vars = [tk.IntVar(value=255) for _ in range(self.joint_count)] + self.watch_command_vars() + self.current_metric = "position" + self._metric_tab_buttons = {} + self._sliders_body = None + self._slider_traces = [] + + self.state_vars = {key: tk.StringVar(value="—") for key in STATE_LABELS} + self.info_vars = [] + + self.status_var = tk.StringVar(value="已就绪") + self.status_dot = None + self.live_dot = None + self.summary_var = tk.StringVar() + + self.update_feedback_topics() + self._build() + self.publish_loop() + self.refresh_feedback() + + # ---------- style ---------- + + def _pick_font_family(self): + try: + families = set(tkfont.families(self.root)) + except tk.TclError: + families = set() + for name in ( + "SF Pro Text", "SF Pro Display", ".AppleSystemUIFont", + "PingFang SC", "Helvetica Neue", "Segoe UI", + "Noto Sans CJK SC", "Ubuntu", + ): + if name in families: + return name + return tkfont.nametofont("TkDefaultFont").actual("family") + + def _load_logo(self): + # Resolve logo.png: prefer installed share dir, fall back to source tree + # (for `colcon build --symlink-install` or direct script execution). + path = None + try: + from ament_index_python.packages import get_package_share_directory + candidate = os.path.join( + get_package_share_directory("linker_hand_cpp_ros2"), + "assets", "logo.png", + ) + if os.path.exists(candidate): + path = candidate + except Exception: + pass + if path is None: + script_dir = os.path.dirname(os.path.abspath(__file__)) + for rel in ("../assets/logo.png", "../../assets/logo.png"): + cand = os.path.abspath(os.path.join(script_dir, rel)) + if os.path.exists(cand): + path = cand + break + if path is None: + return None + try: + img = tk.PhotoImage(file=path) + except tk.TclError: + return None + # Downscale so the ~181px-tall source fits inside the 58px toolbar. + target_h = 40 + h = img.height() or target_h + factor = max(1, round(h / target_h)) + return img.subsample(factor, factor) if factor > 1 else img + + def configure_style(self): + f = self.font_family + style = ttk.Style(self.root) + try: + style.theme_use("clam") + except tk.TclError: + pass + + style.configure("App.TFrame", background=self.BG) + style.configure("Card.TFrame", background=self.CARD) + + style.configure("TLabel", background=self.BG, foreground=self.TEXT, font=(f, 11)) + style.configure("Muted.TLabel", background=self.BG, foreground=self.MUTED, font=(f, 10)) + style.configure("Brand.TLabel", background=self.CARD, foreground=self.TEXT, + font=(f, 15, "bold")) + style.configure("ToolbarLabel.TLabel", background=self.CARD, foreground=self.MUTED, + font=(f, 11)) + style.configure("Section.TLabel", background=self.BG, foreground=self.MUTED, + font=(f, 10, "bold")) + + style.configure( + "Flat.TButton", + background=self.CARD, foreground=self.TEXT, + bordercolor=self.BORDER, lightcolor=self.BORDER, darkcolor=self.BORDER, + borderwidth=1, padding=(14, 6), font=(f, 11), relief="flat", focusthickness=0, + ) + style.map( + "Flat.TButton", + background=[("active", "#e5e7ec"), ("pressed", "#d5d8df")], + bordercolor=[("active", self.BORDER)], + ) + style.configure( + "Accent.TButton", + background=self.ACCENT, foreground="#ffffff", + borderwidth=0, padding=(16, 6), font=(f, 11, "bold"), relief="flat", + focusthickness=0, + ) + style.map( + "Accent.TButton", + background=[("active", self.ACCENT_HOVER), ("pressed", self.ACCENT_PRESS)], + ) + + for name in ("TEntry",): + style.configure( + name, + fieldbackground=self.CARD, foreground=self.TEXT, + bordercolor=self.BORDER, lightcolor=self.BORDER, darkcolor=self.BORDER, + insertcolor=self.TEXT, padding=6, + ) + style.map( + name, + foreground=[("!disabled", self.TEXT), ("disabled", self.MUTED)], + fieldbackground=[("!disabled", self.CARD)], + bordercolor=[("focus", self.ACCENT)], + lightcolor=[("focus", self.ACCENT)], + darkcolor=[("focus", self.ACCENT)], + ) + + style.configure( + "TCombobox", + fieldbackground=self.CARD, background=self.CARD, foreground=self.TEXT, + bordercolor=self.BORDER, lightcolor=self.BORDER, darkcolor=self.BORDER, + arrowcolor=self.TEXT, padding=5, + selectbackground=self.CARD, selectforeground=self.TEXT, + ) + style.map( + "TCombobox", + foreground=[("readonly", self.TEXT), ("!disabled", self.TEXT)], + fieldbackground=[("readonly", self.CARD), ("!disabled", self.CARD)], + selectbackground=[("readonly", self.CARD)], + selectforeground=[("readonly", self.TEXT)], + bordercolor=[("focus", self.ACCENT)], + lightcolor=[("focus", self.ACCENT)], + darkcolor=[("focus", self.ACCENT)], + ) + # Combobox dropdown listbox (a separate Tk widget outside ttk styling). + self.root.option_add("*TCombobox*Listbox.background", self.CARD) + self.root.option_add("*TCombobox*Listbox.foreground", self.TEXT) + self.root.option_add("*TCombobox*Listbox.selectBackground", self.ACCENT) + self.root.option_add("*TCombobox*Listbox.selectForeground", "#ffffff") + self.root.option_add("*TCombobox*Listbox.font", (f, 11)) + + style.configure( + "TSpinbox", + fieldbackground=self.CARD, background=self.CARD, foreground=self.TEXT, + bordercolor=self.BORDER, lightcolor=self.BORDER, darkcolor=self.BORDER, + arrowcolor=self.TEXT, padding=4, + selectbackground=self.ACCENT, selectforeground="#ffffff", + ) + style.map( + "TSpinbox", + foreground=[("!disabled", self.TEXT), ("disabled", self.MUTED)], + fieldbackground=[("!disabled", self.CARD)], + bordercolor=[("focus", self.ACCENT)], + lightcolor=[("focus", self.ACCENT)], + darkcolor=[("focus", self.ACCENT)], + ) + + # Scale: slider (thumb) uses `background`, track uses `troughcolor`. + # Setting slider to accent blue makes it clearly visible on the white card. + style.configure( + "Horizontal.TScale", + background=self.ACCENT, troughcolor=self.TROUGH, + bordercolor=self.ACCENT, lightcolor=self.ACCENT, darkcolor=self.ACCENT, + ) + style.map( + "Horizontal.TScale", + background=[("active", self.ACCENT_HOVER), ("pressed", self.ACCENT_PRESS)], + ) + + style.configure( + "Switch.TCheckbutton", + background=self.CARD, foreground=self.TEXT, + indicatorbackground=self.CARD, indicatorforeground=self.ACCENT, + font=(f, 11), focusthickness=0, padding=2, + ) + style.map( + "Switch.TCheckbutton", + background=[("active", self.CARD)], + ) + + style.configure( + "Vertical.TScrollbar", + background=self.BG, troughcolor=self.CARD, + bordercolor=self.CARD, arrowcolor=self.MUTED, gripcount=0, + ) + style.map("Vertical.TScrollbar", background=[("active", self.TROUGH)]) + + # ---------- build ---------- + + def _build(self): + self._build_toolbar() + + tk.Frame(self.root, background=self.BORDER, height=1).grid( + row=1, column=0, sticky="ew" + ) + + self.main = ttk.Frame(self.root, padding=(20, 14, 20, 14), style="App.TFrame") + self.main.grid(row=2, column=0, sticky="nsew") + self.root.columnconfigure(0, weight=1) + self.root.rowconfigure(2, weight=1) + self.main.columnconfigure(0, weight=1) + self.main.rowconfigure(0, weight=1) + + self.middle_container = ttk.Frame(self.main, style="App.TFrame") + self.middle_container.grid(row=0, column=0, sticky="nsew") + self.middle_container.columnconfigure(0, weight=2) + self.middle_container.columnconfigure(1, weight=1) + self.middle_container.rowconfigure(0, weight=1) + + self._build_feedback(self.middle_container) + self._build_control(self.middle_container) + self.update_summary() + + tk.Frame(self.root, background=self.BORDER, height=1).grid( + row=3, column=0, sticky="ew" + ) + self._build_footer() + + def _build_footer(self): + bar = tk.Frame(self.root, background=self.CARD, height=60) + bar.grid(row=4, column=0, sticky="ew") + bar.grid_propagate(False) + bar.columnconfigure(0, weight=1) + if self._logo_image is not None: + tk.Label( + bar, image=self._logo_image, background=self.CARD, + borderwidth=0, highlightthickness=0, + ).grid(row=0, column=0, pady=8) + else: + ttk.Label(bar, text="LinkerHand", style="Brand.TLabel").grid( + row=0, column=0, pady=14 + ) + + def _build_toolbar(self): + bar = tk.Frame(self.root, background=self.CARD, height=45) + bar.grid(row=0, column=0, sticky="ew") + bar.grid_propagate(False) + bar.rowconfigure(0, weight=1) + bar.columnconfigure(4, weight=1) + + self._logo_image = self._load_logo() + + ttk.Label(bar, text="型号", style="ToolbarLabel.TLabel").grid( + row=0, column=0, padx=(22, 6) + ) + model_box = ttk.Combobox( + bar, textvariable=self.model_var, values=sorted(JOINT_COUNTS), + state="readonly", width=6, + ) + model_box.grid(row=0, column=1, padx=(0, 22)) + model_box.bind( + "<>", + lambda _e, w=model_box: (self.apply_model(), self._clear_combo_selection(w)), + ) + + ttk.Label(bar, text="侧别", style="ToolbarLabel.TLabel").grid( + row=0, column=2, padx=(0, 6) + ) + side_box = ttk.Combobox( + bar, textvariable=self.side_var, values=("left", "right"), + state="readonly", width=6, + ) + side_box.grid(row=0, column=3, padx=(0, 22)) + side_box.bind( + "<>", + lambda _e, w=side_box: (self.apply_side_topic(), self._clear_combo_selection(w)), + ) + + self.live_dot = tk.Canvas( + bar, width=10, height=10, background=self.CARD, highlightthickness=0 + ) + self.live_dot.grid(row=0, column=5, padx=(0, 6)) + ttk.Checkbutton( + bar, text="实时", variable=self.live_var, style="Switch.TCheckbutton", + command=self._refresh_live_dot, + ).grid(row=0, column=6, padx=(0, 22)) + self._refresh_live_dot() + self.live_var.trace_add("write", lambda *_a: self._refresh_live_dot()) + + def _refresh_live_dot(self): + if self.live_dot is None: + return + color = self.GREEN if self.live_var.get() else self.GRAY_DOT + self.live_dot.delete("all") + self.live_dot.create_oval(1, 1, 9, 9, fill=color, outline="") + + def _clear_combo_selection(self, widget): + try: + widget.selection_clear() + except tk.TclError: + pass + self.root.focus_set() + + def _card(self, parent): + return tk.Frame( + parent, background=self.CARD, + highlightthickness=1, highlightbackground=self.BORDER, + ) + + def _build_feedback(self, parent): + if self.feedback_container is not None: + self.feedback_container.destroy() + + container = ttk.Frame(parent, style="App.TFrame") + container.grid(row=0, column=0, sticky="nsew", padx=(0, 10)) + container.columnconfigure(0, weight=1) + # Touch card (row 3) takes remaining vertical space; state card (row 1) + # sits on top with natural single-line height. + container.rowconfigure(3, weight=1) + self.feedback_container = container + + ttk.Label(container, text="反馈", style="Section.TLabel").grid( + row=0, column=0, sticky="w", pady=(0, 6) + ) + state_card = self._card(container) + state_card.grid(row=1, column=0, sticky="ew") + state_card.columnconfigure(0, minsize=76) + state_card.columnconfigure(1, weight=1) + + self._feedback_value_labels = [] + self._feedback_last_width = 0 + + row_idx = 0 + for key, var in self.state_vars.items(): + self._state_row(state_card, row_idx, STATE_LABELS[key], var) + row_idx += 1 + + self.info_vars = [] + for i in range(6): + var = tk.StringVar(value="—") + self.info_vars.append(var) + self._state_row(state_card, row_idx, f"信息 {i + 1}", var, + last=(i == 5)) + row_idx += 1 + + state_card.bind( + "", + lambda e: self._update_feedback_wraplength(e.width), + ) + + ttk.Label(container, text="触觉", style="Section.TLabel").grid( + row=2, column=0, sticky="w", pady=(14, 6) + ) + touch_card = self._card(container) + touch_card.grid(row=3, column=0, sticky="nsew") + touch_card.rowconfigure(0, weight=1) + + self.touch_canvases = [] + cw, ch = self.touch_canvas_size() + for finger in range(5): + touch_card.columnconfigure(finger, weight=1) + cell = tk.Frame(touch_card, background=self.CARD) + cell.grid(row=0, column=finger, sticky="nsew", padx=6, pady=(12, 4)) + cell.columnconfigure(0, weight=1) + cell.rowconfigure(0, weight=1) + canvas = tk.Canvas( + cell, width=cw, height=ch, + background=self.CARD, highlightthickness=0, + ) + canvas.grid(row=0, column=0, sticky="nsew") + canvas.bind("", lambda _e: self.redraw_touch_matrix()) + self.touch_canvases.append(canvas) + tk.Label( + touch_card, text=FINGER_NAMES[finger], + background=self.CARD, foreground=self.MUTED, + font=(self.font_family, 10), + ).grid(row=1, column=finger, pady=(0, 10)) + + def _update_feedback_wraplength(self, card_width): + # Debounce: only apply when width changes meaningfully. + if abs(card_width - self._feedback_last_width) < 4: + return + self._feedback_last_width = card_width + # 14px left + 76px label col + 14px gap + 14px right ≈ 118px chrome. + target = max(120, card_width - 118) + for lbl in self._feedback_value_labels: + try: + lbl.configure(wraplength=target) + except tk.TclError: + pass + + def _state_row(self, parent, row, label_text, var, last=False): + top = 10 if row == 0 else 4 + bottom = 12 if last else 4 + tk.Label( + parent, text=label_text, + background=self.CARD, foreground=self.MUTED, + font=(self.font_family, 10), + ).grid(row=row, column=0, padx=(14, 14), pady=(top, bottom), sticky="nw") + val_lbl = tk.Label( + parent, textvariable=var, + background=self.CARD, foreground=self.TEXT, + font=(self.font_family, 11), anchor="nw", justify="left", + # width=1 lets the column's own weight decide horizontal size, so + # long text never expands the card. wraplength is refreshed by + # _update_feedback_wraplength() so overflow wraps to a new line + # inside the value column instead of being clipped. + width=1, wraplength=320, + ) + val_lbl.grid(row=row, column=1, padx=(0, 14), pady=(top, bottom), sticky="ew") + self._feedback_value_labels.append(val_lbl) + + def _build_control(self, parent): + if self.control_container is not None: + self.control_container.destroy() + + container = ttk.Frame(parent, style="App.TFrame") + container.grid(row=0, column=1, sticky="nsew", padx=(10, 0)) + container.columnconfigure(0, weight=1) + container.rowconfigure(1, weight=1) + self.control_container = container + + ttk.Label(container, text="控制", style="Section.TLabel").grid( + row=0, column=0, sticky="w", pady=(0, 6) + ) + card = self._card(container) + card.grid(row=1, column=0, sticky="nsew") + card.columnconfigure(0, weight=1) + card.rowconfigure(1, weight=1) + + # Segmented tabs: 位置 / 速度 / 力矩 + seg = tk.Frame(card, background=self.TROUGH, highlightthickness=0) + seg.grid(row=0, column=0, columnspan=2, sticky="ew", padx=14, pady=(14, 10)) + self._metric_tab_buttons = {} + for i, (key, label) in enumerate( + (("position", "位置"), ("speed", "速度"), ("torque", "力矩")) + ): + seg.columnconfigure(i, weight=1) + btn = tk.Label( + seg, text=label, + background=self.TROUGH, foreground=self.TEXT, + font=(self.font_family, 11), padx=12, pady=6, cursor="hand2", + ) + btn.grid(row=0, column=i, sticky="ew", padx=2, pady=2) + btn.bind("", lambda _e, k=key: self._set_metric(k)) + self._metric_tab_buttons[key] = btn + + # Scrollable body host — three metric pages are stacked at the same cell + # and switched with tkraise() so there is no destroy/rebuild flicker. + canvas = tk.Canvas(card, background=self.CARD, highlightthickness=0) + scrollbar = ttk.Scrollbar( + card, orient="vertical", command=canvas.yview, + style="Vertical.TScrollbar", + ) + body = tk.Frame(canvas, background=self.CARD) + window_id = canvas.create_window((0, 0), window=body, anchor="nw") + + body.bind( + "", + lambda _e: canvas.configure(scrollregion=canvas.bbox("all")), + ) + canvas.bind( + "", + lambda e: canvas.itemconfigure(window_id, width=e.width), + ) + canvas.configure(yscrollcommand=scrollbar.set) + canvas.grid(row=1, column=0, sticky="nsew", padx=(14, 0), pady=(0, 10)) + scrollbar.grid(row=1, column=1, sticky="ns", padx=(0, 6), pady=(0, 10)) + self._bind_mousewheel(canvas) + + footer = tk.Frame(card, background=self.CARD) + footer.grid(row=2, column=0, columnspan=2, sticky="ew", + padx=10, pady=(0, 6)) + footer.columnconfigure(0, weight=1) + link = tk.Label( + footer, text="复制位置", + background=self.CARD, foreground=self.MUTED, + font=(self.font_family, 10), cursor="hand2", padx=2, pady=2, + ) + link.grid(row=0, column=1, sticky="e") + link.bind("", lambda _e: self._copy_positions()) + link.bind("", lambda _e: link.configure(foreground=self.ACCENT)) + link.bind("", lambda _e: link.configure(foreground=self.MUTED)) + + body.columnconfigure(0, weight=1) + body.rowconfigure(0, weight=1) + self._sliders_body = body + self._slider_traces = [] + self._metric_frames = {} + self._metric_vars = { + "position": self.position_vars, + "speed": self.speed_vars, + "torque": self.torque_vars, + } + + self._ensure_metric_page(self.current_metric) + self._refresh_metric_tabs() + self._metric_frames[self.current_metric].tkraise() + + def _bind_mousewheel(self, canvas): + def _on_wheel(event): + if event.num == 4 or getattr(event, "delta", 0) > 0: + canvas.yview_scroll(-3, "units") + elif event.num == 5 or getattr(event, "delta", 0) < 0: + canvas.yview_scroll(3, "units") + + def _bind(_e): + canvas.bind_all("", _on_wheel) + canvas.bind_all("", _on_wheel) + canvas.bind_all("", _on_wheel) + + def _unbind(_e): + canvas.unbind_all("") + canvas.unbind_all("") + canvas.unbind_all("") + + canvas.bind("", _bind) + canvas.bind("", _unbind) + + def _ensure_metric_page(self, key): + if key in self._metric_frames: + return + vars_list = self._metric_vars[key] + page = tk.Frame(self._sliders_body, background=self.CARD) + page.grid(row=0, column=0, sticky="nsew") + page.columnconfigure(0, weight=0, minsize=44) + page.columnconfigure(1, weight=1) + for i, var in enumerate(vars_list): + tk.Label( + page, text=f"J{i + 1}", + background=self.CARD, foreground=self.MUTED, + font=(self.font_family, 11), + ).grid(row=i, column=0, sticky="w", padx=(0, 12), pady=8) + self._add_slider(page, i, 1, var) + self._metric_frames[key] = page + + def _set_metric(self, key): + if key == self.current_metric: + return + self.current_metric = key + self._ensure_metric_page(key) + self._refresh_metric_tabs() + page = self._metric_frames.get(key) + if page is not None: + page.tkraise() + + def _refresh_metric_tabs(self): + for k, btn in self._metric_tab_buttons.items(): + if k == self.current_metric: + btn.configure(background=self.CARD, foreground=self.TEXT, + font=(self.font_family, 11, "bold")) + else: + btn.configure(background=self.TROUGH, foreground=self.MUTED, + font=(self.font_family, 11)) + + def _add_slider(self, parent, row, column, variable): + frame = tk.Frame(parent, background=self.CARD) + frame.grid(row=row, column=column, padx=(0, 8), pady=4, sticky="ew") + frame.columnconfigure(0, weight=1) + validate_int = (self.root.register(self.validate_byte_text), "%P") + + slider_h = 24 + thumb_r = 7 + pad = thumb_r + 2 + canvas = tk.Canvas( + frame, height=slider_h, background=self.CARD, + highlightthickness=0, borderwidth=0, + ) + canvas.grid(row=0, column=0, sticky="ew") + + y = slider_h // 2 + track_id = canvas.create_line(0, y, 0, y, fill=self.TROUGH, + width=2, capstyle="round") + fill_id = canvas.create_line(0, y, 0, y, fill=self.ACCENT, + width=2, capstyle="round") + thumb_id = canvas.create_oval(0, 0, 0, 0, fill=self.ACCENT, outline="") + + def _redraw(): + w = max(1, canvas.winfo_width()) + try: + v = variable.get() + except tk.TclError: + v = 0 + frac = max(0.0, min(1.0, v / 255.0)) + x0, x1 = pad, w - pad + if x1 <= x0: + x1 = x0 + 1 + cx = x0 + frac * (x1 - x0) + canvas.coords(track_id, x0, y, x1, y) + canvas.coords(fill_id, x0, y, cx, y) + canvas.coords(thumb_id, cx - thumb_r, y - thumb_r, + cx + thumb_r, y + thumb_r) + + def _jump(event): + w = max(1, canvas.winfo_width()) + x0, x1 = pad, w - pad + span = max(1, x1 - x0) + frac = max(0.0, min(1.0, (event.x - x0) / span)) + self.set_int_value(variable, frac * 255) + return "break" + + canvas.bind("", lambda _e: _redraw()) + canvas.bind("", _jump) + canvas.bind("", _jump) + + trace_id = variable.trace_add("write", lambda *_a: _redraw()) + self._slider_traces.append((variable, trace_id)) + + ttk.Spinbox( + frame, from_=0, to=255, width=4, textvariable=variable, + validate="key", validatecommand=validate_int, + ).grid(row=0, column=1, padx=(8, 0)) + + # ---------- helpers ---------- + + def set_int_value(self, variable, value): + variable.set(max(0, min(255, int(round(float(value)))))) + + def _copy_positions(self): + text = ", ".join(str(var.get()) for var in self.position_vars) + self.root.clipboard_clear() + self.root.clipboard_append(text) + self.status_var.set(f"已复制位置到剪贴板 ({len(self.position_vars)} 关节)") + + def validate_byte_text(self, text): + if text == "": + return True + if not text.isdigit(): + return False + return 0 <= int(text) <= 255 + + def apply_side_topic(self): + self.topic_var.set(f"/{self.side_var.get()}_hand_control") + self.update_feedback_topics() + self.apply_topic() + + def apply_topic(self): + topic = self.topic_var.get().strip() + if not topic.startswith("/"): + topic = f"/{topic}" + self.topic_var.set(topic) + self.node.set_topic(topic) + self.update_summary() + + def apply_model(self): + model = self.model_var.get() + if model == getattr(self, "_current_model", None): + return + self._current_model = model + old_speeds = [var.get() for var in self.speed_vars] + old_torques = [var.get() for var in self.torque_vars] + old_touch_shape = self.touch_shape() + old_canvas_size = self.touch_canvas_size() + self.joint_count = JOINT_COUNTS[model] + self.position_vars = [tk.IntVar(value=v) for v in position_defaults(model)] + self.speed_vars = self.resize_vars(old_speeds, 255) + self.torque_vars = self.resize_vars(old_torques, 255) + self.watch_command_vars() + if (self.touch_shape() != old_touch_shape + or self.touch_canvas_size() != old_canvas_size): + self._build_feedback(self.middle_container) + else: + self.redraw_touch_matrix() + self._build_control(self.middle_container) + self.update_summary() + self.command_dirty = False + + def resize_vars(self, values, default): + resized = [] + for index in range(self.joint_count): + value = values[index] if index < len(values) else default + resized.append(tk.IntVar(value=value)) + return resized + + def watch_command_vars(self): + for variable in self.position_vars + self.speed_vars + self.torque_vars: + variable.trace_add("write", self.mark_command_dirty) + + def mark_command_dirty(self, *_args): + self.command_dirty = True + + def update_summary(self): + self.summary_var.set( + f"{self.side_var.get()} · {self.model_var.get()} · " + f"{self.joint_count} 关节 · 状态 {self.state_topic_var.get()} · " + f"触觉 {self.touch_topic_var.get()}" + ) + + def update_feedback_topics(self): + side = self.side_var.get() + self.state_topic_var.set(f"/{side}_hand_state") + self.touch_topic_var.set(f"/{side}_hand_touch") + self.info_topic_var.set(f"/{side}_hand_info") + self.node.set_feedback_topics( + self.state_topic_var.get(), + self.touch_topic_var.get(), + self.info_topic_var.get(), + ) + + def publish(self): + self.command_dirty = False + positions = [var.get() for var in self.position_vars] + speeds = [var.get() for var in self.speed_vars] + torques = [var.get() for var in self.torque_vars] + self.node.publish_command(positions, speeds, torques) + self.status_var.set( + f"已发送 {len(positions)} 关节 → {self.node.topic}" + ) + + def publish_loop(self): + if self.closed: + return + if self.live_var.get() and self.command_dirty: + self.publish() + period_ms = max(1, round(1000 / max(1, self.rate_var.get()))) + self.root.after(period_ms, self.publish_loop) + + # ---------- feedback rendering ---------- + + def refresh_feedback(self): + if self.closed: + return + state, touch, info = self.node.get_feedback() + if state is not None: + self.state_vars["position"].set(self.format_values(state.position)) + self.state_vars["speed"].set(self.format_values(state.velocity)) + self.state_vars["torque"].set(self.format_values(state.effort)) + if info: + self.update_info_lines(info) + if touch: + self.update_touch_matrix(touch) + self.root.after(100, self.refresh_feedback) + + def update_info_lines(self, info): + lines = [part.strip() for part in info.splitlines() if part.strip()] + for index, var in enumerate(self.info_vars): + var.set(lines[index] if index < len(lines) else "—") + + def format_values(self, values): + return " ".join(str(int(value)) for value in values[: self.joint_count]) + + def update_touch_matrix(self, values): + self.latest_touch_values = list(values) + rows, cols = self.touch_shape() + cells_per_finger = rows * cols + for finger in range(5): + matrix = [] + for row in range(rows): + row_values = [] + for col in range(cols): + index = finger * cells_per_finger + row * cols + col + row_values.append(int(values[index]) if index < len(values) else 0) + matrix.append(row_values) + self.draw_heatmap(self.touch_canvases[finger], matrix) + + def redraw_touch_matrix(self): + if self.latest_touch_values: + self.update_touch_matrix(self.latest_touch_values) + + def touch_shape(self): + if self.model_var.get() == "O6": + return 10, 4 + return 12, 6 + + def display_shape(self): + return self.touch_shape() + + def touch_canvas_size(self): + if self.model_var.get() == "O6": + return 60, 130 + return 78, 156 + + def draw_heatmap(self, canvas, matrix): + display_rows, display_cols = self.display_shape() + width = max(1, canvas.winfo_width()) + height = max(1, canvas.winfo_height()) + padding = 4 + usable_w = max(1, width - padding * 2) + usable_h = max(1, height - padding * 2) + target_ratio = display_cols / display_rows + usable_ratio = usable_w / usable_h + if usable_ratio > target_ratio: + draw_h = usable_h + draw_w = draw_h * target_ratio + else: + draw_w = usable_w + draw_h = draw_w / target_ratio + x_off = (width - draw_w) / 2 + y_off = (height - draw_h) / 2 + cell_w = draw_w / display_cols + cell_h = draw_h / display_rows + canvas.delete("all") + for row in range(display_rows): + for col in range(display_cols): + value = self.display_value(matrix, row, col) + x1 = x_off + col * cell_w + y1 = y_off + row * cell_h + canvas.create_rectangle( + x1, y1, x1 + cell_w, y1 + cell_h, + fill=self.heat_color(value), + outline="#dfe3ea", width=1, + ) + + def display_value(self, matrix, display_row, display_col): + if display_row < len(matrix) and display_col < len(matrix[display_row]): + return matrix[display_row][display_col] + return 0 + + # Heatmap gradient: near-neutral idle, then cool→warm spectrum + # (cyan → green → amber → red) so each pressure level has a distinct hue. + _HEAT_STOPS = ( + (0.00, (0xee, 0xf2, 0xf7)), + (0.25, (0x06, 0xb6, 0xd4)), + (0.50, (0x22, 0xc5, 0x5e)), + (0.75, (0xf5, 0x9e, 0x0b)), + (1.00, (0xdc, 0x26, 0x26)), + ) + + def heat_color(self, value): + value = max(0, min(255, int(value))) + ratio = value / 255.0 + stops = self._HEAT_STOPS + for i in range(len(stops) - 1): + p0, c0 = stops[i] + p1, c1 = stops[i + 1] + if ratio <= p1: + t = (ratio - p0) / (p1 - p0) if p1 > p0 else 0.0 + r = int(c0[0] + (c1[0] - c0[0]) * t) + g = int(c0[1] + (c1[1] - c0[1]) * t) + b = int(c0[2] + (c1[2] - c0[2]) * t) + return f"#{r:02x}{g:02x}{b:02x}" + r, g, b = stops[-1][1] + return f"#{r:02x}{g:02x}{b:02x}" + + # ---------- lifecycle ---------- + + def run(self): + self.root.mainloop() + + def close(self): + if self.closed: + return + self.closed = True + self.root.quit() + self.root.destroy() + + +def parse_args(): + parser = argparse.ArgumentParser() + parser.add_argument("--side", default="left", choices=("left", "right")) + parser.add_argument("--topic", default=None) + parser.add_argument( + "--model", + default=None, + choices=sorted(JOINT_COUNTS), + help="hand model; auto-read HAND_JOINTS from the running hand node when omitted", + ) + parser.add_argument( + "--no-live", + action="store_true", + help="disable auto-publish; useful for silent debugging", + ) + parser.add_argument("--rate", type=int, default=60, help="live publish rate in Hz") + return parser.parse_args() + + +def main(): + args = parse_args() + if args.rate <= 0: + raise SystemExit("--rate must be greater than 0") + topic = args.topic or f"/{args.side}_hand_control" + + rclpy.init() + node = HandControlPublisher(topic) + model = args.model or read_hand_model(node, args.side) or "O6" + + gui = HandControlGui( + node=node, + model=model, + side=args.side, + topic=topic, + live_publish=not args.no_live, + publish_rate_hz=args.rate, + ) + spin_thread = threading.Thread(target=rclpy.spin, args=(node,), daemon=True) + spin_thread.start() + try: + gui.run() + finally: + node.destroy_node() + rclpy.shutdown() + + +def read_hand_model(node, side): + node_name = f"/linker_hand_{side}_node" + model = node.get_remote_parameter(node_name, "HAND_JOINTS") + if model in JOINT_COUNTS: + return model + return None + + +if __name__ == "__main__": + main() diff --git a/src/linker_hand_cpp_ros2/src/linker_hand_node.cpp b/src/linker_hand_cpp_ros2/src/linker_hand_node.cpp index f7485cb..ac27124 100644 --- a/src/linker_hand_cpp_ros2/src/linker_hand_node.cpp +++ b/src/linker_hand_cpp_ros2/src/linker_hand_node.cpp @@ -13,7 +13,11 @@ #include #include #include +#include +#include +#include #include +#include #include #include @@ -23,6 +27,8 @@ #include #include "LinkerHandApi.h" +#include "CommFactory.h" +#include "Modbus.h" using namespace std::chrono_literals; @@ -64,7 +70,9 @@ class LinkerHand : public rclcpp::Node this->declare_parameter("HAND_JOINTS", "L7"); this->declare_parameter("HAND_SPEED", 50); this->declare_parameter("HAND_EFFORT", 200); + this->declare_parameter("COMM_TYPE", "CAN"); this->declare_parameter("CAN_CHANNEL", "can0"); + this->declare_parameter("CAN_BITRATE", 1000000); this->declare_parameter("HAND_SETTING_TOPIC", "/cb_hand_setting_cmd"); this->declare_parameter("HAND_CONTROL_TOPIC", "/cb_hand_control_cmd"); @@ -86,43 +94,61 @@ class LinkerHand : public rclcpp::Node this->get_parameter("HAND_JOINTS", hand_joints); this->get_parameter("HAND_SPEED", hand_speed); this->get_parameter("HAND_EFFORT", hand_effort); + this->get_parameter("COMM_TYPE", comm_type); this->get_parameter("CAN_CHANNEL", can_channel); + this->get_parameter("CAN_BITRATE", can_bitrate); // std::cout << "LinkerHand SDK version: " << version << std::endl; + normalizeCommType(); + std::cout << (hand_type == 0 ? "LEFT_HAND" : "RIGHT_HAND") << " HAND_EXISTS:" << hand_exists << " HAND_JOINTS:" << - hand_joints << " HAND_TOUCH:" << hand_touch << std::endl; + hand_joints << " HAND_TOUCH:" << hand_touch << " COMM_TYPE:" << comm_type << std::endl; if (hand_exists != true) {return;} // LinkerHandType mapping std::map linker_hand_map = { + {"L6", LINKER_HAND::L6}, {"L7", LINKER_HAND::L7}, {"L10", LINKER_HAND::L10}, {"L20", LINKER_HAND::L20}, {"L21", LINKER_HAND::L21}, - {"L25", LINKER_HAND::L25} + {"L25", LINKER_HAND::L25}, + {"O6", LINKER_HAND::O6}, + {"O20", LINKER_HAND::O20}, + {"G20", LINKER_HAND::G20} }; - COMM_TYPE channel; - - (can_channel == "can0") ? channel = COMM_TYPE::COMM_CAN_0 : channel = COMM_TYPE::COMM_CAN_1; - if (hand_exists) { auto it = linker_hand_map.find(hand_joints); if (it != linker_hand_map.end()) { + hand_model = it->second; + if (hand_model == LINKER_HAND::O20) { + RCLCPP_ERROR(this->get_logger(), + "O20 requires CAN-FD and is not supported by this ROS2 node yet"); + return; + } + if (hand_type == 0) { - hand_api = std::make_unique(it->second, HAND_TYPE::LEFT, channel); + side = HAND_TYPE::LEFT; } else if (hand_type == 1) { - hand_api = std::make_unique(it->second, HAND_TYPE::RIGHT, channel); + side = HAND_TYPE::RIGHT; } else { std::cout << "Invalid hand_type: " << hand_type << std::endl; + return; } } else { std::cout << "Invalid hand: " << hand_joints << std::endl; + return; } } + if (!initTransport()) { + return; + } + node_active = true; + if (hand_exists) {initHand(hand_api, hand_joints);} auto hand_control_cmd_cb = [this](sensor_msgs::msg::JointState::SharedPtr msg) -> void @@ -160,11 +186,9 @@ class LinkerHand : public rclcpp::Node hand_api->clearFaultCode(); } - // setCurrent if (data["setting_cmd"] == "set_electric_current") { - int tmp = data["params"]["electric_current"].get(); - std::vector e_c(5, tmp); - hand_api->setCurrent(e_c); + RCLCPP_WARN(this->get_logger(), + "set_electric_current is not supported by linkerhand-cpp-sdk v2.1.7"); } // enable @@ -207,32 +231,44 @@ class LinkerHand : public rclcpp::Node if (hand_exists && hand_touch) { pub_touch_thread = std::thread([this]() { - while (rclcpp::ok()) { + while (rclcpp::ok() && node_active) { // rclcpp::spin_some(this); - if (pub_hand_touch_->get_subscription_count() > 0) { - publishTouchData(hand_api, *this->pub_hand_touch_); + try { + if (pub_hand_touch_->get_subscription_count() > 0) { + publishTouchData(hand_api, *this->pub_hand_touch_); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "touch publish error: %s", e.what()); } - std::this_thread::sleep_for(std::chrono::milliseconds(125)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); } pub_state_thread = std::thread([this]() { - while (rclcpp::ok()) { - if (pub_hand_state_->get_subscription_count() > 0) { - publishJointState(hand_api, *this->pub_hand_state_); - // publishJointState(hand_api, *this->pub_hand_state_arc_, true); + while (rclcpp::ok() && node_active) { + try { + if (pub_hand_state_->get_subscription_count() > 0) { + publishJointState(hand_api, *this->pub_hand_state_); + // publishJointState(hand_api, *this->pub_hand_state_arc_, true); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "state publish error: %s", e.what()); } - std::this_thread::sleep_for(std::chrono::milliseconds(20)); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); } }); pub_info_thread = std::thread([this]() { - while (rclcpp::ok()) { - if (pub_hand_info_->get_subscription_count() > 0) { - publishLinkerHandInfo(hand_api, *this->pub_hand_info_); + while (rclcpp::ok() && node_active) { + try { + if (pub_hand_info_->get_subscription_count() > 0) { + publishLinkerHandInfo(hand_api, *this->pub_hand_info_); + } + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "info publish error: %s", e.what()); } - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } }); } @@ -240,11 +276,150 @@ class LinkerHand : public rclcpp::Node ~LinkerHand() { // Ensure the release of resources in the destructor + node_active = false; + + // Stop publisher threads before releasing hand_api, so no thread + // dereferences a freed hand_api during shutdown. + if (pub_touch_thread.joinable()) {pub_touch_thread.join();} + if (pub_state_thread.joinable()) {pub_state_thread.join();} + if (pub_info_thread.joinable()) {pub_info_thread.join();} + hand_api.reset(); + } + + bool isActive() const + { + return node_active; + } + + void normalizeCommType() + { + for (auto & ch : comm_type) { + ch = static_cast(std::toupper(static_cast(ch))); + } + } + + bool initTransport() + { + if (comm_type == "CAN") { + return initCanTransport(); + } + if (comm_type == "MODBUS") { + return initModbusTransport(); + } + RCLCPP_ERROR(this->get_logger(), "Unsupported COMM_TYPE: %s", comm_type.c_str()); + return false; + } + + bool initCanTransport() + { + try { + hand_api = std::make_unique(hand_model, side, COMM_TYPE::CAN); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to create CAN LinkerHandApi: %s", e.what()); + return false; + } + + try { + can_bus = std::shared_ptr( + Communication::CommFactory::createCanBus(side)); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to create CAN bus: %s", e.what()); + RCLCPP_ERROR(this->get_logger(), + "Start CAN first, for example: sudo ip link set can0 up type can bitrate %d && " + "sudo ip link set can0 txqueuelen 1024", + can_bitrate); + return false; + } + + hand_api->setCanTxCallback([this](uint32_t can_id, const uint8_t * data, + uintptr_t data_len) -> int32_t { + try { + std::vector data_vec(data, data + data_len); + can_bus->send(data_vec, can_id); + return 0; + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "CAN send error: %s", e.what()); + return -1; + } + }); + + hand_api->setCanRxCallback([this](uint32_t * can_id_out, uint8_t * data_out, + uint8_t * len_out) -> int32_t { + try { + auto frame = can_bus->recv(); + if (frame.can_id == 0 && frame.can_dlc == 0) { + return -1; + } + *can_id_out = frame.can_id; + *len_out = frame.can_dlc; + std::memcpy(data_out, frame.data, frame.can_dlc); + return 0; + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "CAN receive error: %s", e.what()); + return -1; + } + }); + + return true; + } + + bool initModbusTransport() + { + if (!supportsModbus(hand_joints)) { + RCLCPP_ERROR(this->get_logger(), "MODBUS is only supported for O6, L7, and L10"); + return false; + } + + try { + hand_api = std::make_unique(hand_model, side, COMM_TYPE::MODBUS); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to create Modbus LinkerHandApi: %s", e.what()); + return false; + } - pub_touch_thread.join(); - pub_state_thread.join(); - pub_info_thread.join(); + try { + modbus = std::make_shared(side); + } catch (const std::exception & e) { + RCLCPP_ERROR(this->get_logger(), "Failed to create Modbus transport: %s", e.what()); + return false; + } + if (!modbus || !modbus->isOpen()) { + RCLCPP_ERROR(this->get_logger(), "Failed to open Modbus serial transport"); + return false; + } + + hand_api->setModbusTxCallback([this](uint8_t, uint16_t, const uint8_t * data, + uintptr_t len) -> int32_t { + if (!modbus) { + return -1; + } + return modbus->sendRawFrame(data, len) ? 0 : -1; + }); + + hand_api->setModbusRxCallback([this](uint8_t sid, uint16_t * addr_out, + uint8_t * data_out, uint8_t * len_out) -> int32_t { + if (!modbus) { + return -1; + } + const int len = modbus->receiveCompleteFrame(data_out, 256, 500); + if (len <= 0 || data_out[0] != sid) { + return -1; + } + *len_out = static_cast(len); + if (addr_out) { + *addr_out = 0; + } + return 0; + }); + + RCLCPP_INFO(this->get_logger(), "Modbus transport initialized by hand side"); + return true; + } + + bool supportsModbus(const std::string & hand_name) const + { + return hand_name == "O6" || hand_name == "L7" || hand_name == "L10"; } // General function: Publish touch sensor data @@ -267,6 +442,7 @@ class LinkerHand : public rclcpp::Node } } } + publisher.publish(message); } // General function: public joint state @@ -277,8 +453,8 @@ class LinkerHand : public rclcpp::Node { auto message = sensor_msgs::msg::JointState(); - (is_arc) ? message.position = hand->getStateArc() : message.position = convert(hand->getState()); + (is_arc) ? message.position = hand->getPositionArc() : message.position = convert(hand->getPosition()); message.velocity = convert(hand->getSpeed()); message.effort = convert(hand->getTorque()); @@ -292,9 +468,8 @@ class LinkerHand : public rclcpp::Node rclcpp::Publisher & publisher) { auto message = std_msgs::msg::String(); - message.data = hand->getVersion() + "Temperature: " + vectorToString(hand->getTemperature()) + - "\nFaultCode: " + vectorToString(hand->getFaultCode()) + "\nCurrent: " + - vectorToString(hand->getCurrent()); + message.data = hand->getVersion() + "\nTemperature: " + vectorToString(hand->getTemperature()) + + "\nFaultCode: " + vectorToString(hand->getFaultCode()); publisher.publish(message); @@ -327,44 +502,74 @@ class LinkerHand : public rclcpp::Node std::vector speed(5, hand_speed); std::vector effort(5, hand_effort); - if (hand_name == "L7" && msg->position.size() == 7) { - (msg->velocity.size() == 7) ? speed = convert(msg->velocity) : speed = std::vector(7, hand_speed); - (msg->effort.size() == 7) ? effort = convert(msg->effort) : effort = std::vector(7, hand_effort); - - hand->setSpeed(speed); // speed - hand->setTorque(effort); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } else if (hand_name == "L10" && msg->position.size() == 10) { - if (msg->velocity.size() == 5) {speed = convert(msg->velocity);} - - hand->setSpeed(speed); // speed - hand->setTorque(effort); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } else if (hand_name == "L20" && msg->position.size() == 20) { - if (msg->velocity.size() == 5) {speed = convert(msg->velocity);} - - hand->setSpeed(speed); // speed - // hand->setTorque({100, 100, 100, 100, 100}); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } else if ((hand_name == "L25" || hand_name == "L21") && msg->position.size() == 25) { - (msg->velocity.size() == 25) ? speed = convert(msg->velocity) : speed = std::vector(25, hand_speed); - (msg->effort.size() == 25) ? effort = convert(msg->effort) : effort = std::vector(25, hand_effort); - - hand->setSpeed(speed); // speed - hand->setTorque(effort); // torque - (is_arc) ? hand->fingerMoveArc(msg->position) : hand->fingerMove(convert(msg->position)); // joint position - } else { + const auto expected_joint_count = getJointCount(hand_name); + if (expected_joint_count == 0 || msg->position.size() != expected_joint_count) { std::cout << hand_name << " Invalid joint number: " << msg->position.size() << std::endl; return; } + + if (msg->velocity.size() == expected_joint_count) { + speed = convert(msg->velocity); + } else { + speed = std::vector(expected_joint_count, hand_speed); + } + if (msg->effort.size() == expected_joint_count) { + effort = convert(msg->effort); + } else { + effort = std::vector(expected_joint_count, hand_effort); + } + + const auto position = convert(msg->position); + bool send_position = true; + bool send_speed = true; + bool send_effort = true; + { + std::lock_guard lock(control_mutex_); + if (has_last_control_command && last_control_is_arc == is_arc) { + if (!is_arc) { + send_position = last_position_command != position; + } + send_speed = last_speed_command != speed; + send_effort = last_effort_command != effort; + if (!send_position && !send_speed && !send_effort) { + return; + } + } + has_last_control_command = true; + last_control_is_arc = is_arc; + if (!is_arc && send_position) { + last_position_command = position; + } + if (send_speed) { + last_speed_command = speed; + } + if (send_effort) { + last_effort_command = effort; + } + } + + if (send_speed) { + hand->setSpeed(speed); + } + if (send_effort) { + hand->setTorque(effort); + } + if (send_position) { + (is_arc) ? hand->setPositionArc(msg->position) : hand->setPosition(position); + } + } + + size_t getJointCount(const std::string & hand_name) const + { + if (hand_name == "L6" || hand_name == "O6") {return 6;} + if (hand_name == "L7") {return 7;} + if (hand_name == "L10") {return 10;} + if (hand_name == "L20") {return 20;} + if (hand_name == "L21") {return 21;} + if (hand_name == "L25") {return 25;} + if (hand_name == "G20") {return 16;} + if (hand_name == "O20") {return 16;} + return 0; } // std::vector to std::string @@ -384,9 +589,10 @@ class LinkerHand : public rclcpp::Node void initHand(const HandType & hand, const std::string & hand_name) { if (hand_name == "L10") { - hand->setSpeed(std::vector(5, hand_speed)); // speed - hand->setTorque(std::vector(5, hand_effort)); // torque - hand->fingerMove({255, 128, 255, 255, 255, 255, 128, 128, 128, 128}); // joint position + const auto joint_count = getJointCount(hand_name); + hand->setSpeed(std::vector(joint_count, hand_speed)); // speed + hand->setTorque(std::vector(joint_count, hand_effort)); // torque + hand->setPosition({255, 128, 255, 255, 255, 255, 128, 128, 128, 128}); // joint position } } @@ -408,10 +614,17 @@ class LinkerHand : public rclcpp::Node std::string version; bool hand_exists; bool hand_touch; + std::atomic node_active{false}; std::string hand_joints; + std::string comm_type; int hand_speed; int hand_effort; + int can_bitrate; + HAND_TYPE side; + LINKER_HAND hand_model; + std::shared_ptr can_bus; + std::shared_ptr modbus; // 创建线程对象 std::thread pub_touch_thread; @@ -427,19 +640,18 @@ class LinkerHand : public rclcpp::Node std::string hand_control_arc_topic; int hand_type; + std::mutex control_mutex_; + bool has_last_control_command{false}; + bool last_control_is_arc{false}; + std::vector last_position_command; + std::vector last_speed_command; + std::vector last_effort_command; }; // signal void signalHandler(int signal) { if (signal == SIGINT) { - // stop can channel - int result = system(std::string("sudo ip link set " + can_channel + " down").c_str()); - if (result == 0) { - std::cout << ((can_channel == - "can0") ? "CAN0" : "CAN1") << " interface stopped successfully." << std::endl; - } - RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Ctrl+C detected. Shutting down..."); rclcpp::shutdown(); } @@ -453,6 +665,11 @@ int main(int argc, char *argv[]) std::signal(SIGINT, signalHandler); auto node = std::make_shared(); + if (!node->isActive()) { + RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "Hand is disabled or failed to initialize. Exiting."); + rclcpp::shutdown(); + return 0; + } rclcpp::spin(node); return 0;