diff --git a/README.md b/README.md index e69de29..bd44893 100644 --- a/README.md +++ b/README.md @@ -0,0 +1,255 @@ +# wpz_LbotArm 接口说明 + +本工程当前使用 `src/officer_sdk/lbot_driver` 作为机器人网口连接和 ROS2 bridge。启动脚本为： + +```bash +cd ~/workspace/wpz_LbotArm/scripts +./lbot_arm_control.sh start +``` + +停止服务： + +```bash +cd ~/workspace/wpz_LbotArm/scripts +./lbot_arm_control.sh stop +``` + +默认启动的机器人 namespace 是 `/robot1`，默认连接控制器 IP 为 `192.168.10.21`。 + +## 机械臂反馈订阅 + +外部软件如果需要订阅机器人当前机械臂各关节角度、速度、电流/力矩，使用下面两个 topic： + +```text +/robot1/left_arm/joint_states +/robot1/right_arm/joint_states +``` + +消息类型： + +```text +sensor_msgs/msg/JointState +``` + +字段含义： + +```text +name[] 关节名称 +position[] 当前 7 个关节角度，单位 rad +velocity[] 当前 7 个关节速度，单位 rad/s +effort[] 当前 7 个关节 effort 字段；当前 driver 直接填入 SDK 的 effort[7]，按项目使用可视为关节电流/力矩反馈字段 +``` + +示例： + +```bash +ros2 topic echo /robot1/left_arm/joint_states +ros2 topic echo /robot1/right_arm/joint_states +``` + +当前反馈中没有关节加速度字段。如果外部软件必须获取关节加速度，需要在上游根据 `velocity[]` 和时间戳自行差分，或者让厂家提供 SDK 原生加速度反馈接口。 + +## 机械臂关节空间控制 + +外部软件如果需要发送机械臂各关节目标位置，使用 MoveJ 服务，不是 topic。 + +左臂： + +```text +/robot1/left_arm/move_joint +``` + +右臂： + +```text +/robot1/right_arm/move_joint +``` + +服务类型： + +```text +lbot_arm_interfaces/srv/MoveJ +``` + +服务定义： + +```srv +float32[] joints +float32 speed +float32 acce +bool block +--- +bool success +``` + +字段含义： + +```text +joints 7 个目标关节角，单位 rad +speed 整条机械臂统一目标速度，单位 rad/s +acce 整条机械臂统一目标加速度，单位 rad/s^2 +block true 表示阻塞等待运动完成，false 表示非阻塞下发 +``` + +示例： + +```bash +ros2 service call /robot1/left_arm/move_joint lbot_arm_interfaces/srv/MoveJ \ +"{joints: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], speed: 1.0, acce: 1.0, block: true}" +``` + +当前限制： + +```text +MoveJ 支持每个关节独立目标位置 joints[7] +MoveJ 不支持每个关节独立目标速度 velocity[7] +MoveJ 不支持每个关节独立目标加速度 acceleration[7] +speed 和 acce 是整条机械臂统一标量 +``` + +如果后续必须支持每个关节单独目标速度和加速度，需要修改： + +```text +src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv +src/officer_sdk/lbot_driver/src/lbot_driver.cpp +src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h +``` + +但当前 SDK 的 `lbot_move_joint()` 接口本身只接收： + +```cpp +const double joints[7], double speed, double accel, bool block +``` + +因此即使 ROS service 增加 `velocity[7]`、`acceleration[7]` 字段，也不能直接传给现有 SDK。要真正实现每关节速度/加速度控制，需要厂家提供更底层的控制接口，或者在 ROS 层自己做轨迹插值，再周期性下发关节位置。 + +## L20/R20 灵巧手整手位置控制 + +外部软件如果需要一次性控制机器人灵巧手所有关节位置，使用下面两个 topic： + +```text +/robot1/left_hand/set_l20_joint +/robot1/right_hand/set_l20_joint +``` + +消息类型： + +```text +std_msgs/msg/Int32MultiArray +``` + +字段含义： + +```text +data[] L20/R20 灵巧手目标位置数组 +``` + +当前 driver 中该 topic 直接调用 SDK： + +```cpp +lbot_l20_set_all_position(lbot_handle, LBOT_LEFT_ARM, hand_joints) +lbot_l20_set_all_position(lbot_handle, LBOT_RIGHT_ARM, hand_joints) +``` + +SDK 头文件注释写明该接口参数是： + +```text +16 个自由度的目标位置，单位 degree +``` + +因此当前整手控制按 16 个整数位置发送。 + +示例： + +```bash +ros2 topic pub --once /robot1/left_hand/set_l20_joint std_msgs/msg/Int32MultiArray \ +"{data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}" +``` + +右手示例： + +```bash +ros2 topic pub --once /robot1/right_hand/set_l20_joint std_msgs/msg/Int32MultiArray \ +"{data: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]}" +``` + +## L20/R20 单指串联位置控制 + +如果只需要控制单根手指，可以使用： + +```text +/robot1/left_hand/set_l20_series_position +/robot1/right_hand/set_l20_series_position +``` + +消息类型： + +```text +std_msgs/msg/Int32MultiArray +``` + +当前约定的数据格式： + +```text +data[0] finger 编号 +data[1..6] 该手指的 6 个位置参数 +``` + +finger 编号： + +```text +0 thumb +1 index +2 middle +3 ring +4 little +``` + +示例，控制左手食指： + +```bash +ros2 topic pub --once /robot1/left_hand/set_l20_series_position std_msgs/msg/Int32MultiArray \ +"{data: [1, 0, 30, 30, 0, 0, 0]}" +``` + +## 相关代码位置 + +反馈发布： + +```text +src/officer_sdk/lbot_driver/src/lbot_driver.cpp +``` + +主要位置： + +```text +LBot::state_publish_timer_callback() +``` + +机械臂 MoveJ 服务： + +```text +LeftArmServiceNode::move_joint_callback() +RightArmServiceNode::move_joint_callback() +``` + +L20/R20 灵巧手 topic： + +```text +LeftArmServiceNode::left_hand_l20_set_joint_callback() +RightArmServiceNode::right_hand_l20_set_joint_callback() +LeftArmServiceNode::left_hand_l20_set_series_position_callback() +RightArmServiceNode::right_hand_l20_set_series_position_callback() +``` + +SDK API 声明： + +```text +src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api_cpp.h +``` + +ROS 接口定义： + +```text +src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv +``` diff --git a/scripts/lbot_arm_control.sh b/scripts/lbot_arm_control.sh new file mode 100755 index 0000000..ca10ac5 --- /dev/null +++ b/scripts/lbot_arm_control.sh @@ -0,0 +1,138 @@ +#!/usr/bin/env bash +set -euo pipefail + +SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)" +WS_DIR="$(cd "${SCRIPT_DIR}/.." && pwd)" +STATE_DIR="/tmp/lbot_arm_control" +PID_FILE="${STATE_DIR}/lbot_driver.pid" +PGID_FILE="${STATE_DIR}/lbot_driver.pgid" +LOG_FILE="${WS_DIR}/log/lbot_arm_control.log" + +usage() { + echo "Usage: $0 {start|stop|status}" +} + +is_running() { + [[ -f "${PID_FILE}" ]] || return 1 + local pid + pid="$(cat "${PID_FILE}")" + [[ -n "${pid}" ]] || return 1 + kill -0 "${pid}" 2>/dev/null +} + +source_workspace() { + set +u + if [[ -n "${ROS_DISTRO:-}" && -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then + # shellcheck disable=SC1090 + source "/opt/ros/${ROS_DISTRO}/setup.bash" + elif [[ -f "/opt/ros/jazzy/setup.bash" ]]; then + # shellcheck disable=SC1091 + source "/opt/ros/jazzy/setup.bash" + fi + set -u + + if [[ ! -f "${WS_DIR}/install/setup.bash" ]]; then + echo "Workspace is not built: ${WS_DIR}/install/setup.bash not found" + echo "Run: cd ${WS_DIR} && colcon build --packages-select lbot_arm_interfaces lbot_driver" + exit 1 + fi + + # shellcheck disable=SC1091 + set +u + source "${WS_DIR}/install/setup.bash" + set -u +} + +start_driver() { + mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")" + + if is_running; then + echo "lbot_driver is already running, pid=$(cat "${PID_FILE}")" + return 0 + fi + + source_workspace + + echo "Starting lbot_driver from officer_sdk..." + echo "Log: ${LOG_FILE}" + + : > "${LOG_FILE}" + setsid bash -lc "set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch lbot_driver lbot_start_driver.launch.py" \ + >> "${LOG_FILE}" 2>&1 & + + local pid=$! + local pgid + pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')" + echo "${pid}" > "${PID_FILE}" + echo "${pgid:-${pid}}" > "${PGID_FILE}" + + sleep 1 + if is_running; then + echo "lbot_driver started, pid=${pid}, pgid=$(cat "${PGID_FILE}")" + else + echo "lbot_driver failed to start. Check log: ${LOG_FILE}" + rm -f "${PID_FILE}" "${PGID_FILE}" + exit 1 + fi +} + +stop_driver() { + if ! [[ -f "${PID_FILE}" ]]; then + echo "lbot_driver is not running" + return 0 + fi + + local pid pgid + pid="$(cat "${PID_FILE}")" + pgid="${pid}" + if [[ -f "${PGID_FILE}" ]]; then + pgid="$(cat "${PGID_FILE}")" + fi + + if ! kill -0 "${pid}" 2>/dev/null; then + echo "lbot_driver process is not alive, cleaning state" + rm -f "${PID_FILE}" "${PGID_FILE}" + return 0 + fi + + echo "Stopping lbot_driver, pid=${pid}, pgid=${pgid}..." + kill -TERM -- "-${pgid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true + + for _ in {1..30}; do + if ! kill -0 "${pid}" 2>/dev/null; then + rm -f "${PID_FILE}" "${PGID_FILE}" + echo "lbot_driver stopped" + return 0 + fi + sleep 0.2 + done + + echo "lbot_driver did not exit after SIGTERM, forcing stop..." + kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true + rm -f "${PID_FILE}" "${PGID_FILE}" + echo "lbot_driver stopped" +} + +status_driver() { + if is_running; then + echo "lbot_driver is running, pid=$(cat "${PID_FILE}"), pgid=$(cat "${PGID_FILE}" 2>/dev/null || cat "${PID_FILE}")" + else + echo "lbot_driver is not running" + fi +} + +case "${1:-}" in + start) + start_driver + ;; + stop) + stop_driver + ;; + status) + status_driver + ;; + *) + usage + exit 1 + ;; +esac diff --git a/src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt b/src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt new file mode 100755 index 0000000..87b542c --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt @@ -0,0 +1,71 @@ +cmake_minimum_required(VERSION 3.5) +project(lbot_arm_interfaces) + +# 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() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(std_msgs REQUIRED) +find_package(sensor_msgs REQUIRED) + +find_package(rosidl_default_generators REQUIRED) + +rosidl_generate_interfaces(${PROJECT_NAME} + # srv + "srv/MoveJ.srv" + "srv/MoveL.srv" + "srv/MoveC.srv" + "srv/MoveJP.srv" + "srv/InverseKinematics.srv" + "srv/ForwardKinematics.srv" + "srv/SetFrame.srv" + "srv/SetString.srv" + "srv/GetFrame.srv" + "srv/GetCurrentFrame.srv" + "srv/ChangeFrame.srv" + "srv/DeleteFrame.srv" + "srv/GetAllFrames.srv" + "srv/SetZero.srv" + "srv/SetEmergency.srv" + "srv/SetEnable.srv" + # msg + "msg/ArmState.msg" + "msg/LbotPose.msg" + "msg/LbotFrame.msg" + "msg/FollowJoint.msg" + + DEPENDENCIES std_msgs geometry_msgs sensor_msgs + ) + + ament_export_dependencies(rosidl_default_runtime) + + +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # uncomment the line when a copyright and license is not present in all source files + #set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # uncomment the line when this package is not in a git repo + #set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/officer_sdk/lbot_arm_interfaces/msg/ArmState.msg b/src/officer_sdk/lbot_arm_interfaces/msg/ArmState.msg new file mode 100755 index 0000000..68e26f0 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/msg/ArmState.msg @@ -0,0 +1,3 @@ +float32[] joints +geometry_msgs/Vector3 euler +geometry_msgs/Pose pose diff --git a/src/officer_sdk/lbot_arm_interfaces/msg/FollowJoint.msg b/src/officer_sdk/lbot_arm_interfaces/msg/FollowJoint.msg new file mode 100755 index 0000000..c8f6760 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/msg/FollowJoint.msg @@ -0,0 +1,2 @@ +float32[] joints +bool follow diff --git a/src/officer_sdk/lbot_arm_interfaces/msg/LbotFrame.msg b/src/officer_sdk/lbot_arm_interfaces/msg/LbotFrame.msg new file mode 100644 index 0000000..886d08c --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/msg/LbotFrame.msg @@ -0,0 +1,3 @@ +string name +geometry_msgs/Vector3 euler +geometry_msgs/Vector3 position \ No newline at end of file diff --git a/src/officer_sdk/lbot_arm_interfaces/msg/LbotPose.msg b/src/officer_sdk/lbot_arm_interfaces/msg/LbotPose.msg new file mode 100755 index 0000000..e279aa2 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/msg/LbotPose.msg @@ -0,0 +1,2 @@ +geometry_msgs/Vector3 euler +geometry_msgs/Vector3 position diff --git a/src/officer_sdk/lbot_arm_interfaces/package.xml b/src/officer_sdk/lbot_arm_interfaces/package.xml new file mode 100755 index 0000000..eb8b176 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/package.xml @@ -0,0 +1,25 @@ + + + + lbot_arm_interfaces + 0.0.0 + TODO: Package description + Ross + TODO: License declaration + + ament_cmake + + rosidl_default_generators + rosidl_default_runtime + rosidl_interface_packages + + ament_lint_auto + ament_lint_common + + + ament_cmake + + std_msgs + geometry_msgs + sensor_msgs + diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/ChangeFrame.srv b/src/officer_sdk/lbot_arm_interfaces/srv/ChangeFrame.srv new file mode 100644 index 0000000..59e4a8a --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/ChangeFrame.srv @@ -0,0 +1,3 @@ +string name +--- +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/DeleteFrame.srv b/src/officer_sdk/lbot_arm_interfaces/srv/DeleteFrame.srv new file mode 100644 index 0000000..59e4a8a --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/DeleteFrame.srv @@ -0,0 +1,3 @@ +string name +--- +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/ForwardKinematics.srv b/src/officer_sdk/lbot_arm_interfaces/srv/ForwardKinematics.srv new file mode 100755 index 0000000..9622f3d --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/ForwardKinematics.srv @@ -0,0 +1,7 @@ +float32[] joints + +--- +geometry_msgs/Vector3 position +geometry_msgs/Vector3 euler + +bool success \ No newline at end of file diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/GetAllFrames.srv b/src/officer_sdk/lbot_arm_interfaces/srv/GetAllFrames.srv new file mode 100644 index 0000000..f468109 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/GetAllFrames.srv @@ -0,0 +1,4 @@ + +--- +string[] names +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/GetCurrentFrame.srv b/src/officer_sdk/lbot_arm_interfaces/srv/GetCurrentFrame.srv new file mode 100644 index 0000000..824338f --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/GetCurrentFrame.srv @@ -0,0 +1,5 @@ +--- +string name +lbot_arm_interfaces/LbotFrame frame +bool success + diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/GetFrame.srv b/src/officer_sdk/lbot_arm_interfaces/srv/GetFrame.srv new file mode 100644 index 0000000..2562797 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/GetFrame.srv @@ -0,0 +1,4 @@ +string name +--- +lbot_arm_interfaces/LbotFrame frame +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/InverseKinematics.srv b/src/officer_sdk/lbot_arm_interfaces/srv/InverseKinematics.srv new file mode 100755 index 0000000..fc91edd --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/InverseKinematics.srv @@ -0,0 +1,6 @@ +float32[] joints # 此关节角度不设置会默认从机械臂读取当前角度，如果设置则基于此值为初始角度进行逆解 +geometry_msgs/Vector3 position +geometry_msgs/Vector3 euler +--- +float32[] joints +bool success \ No newline at end of file diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/MoveC.srv b/src/officer_sdk/lbot_arm_interfaces/srv/MoveC.srv new file mode 100755 index 0000000..e543615 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/MoveC.srv @@ -0,0 +1,9 @@ +geometry_msgs/Vector3 position +geometry_msgs/Vector3 euler +float32 speed +float32 acce +bool block + +--- + +bool success \ No newline at end of file diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv b/src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv new file mode 100755 index 0000000..5ebaa0a --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/MoveJ.srv @@ -0,0 +1,7 @@ +float32[] joints +float32 speed +float32 acce +bool block + +--- +bool success \ No newline at end of file diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/MoveJP.srv b/src/officer_sdk/lbot_arm_interfaces/srv/MoveJP.srv new file mode 100755 index 0000000..e543615 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/MoveJP.srv @@ -0,0 +1,9 @@ +geometry_msgs/Vector3 position +geometry_msgs/Vector3 euler +float32 speed +float32 acce +bool block + +--- + +bool success \ No newline at end of file diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/MoveL.srv b/src/officer_sdk/lbot_arm_interfaces/srv/MoveL.srv new file mode 100755 index 0000000..e543615 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/MoveL.srv @@ -0,0 +1,9 @@ +geometry_msgs/Vector3 position +geometry_msgs/Vector3 euler +float32 speed +float32 acce +bool block + +--- + +bool success \ No newline at end of file diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/SetEmergency.srv b/src/officer_sdk/lbot_arm_interfaces/srv/SetEmergency.srv new file mode 100755 index 0000000..6781332 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/SetEmergency.srv @@ -0,0 +1,5 @@ +bool emergency + +--- + +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/SetEnable.srv b/src/officer_sdk/lbot_arm_interfaces/srv/SetEnable.srv new file mode 100755 index 0000000..f5d9de0 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/SetEnable.srv @@ -0,0 +1,5 @@ +bool enable + +--- + +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/SetFrame.srv b/src/officer_sdk/lbot_arm_interfaces/srv/SetFrame.srv new file mode 100644 index 0000000..1eadc65 --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/SetFrame.srv @@ -0,0 +1,3 @@ +lbot_arm_interfaces/LbotFrame frame +--- +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/SetString.srv b/src/officer_sdk/lbot_arm_interfaces/srv/SetString.srv new file mode 100644 index 0000000..59e4a8a --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/SetString.srv @@ -0,0 +1,3 @@ +string name +--- +bool success diff --git a/src/officer_sdk/lbot_arm_interfaces/srv/SetZero.srv b/src/officer_sdk/lbot_arm_interfaces/srv/SetZero.srv new file mode 100644 index 0000000..d857c6a --- /dev/null +++ b/src/officer_sdk/lbot_arm_interfaces/srv/SetZero.srv @@ -0,0 +1,3 @@ + +--- +bool success diff --git a/src/officer_sdk/lbot_driver/CMakeLists.txt b/src/officer_sdk/lbot_driver/CMakeLists.txt new file mode 100644 index 0000000..9aa9b81 --- /dev/null +++ b/src/officer_sdk/lbot_driver/CMakeLists.txt @@ -0,0 +1,64 @@ +cmake_minimum_required(VERSION 3.8) +project(lbot_driver) + +set(CMAKE_CXX_STANDARD 14) +set(CMAKE_CXX_STANDARD_REQUIRED ON) + +add_compile_options(-Wall -Wextra -Wpedantic) + +# ROS2 dependencies +find_package(ament_cmake REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(std_srvs REQUIRED) +find_package(sensor_msgs REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) +find_package(lbot_arm_interfaces REQUIRED) + +# include directories +include_directories( + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR}/include/${PROJECT_NAME} +) + +# library directory +link_directories(${PROJECT_SOURCE_DIR}/lib) + +# executable +add_executable(lbot_driver src/lbot_driver.cpp) + +# link the API library (name only! no lib prefix, no .so suffix) +target_link_libraries(lbot_driver + lbot_api_cpp +) + +# ros deps +ament_target_dependencies(lbot_driver + rclcpp + std_msgs + std_srvs + sensor_msgs + geometry_msgs + tf2 + tf2_geometry_msgs + lbot_arm_interfaces +) + +# install binary +install(TARGETS lbot_driver + DESTINATION lib/${PROJECT_NAME} +) + +# install launch & config +install(DIRECTORY launch config + DESTINATION share/${PROJECT_NAME} +) + +# install .so libraries +install(DIRECTORY lib/ + DESTINATION lib +) + +ament_package() diff --git a/src/officer_sdk/lbot_driver/config/lbot_config.yaml b/src/officer_sdk/lbot_driver/config/lbot_config.yaml new file mode 100755 index 0000000..af42273 --- /dev/null +++ b/src/officer_sdk/lbot_driver/config/lbot_config.yaml @@ -0,0 +1,4 @@ +lbot_driver: + ros__parameters: + #robot param + arm_ip: "192.168.10.21" #设置TCP连接时的IP \ No newline at end of file diff --git a/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api.h b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api.h new file mode 100644 index 0000000..4634746 --- /dev/null +++ b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api.h @@ -0,0 +1,485 @@ +/** + * @file lbot_api.h + * @brief LBot机器人控制API接口 + * @date 2026.1.19 + * @copyright 灵心巧手科技有限公司 + */ +#ifndef LBOT_API_H +#define LBOT_API_H + +#ifdef __cplusplus +extern "C" { +#endif + +// 跨平台导出宏 +#ifdef _WIN32 + #ifdef LBOT_API_EXPORTS + #define LBOT_API __declspec(dllexport) + #else + #define LBOT_API __declspec(dllimport) + #endif +#else + #define LBOT_API __attribute__((visibility("default"))) +#endif + +#include "lbot_types.h" +#include "lbot_version.h" + +// ============================================== +// API初始化和清理函数 +// ============================================== +/** + * @brief 初始化LBot API连接 + * @param tcp_host TCP服务器地址格式："192.168.10.21" + * @return lbot_handle_t 连接句柄，连接成功句柄ID>0，失败返回NULL + */ +LBOT_API lbot_handle_t *lbot_init(const char* tcp_host); + +/** + * @brief 断开指定连接 + * @param handle 机械臂句柄 + * @return true 断开成功，false 断开失败 + */ +LBOT_API bool lbot_disconnect(lbot_handle_t *handle); + +/** + * @brief 清理API资源，断开连接 + */ +LBOT_API void lbot_cleanup(); + +/** + * @brief 获取API版本信息 + * @return 版本字符串 + */ +LBOT_API const char* lbot_get_api_version(); +// ============================================== +// 系统信息获取函数 +// ============================================== +/** + * @brief 获取控制器信息 + * @param handle 机械臂句柄 + * @param robot_model 返回的机器人型号字符串（需要调用者释放） + * @param controller_version 返回的控制器版本字符串（需要调用者释放） + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_controller_info(lbot_handle_t *handle, char** robot_model, char** controller_version); + +// ============================================== +// 状态监控和管理函数 +// ============================================== +/** + * @brief 启动状态监控 + * @param state_cb 状态回调函数，当机器人状态更新时调用 + * @param error_cb 错误回调函数，当发生错误时调用 + * @return true 启动成功，false 启动失败 + */ +LBOT_API bool lbot_start_state_monitor(lbot_state_callback_t state_cb, lbot_error_callback_t error_cb); + +/** + * @brief 停止状态监控 + */ +LBOT_API void lbot_stop_state_monitor(); + +/** + * @brief 获取当前机器人完整状态，此接口需要在启动状态监控后才能调用 + * @param handle 机械臂句柄 + * @param state 返回的机器人状态结构体指针 + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_current_state(lbot_handle_t *handle, lbot_full_state_t* state); + +// ============================================== +// 运动控制函数 +// ============================================== +/** + * @brief 关节空间运动 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 7个关节的目标角度（弧度） + * @param speed 运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 加速度（0.0~20.0） 单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_move_joint(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], double speed, double accel, bool block); + +/** + * @brief 笛卡尔空间姿态运动（关节插值） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 目标位置（x, y, z，单位：米） + * @param euler 机械臂末端目标欧拉角（roll, pitch, yaw，单位：弧度） + * @param speed 机械臂末端运动速度（0.0~20.0）单位m/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 机械臂末端加速度（0.0~20.0）单位m/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_move_pose(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, + double speed, double accel, bool block); + +/** + * @brief 笛卡尔空间直线运动（直线插值） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 目标位置（x, y, z，单位：米） + * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度） + * @param speed 关节运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 关节运动加速度（0.0~20.0） 单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_move_linear(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, + double speed, double accel, bool block); + +// ============================================== +// 关节跟随函数（用于遥操作） +// ============================================== +/** + * @brief 关节跟随控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 7个关节的目标角度（弧度） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_joint_follow(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7]); + +/** + * @brief 笛卡尔空间姿态跟随运动 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param pos 目标位置（x, y, z，单位：米） + * @param eul 目标欧拉角（roll, pitch, yaw，单位：弧度） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_pose_follow(lbot_handle_t *handle, lbot_arm_t arm, lbot_position_t pos, lbot_euler_t eul); +// ============================================== +// l6 手控制接口 +// ============================================== +/** + * @brief 设置L6手的位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 6个手指的目标位置（0~255） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l6_set_position(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t position[6]); + +/** + * @brief 设置L6手的速度控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param velocity 6个手指的目标速度（0~255） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l6_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t velocity[6]); + +/** + * @brief 设置L6手的力矩控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param torque 6个手指的目标力矩（0~255） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l6_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t torque[6]); + +// ============================================== +// l10 手控制接口（10个自由度） +// ============================================== +/** + * @brief 设置L10手的位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 10个手指的目标位置（0~255） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l10_set_position(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t position[10]); + +/** + * @brief 设置L10手的速度控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param velocity 10个手指的目标速度（0~255） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l10_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t velocity[10]); + +/** + * @brief 设置L10手的力矩控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param torque 10个手指的目标力矩（0~255） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l10_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const uint8_t torque[10]); + +// ============================================== +// r20 手控制接口 +// ============================================== +/** + * @brief 设置R20手各手指位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param cmd 目标位置命令 + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l20_set_series_position(lbot_handle_t *handle, lbot_arm_t arm, const lbot_l20_series_cmd_t* cmd); +/** + * @brief 设置R20手所有自由度位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param cmd 目标位置命令, 16个自由度的目标位置(度)，分别为：拇指指根（0~120）、 + * 拇指指尖（0~150）、拇指侧摆（0~180）、拇指旋转（0~130）、食指侧摆（-30~30）、 + * 食指指根（0~180）、食指指尖（0~180）、中指侧摆（-30~30）、中指指根（0~180）、 + * 中指指尖（0~180）、无名指侧摆（-20~20）、无名指指根（0~180）、无名指指尖（0~180）、 + * 小指侧摆（-20~20）、小指指根（0~180）、小指指尖（0~180） + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_l20_set_all_position(lbot_handle_t *handle, lbot_arm_t arm, const int cmd[16]); + + +// ============================================== +// 运动学计算函数 +// ============================================== +/** + * @brief 正运动学计算 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 7个关节角度（弧度） + * @param position 返回的末端位置（x, y, z，单位：米） + * @param euler 返回的末端欧拉角（roll, pitch, yaw，单位：弧度） + * @return true 计算成功，false 计算失败 + */ +LBOT_API bool lbot_forward_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], + lbot_position_t* position, lbot_euler_t* euler); + +/** + * @brief 逆运动学计算 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param initial_joints 初始关节角度（弧度），用于求解器迭代 + * @param position 目标位置（x, y, z，单位：米） + * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度） + * @param result_joints 返回的7个关节角度解（弧度） + * @return true 求解成功，false 求解失败 + */ +LBOT_API bool lbot_inverse_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double initial_joints[7], + const lbot_position_t* position, const lbot_euler_t* euler, + double result_joints[7]); + +// ============================================== +// 工具坐标系管理函数 +// ============================================== +/** + * @brief 设置工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工具坐标系名称（最大32字符） + * @param position 工具坐标系相对于法兰盘的位置偏移（x, y, z，单位：米） + * @param euler 工具坐标系相对于法兰盘的欧拉角偏移（roll, pitch, yaw，单位：弧度） + * @return true 设置成功，false 设置失败 + */ +LBOT_API bool lbot_set_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + const lbot_position_t* position, const lbot_euler_t* euler); + +/** + * @brief 获取工具坐标系参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工具坐标系名称 + * @param position 返回的工具坐标系位置偏移 + * @param euler 返回的工具坐标系欧拉角偏移 + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + lbot_position_t* position, lbot_euler_t* euler); + +/** + * @brief 获取当前使用的工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 返回的当前工具坐标系名称（需要调用者释放） + * @param position 返回的当前工具坐标系位置偏移 + * @param euler 返回的当前工具坐标系欧拉角偏移 + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_current_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, + char** name, + lbot_position_t* position, + lbot_euler_t* euler); + +/** + * @brief 切换当前工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要切换到的工具坐标系名称 + * @return true 切换成功，false 切换失败 + */ +LBOT_API bool lbot_change_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + +/** + * @brief 删除工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要删除的工具坐标系名称 + * @return true 删除成功，false 删除失败 + */ +LBOT_API bool lbot_delete_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + +/** + * @brief 获取所有工具坐标系名称 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param names 返回的工具坐标系名称数组（需要调用lbot_free_string_array释放） + * @param count 返回的工具坐标系数量 + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_all_tool_frames(lbot_handle_t *handle, lbot_arm_t arm, char*** names, int* count); + +// ============================================== +// 工作坐标系管理函数 +// ============================================== +/** + * @brief 设置工作坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工作坐标系名称（最大32字符） + * @param position 工作坐标系相对于基坐标系的位置偏移（x, y, z，单位：米） + * @param euler 工作坐标系相对于基坐标系的欧拉角偏移（roll, pitch, yaw，单位：弧度） + * @return true 设置成功，false 设置失败 + */ +LBOT_API bool lbot_set_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + const lbot_position_t* position, const lbot_euler_t* euler); + +/** + * @brief 获取工作坐标系参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工作坐标系名称 + * @param position 返回的工作坐标系位置偏移 + * @param euler 返回的工作坐标系欧拉角偏移 + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + lbot_position_t* position, lbot_euler_t* euler); + +/** + * @brief 切换当前工作坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要切换到的工作坐标系名称 + * @return true 切换成功，false 切换失败 + */ +LBOT_API bool lbot_change_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + +/** + * @brief 删除工作坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要删除的工作坐标系名称 + * @return true 删除成功，false 删除失败 + */ +LBOT_API bool lbot_delete_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + +/** + * @brief 获取所有工作坐标系名称 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param names 返回的工作坐标系名称数组（需要调用lbot_free_string_array释放） + * @param count 返回的工作坐标系数量 + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_all_work_frames(lbot_handle_t *handle, lbot_arm_t arm, char*** names, int* count); + +// ============================================== +// 系统功能函数 +// ============================================== +/** + * @brief 重新标定电机零位，设置当前位置为零位 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @return true 设置成功，false 设置失败 + */ +LBOT_API bool lbot_set_zero(lbot_handle_t *handle, lbot_arm_t arm); + +/** + * @brief 使能/掉使能机械臂 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param enable true 使能，false 掉使能 + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_enable_arm(lbot_handle_t *handle, lbot_arm_t arm, bool enable); + +/** + * @brief 紧急停止/恢复机械臂运行 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param enable true 紧急停止，false 恢复运行 + * @return true 指令发送成功，false 发送失败 + */ +LBOT_API bool lbot_emergency_stop(lbot_handle_t *handle, lbot_arm_t arm, bool enable); + +/** + * @brief 清除所有错误 + * @param handle 机械臂句柄 + * @return true 清除成功，false 清除失败 + */ +LBOT_API bool lbot_clear_errors(lbot_handle_t *handle); + +/** + * @brief 设置机械臂关节限位 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param upper_joint_limit 最大关节限位参数 + * @param lower_joint_limit 最小关节限位参数 + * @return true 设置成功，false 设置失败 + */ +LBOT_API bool lbot_set_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, const double upper_joint_limit[7], const double lower_joint_limit[7]); +/** + * @brief 获取机械臂关节限位参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param upper_joint_limit 返回的最大关节限位参数 + * @param lower_joint_limit 返回的最小关节限位参数 + * @return true 获取成功，false 获取失败 + */ +LBOT_API bool lbot_get_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]); +/** + * @brief 恢复默认关节限位参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param upper_joint_limit 返回的默认最大关节限位参数 + * @param lower_joint_limit 返回的默认最小关节限位参数 + * @return true 恢复成功，false 恢复失败 + */ +LBOT_API bool lbot_get_default_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]); +// ============================================== +// 内存管理辅助函数 +// ============================================== +/** + * @brief 释放字符串数组内存 + * @param array 要释放的字符串数组 + * @param count 数组元素数量 + */ +LBOT_API void lbot_free_string_array(char** array, int count); + +// ============================================== +// 工具函数 +// ============================================== +/** + * @brief 获取最后一次错误信息 + * @return 错误信息字符串指针 + */ +LBOT_API const char* lbot_get_last_error(lbot_handle_t *handle); + +/** + * @brief 设置日志级别 + * @param level 日志级别：0-ERROR, 1-WARN, 2-INFO, 3-DEBUG + */ +LBOT_API void lbot_set_log_level(int level); + +#ifdef __cplusplus +} +#endif + +#endif // LBOT_API_H \ No newline at end of file diff --git a/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api_cpp.h b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api_cpp.h new file mode 100644 index 0000000..20c46ea --- /dev/null +++ b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_api_cpp.h @@ -0,0 +1,660 @@ +/** + * @file lbot_api_cpp.h + * @brief 灵心巧手机械臂控制API C++封装头文件 + * @date 2026.1.19 + * @copyright 灵心巧手科技有限公司 + */ +#ifndef LBOT_API_CPP_H +#define LBOT_API_CPP_H + +#include "lbot_api.h" +#include +#include +#include + +/** + * @brief 灵心巧手机械臂控制API C++封装类 + * @details 提供C++友好的接口封装，使用std::string和std::vector简化内存管理 + */ +namespace lbot { + +class LbotApi { +public: + // ============================================== + // 构造函数和析构函数 + // ============================================== + /** + * @brief 构造函数 + */ + LbotApi(); + + /** + * @brief 析构函数，自动清理资源 + */ + ~LbotApi(); + + // ============================================== + // API初始化和清理函数 + // ============================================== + /** + * @brief 初始化LBot API连接 + * @param tcp_host TCP服务器地址格式："192.168.10.21" + * @return lbot_handle_t 连接句柄，连接成功句柄ID>0，失败返回NULL + */ + LBOT_API lbot_handle_t *lbot_init(const char* tcp_host); + + /** + * @brief 断开指定连接 + * @param handle 机械臂句柄 + * @return true 断开成功，false 断开失败 + */ + LBOT_API bool lbot_disconnect(lbot_handle_t *handle); + + /** + * @brief 清理API资源，断开连接 + */ + LBOT_API void lbot_cleanup(); + + /** + * @brief 获取API版本信息 + * @return 版本字符串 + */ + LBOT_API std::string lbot_get_api_version(); + + // ============================================== + // 系统信息获取函数 + // ============================================== + /** + * @brief 获取控制器信息 + * @param handle 机械臂句柄 + * @param robot_model 返回的机器人型号字符串 + * @param controller_version 返回的控制器版本字符串 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_controller_info(lbot_handle_t *handle, std::string& robot_model, std::string& controller_version); + + // ============================================== + // 状态监控和管理函数 + // ============================================== + /** + * @brief 启动状态监控 + * @param state_cb 状态回调函数，当机器人状态更新时调用 + * @param error_cb 错误回调函数，当发生错误时调用 + * @return true 启动成功，false 启动失败 + */ + LBOT_API bool lbot_start_state_monitor(lbot_state_callback_t state_cb, lbot_error_callback_t error_cb); + + /** + * @brief 停止状态监控 + */ + LBOT_API void lbot_stop_state_monitor(); + + /** + * @brief 获取当前机器人完整状态，此接口需要在启动状态监控后才能调用 + * @param handle 机械臂句柄 + * @param state 返回的机器人状态结构体指针 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_current_state(lbot_handle_t *handle, lbot_full_state_t* state); + + // ============================================== + // 运动控制函数 + // ============================================== + /** + * @brief 关节空间运动 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 7个关节的目标角度（弧度） + * @param speed 运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_move_joint(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], double speed, double accel, bool block); + + /** + * @brief 关节空间运动（使用vector） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 7个关节的目标角度（弧度）向量 + * @param speed 运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_move_joint(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& joints, double speed, double accel, bool block); + + /** + * @brief 笛卡尔空间姿态运动（关节插值） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 目标位置（x, y, z，单位：米） + * @param euler 机械臂末端目标欧拉角（roll, pitch, yaw，单位：弧度） + * @param speed 机械臂末端运动速度（0.0~20.0）单位m/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 机械臂末端加速度（0.0~20.0）单位m/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_move_pose(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, + double speed, double accel, bool block); + + /** + * @brief 笛卡尔空间姿态运动（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 目标位置（x, y, z，单位：米）向量 + * @param euler 机械臂末端目标欧拉角（roll, pitch, yaw，单位：弧度）向量 + * @param speed 机械臂末端运动速度（0.0~20.0）单位m/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 机械臂末端加速度（0.0~20.0）单位m/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_move_pose(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& position, const std::vector& euler, + double speed, double accel, bool block); + + /** + * @brief 笛卡尔空间直线运动（直线插值） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 目标位置（x, y, z，单位：米） + * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度） + * @param speed 关节运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 关节运动加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_move_linear(lbot_handle_t *handle, lbot_arm_t arm, const lbot_position_t* position, const lbot_euler_t* euler, + double speed, double accel, bool block); + + /** + * @brief 笛卡尔空间直线运动（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 目标位置（x, y, z，单位：米）向量 + * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）向量 + * @param speed 关节运动速度（0.0~20.0）单位是rad/s, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param accel 关节运动加速度（0.0~20.0）单位是rad/s^2, 建议从（0.0~2.0）开始使用后续如有需要逐步提高 + * @param block 是否阻塞执行：true 等待运动完成，false 立即返回 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_move_linear(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& position, const std::vector& euler, + double speed, double accel, bool block); + + // ============================================== + // 关节跟随函数（用于遥操作） + // ============================================== + /** + * @brief 关节跟随控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 包含7个关节目标角度（弧度）的向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_joint_follow(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& joints); + + // ============================================== + // 姿态跟随函数 + // ============================================== + /** + * @brief 笛卡尔空间姿态跟随运动 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param pos 目标位置（x, y, z，单位：米） + * @param eul 目标欧拉角（roll, pitch, yaw，单位：弧度） + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_pose_follow(lbot_handle_t *handle, lbot_arm_t arm, lbot_position_t pos, lbot_euler_t eul); + + // ============================================== + // L6手控制接口 + // ============================================== + /** + * @brief 设置L6手的位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 6个手指的目标位置（0~255）向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l6_set_position(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& position); + + /** + * @brief 设置L6手的速度控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param velocity 6个手指的目标速度（0~255）向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l6_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& velocity); + + /** + * @brief 设置L6手的力矩控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param torque 6个手指的目标力矩（0~255）向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l6_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& torque); + + // ============================================== + // L10手控制接口（10个自由度） + // ============================================== + /** + * @brief 设置L10手的位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 10个手指的目标位置（0~255）向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l10_set_position(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& position); + + /** + * @brief 设置L10手的速度控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param velocity 10个手指的目标速度（0~255）向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l10_set_velocity(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& velocity); + + /** + * @brief 设置L10手的力矩控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param torque 10个手指的目标力矩（0~255）向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l10_set_effort(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& torque); + + // ============================================== + // L20手控制接口（20个自由度） + // ============================================== + /** + * @brief 设置R20手的串联位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param cmd 目标位置命令，包含手指标识和6个电机位置 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l20_set_series_position(lbot_handle_t *handle, lbot_arm_t arm, const lbot_l20_series_cmd_t* cmd); + + /** + * @brief 设置R20手所有自由度位置控制 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param position 16个自由度的目标位置（度）向量 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_l20_set_all_position(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& position); + + // ============================================== + // 运动学计算函数 + // ============================================== + /** + * @brief 正运动学计算 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 7个关节角度（弧度） + * @param position 返回的末端位置（x, y, z，单位：米） + * @param euler 返回的末端欧拉角（roll, pitch, yaw，单位：弧度） + * @return true 计算成功，false 计算失败 + */ + LBOT_API bool lbot_forward_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double joints[7], + lbot_position_t* position, lbot_euler_t* euler); + + /** + * @brief 正运动学计算（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param joints 7个关节角度（弧度）向量 + * @param position 返回的末端位置（x, y, z，单位：米）向量 + * @param euler 返回的末端欧拉角（roll, pitch, yaw，单位：弧度）向量 + * @return true 计算成功，false 计算失败 + */ + LBOT_API bool lbot_forward_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& joints, + std::vector& position, std::vector& euler); + + /** + * @brief 逆运动学计算 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param initial_joints 初始关节角度（弧度），用于求解器迭代 + * @param position 目标位置（x, y, z，单位：米） + * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度） + * @param result_joints 返回的7个关节角度解（弧度） + * @return true 求解成功，false 求解失败 + */ + LBOT_API bool lbot_inverse_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const double initial_joints[7], + const lbot_position_t* position, const lbot_euler_t* euler, + double result_joints[7]); + + /** + * @brief 逆运动学计算（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param initial_joints 初始关节角度（弧度）向量，用于求解器迭代 + * @param position 目标位置（x, y, z，单位：米）向量 + * @param euler 目标欧拉角（roll, pitch, yaw，单位：弧度）向量 + * @param result_joints 返回的7个关节角度解（弧度）向量 + * @return true 求解成功，false 求解失败 + */ + LBOT_API bool lbot_inverse_kinematics(lbot_handle_t *handle, lbot_arm_t arm, const std::vector& initial_joints, + const std::vector& position, const std::vector& euler, + std::vector& result_joints); + + // ============================================== + // 工具坐标系管理函数 + // ============================================== + /** + * @brief 设置工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工具坐标系名称（最大32字符） + * @param position 工具坐标系相对于法兰盘的位置偏移（x, y, z，单位：米） + * @param euler 工具坐标系相对于法兰盘的欧拉角偏移（roll, pitch, yaw，单位：弧度） + * @return true 设置成功，false 设置失败 + */ + LBOT_API bool lbot_set_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + const lbot_position_t* position, const lbot_euler_t* euler); + + /** + * @brief 设置工具坐标系（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工具坐标系名称（最大32字符） + * @param position 工具坐标系相对于法兰盘的位置偏移（x, y, z，单位：米）向量 + * @param euler 工具坐标系相对于法兰盘的欧拉角偏移（roll, pitch, yaw，单位：弧度）向量 + * @return true 设置成功，false 设置失败 + */ + LBOT_API bool lbot_set_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name, + const std::vector& position, const std::vector& euler); + + /** + * @brief 获取工具坐标系参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工具坐标系名称 + * @param position 返回的工具坐标系位置偏移 + * @param euler 返回的工具坐标系欧拉角偏移 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + lbot_position_t* position, lbot_euler_t* euler); + + /** + * @brief 获取工具坐标系参数（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工具坐标系名称 + * @param position 返回的工具坐标系位置偏移向量 + * @param euler 返回的工具坐标系欧拉角偏移向量 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name, + std::vector& position, std::vector& euler); + + /** + * @brief 获取当前使用的工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 返回的当前工具坐标系名称 + * @param position 返回的当前工具坐标系位置偏移 + * @param euler 返回的当前工具坐标系欧拉角偏移 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_current_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, + std::string& name, + lbot_position_t& position, + lbot_euler_t& euler); + + /** + * @brief 获取当前使用的工具坐标系（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 返回的当前工具坐标系名称 + * @param position 返回的当前工具坐标系位置偏移向量 + * @param euler 返回的当前工具坐标系欧拉角偏移向量 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_current_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, std::string& name, + std::vector& position, + std::vector& euler); + + /** + * @brief 切换当前工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要切换到的工具坐标系名称 + * @return true 切换成功，false 切换失败 + */ + LBOT_API bool lbot_change_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + + /** + * @brief 切换当前工具坐标系（使用string） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要切换到的工具坐标系名称 + * @return true 切换成功，false 切换失败 + */ + LBOT_API bool lbot_change_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name); + + /** + * @brief 删除工具坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要删除的工具坐标系名称 + * @return true 删除成功，false 删除失败 + */ + LBOT_API bool lbot_delete_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + + /** + * @brief 删除工具坐标系（使用string） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要删除的工具坐标系名称 + * @return true 删除成功，false 删除失败 + */ + LBOT_API bool lbot_delete_tool_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name); + + /** + * @brief 获取所有工具坐标系名称 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param names 返回的工具坐标系名称向量 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_all_tool_frames(lbot_handle_t *handle, lbot_arm_t arm, std::vector& names); + + // ============================================== + // 工作坐标系管理函数 + // ============================================== + /** + * @brief 设置工作坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工作坐标系名称（最大32字符） + * @param position 工作坐标系相对于基坐标系的位置偏移（x, y, z，单位：米） + * @param euler 工作坐标系相对于基坐标系的欧拉角偏移（roll, pitch, yaw，单位：弧度） + * @return true 设置成功，false 设置失败 + */ + LBOT_API bool lbot_set_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + const lbot_position_t* position, const lbot_euler_t* euler); + + /** + * @brief 设置工作坐标系（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工作坐标系名称（最大32字符） + * @param position 工作坐标系相对于基坐标系的位置偏移（x, y, z，单位：米）向量 + * @param euler 工作坐标系相对于基坐标系的欧拉角偏移（roll, pitch, yaw，单位：弧度）向量 + * @return true 设置成功，false 设置失败 + */ + LBOT_API bool lbot_set_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name, + const std::vector& position, const std::vector& euler); + + /** + * @brief 获取工作坐标系参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工作坐标系名称 + * @param position 返回的工作坐标系位置偏移 + * @param euler 返回的工作坐标系欧拉角偏移 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name, + lbot_position_t* position, lbot_euler_t* euler); + + /** + * @brief 获取工作坐标系参数（使用向量） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 工作坐标系名称 + * @param position 返回的工作坐标系位置偏移向量 + * @param euler 返回的工作坐标系欧拉角偏移向量 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name, + std::vector& position, std::vector& euler); + + /** + * @brief 切换当前工作坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要切换到的工作坐标系名称 + * @return true 切换成功，false 切换失败 + */ + LBOT_API bool lbot_change_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + + /** + * @brief 切换当前工作坐标系（使用string） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要切换到的工作坐标系名称 + * @return true 切换成功，false 切换失败 + */ + LBOT_API bool lbot_change_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name); + + /** + * @brief 删除工作坐标系 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要删除的工作坐标系名称 + * @return true 删除成功，false 删除失败 + */ + LBOT_API bool lbot_delete_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const char* name); + + /** + * @brief 删除工作坐标系（使用string） + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param name 要删除的工作坐标系名称 + * @return true 删除成功，false 删除失败 + */ + LBOT_API bool lbot_delete_work_frame(lbot_handle_t *handle, lbot_arm_t arm, const std::string& name); + + /** + * @brief 获取所有工作坐标系名称 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param names 返回的工作坐标系名称向量 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_all_work_frames(lbot_handle_t *handle, lbot_arm_t arm, std::vector& names); + + // ============================================== + // 系统功能函数 + // ============================================== + /** + * @brief 重新标定电机零位，设置当前位置为零位 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @return true 设置成功，false 设置失败 + */ + LBOT_API bool lbot_set_zero(lbot_handle_t *handle, lbot_arm_t arm); + + /** + * @brief 使能/掉使能机械臂 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param enable true 使能，false 掉使能 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_enable_arm(lbot_handle_t *handle, lbot_arm_t arm, bool enable); + + /** + * @brief 紧急停止/恢复 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param enable true 紧急停止，false 恢复运行 + * @return true 指令发送成功，false 发送失败 + */ + LBOT_API bool lbot_emergency_stop(lbot_handle_t *handle, lbot_arm_t arm, bool enable); + + /** + * @brief 清除所有错误 + * @param handle 机械臂句柄 + * @return true 清除成功，false 清除失败 + */ + LBOT_API bool lbot_clear_errors(lbot_handle_t *handle); + + /** + * @brief 设置机械臂关节限位 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param upper_joint_limit 最大关节限位参数 + * @param lower_joint_limit 最小关节限位参数 + * @return true 设置成功，false 设置失败 + */ + LBOT_API bool lbot_set_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, const double upper_joint_limit[7], const double lower_joint_limit[7]); + /** + * @brief 获取机械臂关节限位参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param upper_joint_limit 返回的最大关节限位参数 + * @param lower_joint_limit 返回的最小关节限位参数 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]); + /** + * @brief 获取机械臂默认关节限位参数 + * @param handle 机械臂句柄 + * @param arm 机械臂选择：LBOT_LEFT_ARM 或 LBOT_RIGHT_ARM + * @param upper_joint_limit 返回的最大关节限位参数 + * @param lower_joint_limit 返回的最小关节限位参数 + * @return true 获取成功，false 获取失败 + */ + LBOT_API bool lbot_get_default_joint_limit(lbot_handle_t *handle, lbot_arm_t arm, double upper_joint_limit[7], double lower_joint_limit[7]); + + // ============================================== + // 工具函数 + // ============================================== + /** + * @brief 获取最后一次错误信息 + * @return 错误信息字符串 + */ + LBOT_API std::string lbot_get_last_error(lbot_handle_t *handle); + + /** + * @brief 设置日志级别 + * @param level 日志级别：0-ERROR, 1-WARN, 2-INFO, 3-DEBUG + */ + LBOT_API void lbot_set_log_level(int level); + + // ============================================== + // 内存管理辅助函数 + // ============================================== + /** + * @brief 释放字符串数组内存 + * @param array 要释放的字符串数组 + * @param count 数组元素数量 + */ + LBOT_API void lbot_free_string_array(char** array, int count); + +private: + // 禁用拷贝构造和赋值操作 + LbotApi(const LbotApi&) = delete; + LbotApi& operator=(const LbotApi&) = delete; +}; + +} // namespace lbot + +#endif // LBOT_API_CPP_H \ No newline at end of file diff --git a/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h new file mode 100644 index 0000000..2aeb98d --- /dev/null +++ b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h @@ -0,0 +1,431 @@ +// Copyright (c) 2025 LingSmart Tech +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef LBOT_DRIVER_H +#define LBOT_DRIVER_H + +#include +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/clock.hpp" +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "lbot_api_cpp.h" + +// ROS2 标准消息类型 +#include +#include +#include +#include +#include +#include +#include +#include "std_msgs/msg/u_int8_multi_array.hpp" +#include "std_msgs/msg/int32_multi_array.hpp" + +// 自定义 Message 类型 +#include "lbot_arm_interfaces/msg/arm_state.hpp" +#include "lbot_arm_interfaces/msg/lbot_pose.hpp" +#include "lbot_arm_interfaces/msg/lbot_frame.hpp" +#include "lbot_arm_interfaces/msg/follow_joint.hpp" + +// 自定义 Service 类型 +#include "lbot_arm_interfaces/srv/change_frame.hpp" +#include "lbot_arm_interfaces/srv/delete_frame.hpp" +#include "lbot_arm_interfaces/srv/forward_kinematics.hpp" +#include "lbot_arm_interfaces/srv/inverse_kinematics.hpp" +#include "lbot_arm_interfaces/srv/move_c.hpp" +#include "lbot_arm_interfaces/srv/move_j.hpp" +#include "lbot_arm_interfaces/srv/move_jp.hpp" +#include "lbot_arm_interfaces/srv/move_l.hpp" +#include "lbot_arm_interfaces/srv/set_frame.hpp" +#include "lbot_arm_interfaces/srv/set_string.hpp" +#include "lbot_arm_interfaces/srv/set_zero.hpp" +#include "lbot_arm_interfaces/srv/set_enable.hpp" +#include "lbot_arm_interfaces/srv/set_emergency.hpp" +#include "lbot_arm_interfaces/srv/get_frame.hpp" +#include "lbot_arm_interfaces/srv/get_current_frame.hpp" +#include "lbot_arm_interfaces/srv/get_all_frames.hpp" + +#define RAD_DEGREE 57.295791433 +#define DEGREE_RAD 0.01745329252 + +using namespace std::chrono_literals; + +// 状态回调函数 +void lbot_state_callback_wrapper(const lbot_full_state_t* state); +void lbot_error_callback_wrapper(int error_code, const char* error_msg); + +// 全局连接状态枚举 +enum class GlobalConnState { + DISCONNECTED, + CONNECTING, + CONNECTED +}; + +// 全局变量 +extern bool lbot_ctrl_flag; +extern lbot::LbotApi lbot_api; +extern lbot_handle_t *lbot_handle; +extern std::atomic g_conn_state; // 全局连接状态 + +namespace lbot_driver { + +// 主节点 - 负责连接管理、状态发布、心跳重连、关节跟随 +class LBot: public rclcpp::Node +{ +public: + LBot(const std::string& node_name = "lbot_main_node"); + ~LBot(); + + // 线程安全的状态缓存 + std::mutex state_mutex_, conn_mutex_; + lbot_full_state_t current_lbot_state_; + // 节点关闭状态变量 + std::atomic shutting_down_{false}; + // 单例类指针 + static LBot* g_instance; + + // 重连机制相关线程与变量 + std::thread reconnect_thread_; + std::atomic reconnect_thread_running_{false}; + std::mutex reconnect_thread_mutex_; + + void disconnect_robot(); + +private: + // 初始化和连接相关 + bool connect_robot(); + void get_robot_info(); + void state_publish_timer_callback(); + + // 心跳与重连机制回调函数 + void heartbeat_timer_callback(); + void reconnect_timer_callback(); + + // 关节跟随订阅回调函数（移到主节点，避免阻塞） + void left_joint_follow_callback(const lbot_arm_interfaces::msg::FollowJoint::SharedPtr msg); + void right_joint_follow_callback(const lbot_arm_interfaces::msg::FollowJoint::SharedPtr msg); + + /****************************** 发布器 ******************************/ + + // 状态发布器 (50Hz) + rclcpp::Publisher::SharedPtr left_joint_pub_; + rclcpp::Publisher::SharedPtr right_joint_pub_; + rclcpp::Publisher::SharedPtr left_pose_pub_; + rclcpp::Publisher::SharedPtr right_pose_pub_; + + /****************************** 订阅器 ******************************/ + + // 关节跟随订阅器 (用于遥操作) - 移到主节点 + rclcpp::Subscription::SharedPtr left_joint_follow_sub_; + rclcpp::Subscription::SharedPtr right_joint_follow_sub_; + + /****************************** 定时器 ******************************/ + + // 状态发布定时器 (50Hz) + rclcpp::TimerBase::SharedPtr state_publish_timer_; + rclcpp::TimerBase::SharedPtr heartbeat_timer_; + rclcpp::TimerBase::SharedPtr reconnect_timer_; + + // 参数 + std::string arm_ip_ = "192.168.10.21"; + + // 回调组 + rclcpp::CallbackGroup::SharedPtr callback_group_timer_; + rclcpp::CallbackGroup::SharedPtr callback_group_subscribers_; + + // 连接状态 + bool is_state_monitor_started_ = false; + + GlobalConnState conn_state_ = GlobalConnState::DISCONNECTED; + + // 心跳计数器（连续失败次数） + int heartbeat_fail_count_ = 0; +}; + +// 左臂服务节点 - 负责左臂所有服务 +class LeftArmServiceNode : public rclcpp::Node +{ +public: + LeftArmServiceNode(const std::string& node_name = "lbot_left_arm_node"); + ~LeftArmServiceNode() = default; + +private: + rclcpp::CallbackGroup::SharedPtr callback_group_, callback_group_subscribers_; + + void create_services(); + + // 灵巧手设置回调函数 + void left_hand_l6_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void left_hand_l6_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void left_hand_l6_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void left_hand_l10_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void left_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void left_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void left_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg); + void left_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg); + + /****************************** 连接状态检查函数 ******************************/ + + bool check_connection_state(const std::string& service_name) { + if (g_conn_state.load() != GlobalConnState::CONNECTED) { + RCLCPP_WARN(this->get_logger(), "%s: Robot not connected, service rejected", service_name.c_str()); + return false; + } + return true; + } + + /****************************** 左臂服务回调函数 ******************************/ + + // 运动控制 + void move_joint_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void move_pose_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void move_linear_callback( + const std::shared_ptr request, + std::shared_ptr response); + + // 运动学计算 + void forward_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void inverse_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response); + + // 工具坐标系管理 + void set_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void get_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void get_current_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void change_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void delete_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void get_all_tool_frames_callback( + const std::shared_ptr request, + std::shared_ptr response); + + // 系统设置 + void set_zero_callback( + const std::shared_ptr request, + std::shared_ptr response); + void set_enable_callback( + const std::shared_ptr request, + std::shared_ptr response); + void set_emergency_callback( + const std::shared_ptr request, + std::shared_ptr response); + + /****************************** 服务器 ******************************/ + + // 运动控制服务器 + rclcpp::Service::SharedPtr move_joint_service_; + rclcpp::Service::SharedPtr move_pose_service_; + rclcpp::Service::SharedPtr move_linear_service_; + + // 运动学计算服务器 + rclcpp::Service::SharedPtr forward_kinematics_service_; + rclcpp::Service::SharedPtr inverse_kinematics_service_; + + // 工具坐标系管理服务器 + rclcpp::Service::SharedPtr set_tool_frame_service_; + rclcpp::Service::SharedPtr get_tool_frame_service_; + rclcpp::Service::SharedPtr get_current_tool_frame_service_; + rclcpp::Service::SharedPtr change_tool_frame_service_; + rclcpp::Service::SharedPtr delete_tool_frame_service_; + rclcpp::Service::SharedPtr get_all_tool_frames_service_; + + // 系统设置服务器 + rclcpp::Service::SharedPtr set_zero_service_; + rclcpp::Service::SharedPtr set_enable_service_; + rclcpp::Service::SharedPtr set_emergency_service_; + + /****************************** 灵巧手Topic ******************************/ + + rclcpp::Subscription::SharedPtr left_hand_l6_joint_sub_; + rclcpp::Subscription::SharedPtr left_hand_l6_force_sub_; + rclcpp::Subscription::SharedPtr left_hand_l6_speed_sub_; + rclcpp::Subscription::SharedPtr left_hand_l10_joint_sub_; + rclcpp::Subscription::SharedPtr left_hand_l10_force_sub_; + rclcpp::Subscription::SharedPtr left_hand_l10_speed_sub_; + rclcpp::Subscription::SharedPtr left_hand_l20_joint_sub_; + rclcpp::Subscription::SharedPtr left_hand_l20_series_position_sub_; +}; + +// 右臂服务节点 - 负责右臂所有服务 +class RightArmServiceNode : public rclcpp::Node +{ +public: + RightArmServiceNode(const std::string& node_name = "lbot_right_arm_node"); + ~RightArmServiceNode() = default; + +private: + rclcpp::CallbackGroup::SharedPtr callback_group_, callback_group_subscribers_; + + void create_services(); + + // 灵巧手设置回调函数 + void right_hand_l6_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void right_hand_l6_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void right_hand_l6_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void right_hand_l10_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void right_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void right_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg); + void right_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg); + void right_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg); + + /****************************** 连接状态检查函数 ******************************/ + + bool check_connection_state(const std::string& service_name) { + if (g_conn_state.load() != GlobalConnState::CONNECTED) { + RCLCPP_WARN(this->get_logger(), "%s: Robot not connected, service rejected", service_name.c_str()); + return false; + } + return true; + } + + /****************************** 右臂服务回调函数 ******************************/ + + // 运动控制 + void move_joint_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void move_pose_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void move_linear_callback( + const std::shared_ptr request, + std::shared_ptr response); + + // 运动学计算 + void forward_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void inverse_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response); + + // 工具坐标系管理 + void set_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void get_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void get_current_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void change_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void delete_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response); + + void get_all_tool_frames_callback( + const std::shared_ptr request, + std::shared_ptr response); + + // 系统设置 + void set_zero_callback( + const std::shared_ptr request, + std::shared_ptr response); + void set_enable_callback( + const std::shared_ptr request, + std::shared_ptr response); + void set_emergency_callback( + const std::shared_ptr request, + std::shared_ptr response); + + /****************************** 服务器 ******************************/ + + // 运动控制服务器 + rclcpp::Service::SharedPtr move_joint_service_; + rclcpp::Service::SharedPtr move_pose_service_; + rclcpp::Service::SharedPtr move_linear_service_; + + // 运动学计算服务器 + rclcpp::Service::SharedPtr forward_kinematics_service_; + rclcpp::Service::SharedPtr inverse_kinematics_service_; + + // 工具坐标系管理服务器 + rclcpp::Service::SharedPtr set_tool_frame_service_; + rclcpp::Service::SharedPtr get_tool_frame_service_; + rclcpp::Service::SharedPtr get_current_tool_frame_service_; + rclcpp::Service::SharedPtr change_tool_frame_service_; + rclcpp::Service::SharedPtr delete_tool_frame_service_; + rclcpp::Service::SharedPtr get_all_tool_frames_service_; + + // 系统设置服务器 + rclcpp::Service::SharedPtr set_zero_service_; + rclcpp::Service::SharedPtr set_enable_service_; + rclcpp::Service::SharedPtr set_emergency_service_; + + /****************************** 灵巧手Topic ******************************/ + + rclcpp::Subscription::SharedPtr right_hand_l6_joint_sub_; + rclcpp::Subscription::SharedPtr right_hand_l6_force_sub_; + rclcpp::Subscription::SharedPtr right_hand_l6_speed_sub_; + rclcpp::Subscription::SharedPtr right_hand_l10_joint_sub_; + rclcpp::Subscription::SharedPtr right_hand_l10_force_sub_; + rclcpp::Subscription::SharedPtr right_hand_l10_speed_sub_; + rclcpp::Subscription::SharedPtr right_hand_l20_joint_sub_; + rclcpp::Subscription::SharedPtr right_hand_l20_series_position_sub_; +}; + +} // namespace lbot_driver + +#endif // LBOT_DRIVER_H diff --git a/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_types.h b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_types.h new file mode 100644 index 0000000..219b921 --- /dev/null +++ b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_types.h @@ -0,0 +1,99 @@ +/** + * @file lbot_types.h + * @brief 该文件定义了结构和枚举定义 + * @date 2026.1.19 + * @copyright 灵心巧手科技有限公司 + */ +#ifndef LBOT_TYPES_H +#define LBOT_TYPES_H + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + + +// 机械臂类型枚举 +typedef enum { + LBOT_LEFT_ARM = 0, + LBOT_RIGHT_ARM = 1 +} lbot_arm_t; + +// 机械臂控制句柄 +typedef struct { + uint64_t id; // 句柄ID，连接成功返回>0的值，0表示无效句柄 +}lbot_handle_t; + +// 运动类型枚举 +typedef enum { + LBOT_MOVE_JOINT = 0, // 关节空间运动 + LBOT_MOVE_POSE = 1, // 笛卡尔空间点到点 + LBOT_MOVE_LINEAR = 2 // 笛卡尔空间直线运动 +} lbot_move_type_t; + +// 坐标系结构体 +typedef struct { + double x, y, z; +} lbot_position_t; + +typedef struct { + double x, y, z, w; +} lbot_orientation_t; + +typedef struct { + double x, y, z; +} lbot_euler_t; + +// 关节状态结构体 +typedef struct { + // 关节数据 + char name[7][32]; // 7个关节名称 + double joint_position[7]; // 7个关节位置 + double velocity[7]; // 7个关节速度 + double effort[7]; // 7个关节力矩 + double temperature[7]; // 7个关节温度 + + // 时间戳 + int32_t sec; // 秒 + uint32_t nanosec; // 纳秒 + char frame_id[64]; // 工作坐标系 + + // 末端状态 + lbot_position_t end_effector_position; // 末端位置 + lbot_euler_t euler; // 欧拉角 + lbot_orientation_t orientation; // 四元数姿态 +} lbot_arm_state_t; + +// 机械臂完整状态结构体 +typedef struct { + lbot_arm_state_t left_arm; + lbot_arm_state_t right_arm; + uint64_t system_timestamp; // 系统时间戳（纳秒） + char arm_ip[16]; // 机械臂IP地址 +} lbot_full_state_t; + +// 回调函数类型定义 +typedef void (*lbot_state_callback_t)(const lbot_full_state_t* state); +typedef void (*lbot_error_callback_t)(int error_code, const char* error_msg); + + +typedef enum { + LBOT_FINGER_THUMB = 0, // 大拇指 + LBOT_FINGER_INDEX = 1, // 食指 + LBOT_FINGER_MID = 2, // 中指 + LBOT_FINGER_RING = 3, // 无名指 + LBOT_FINGER_LITTLE = 4 // 小指 +} lbot_finger_type_t; + +typedef struct { + uint8_t data[6]; // 单根手指各电机位置（指根、指尖、侧摆、旋转） + lbot_finger_type_t finger; // 手指标识，0~4表示thumb,index,mid,ring,little +} lbot_l20_series_cmd_t; + +#ifdef __cplusplus +} +#endif + +#endif // LBOT_TYPES_H diff --git a/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_version.h b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_version.h new file mode 100644 index 0000000..fbeb086 --- /dev/null +++ b/src/officer_sdk/lbot_driver/include/lbot_driver/lbot_version.h @@ -0,0 +1,23 @@ +/** + * @file lbot_version.h + * @brief 该文件指定API版本号 + * @date 2026.1.19 + * @copyright 灵心巧手科技有限公司 + */ +#ifndef LBOT_VERSION_H +#define LBOT_VERSION_H + +#ifdef __cplusplus +extern "C" { +#endif + +#define SDK_VERSION ("1.0.5") +#define SDK_BUILD_TIME ("2026.4.30") + + +#ifdef __cplusplus +} +#endif + +#endif + \ No newline at end of file diff --git a/src/officer_sdk/lbot_driver/launch/lbot_start_driver.launch.py b/src/officer_sdk/lbot_driver/launch/lbot_start_driver.launch.py new file mode 100755 index 0000000..645d675 --- /dev/null +++ b/src/officer_sdk/lbot_driver/launch/lbot_start_driver.launch.py @@ -0,0 +1,41 @@ +import launch +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + # YAML 默认模板文件 + base_yaml_file = os.path.join( + get_package_share_directory('lbot_driver'), + 'config', 'lbot_config.yaml' + ) + + # 定义机器人列表，每个机器人名字和 IP + robots = [ + {"name": "robot1", "arm_ip": "192.168.10.21"}, + # {"name": "robot2", "arm_ip": "192.168.10.22"}, + ] + + nodes = [] + + for robot in robots: + # 每个机器人只需要启动一个 lbot_driver 可执行文件 + # 这个可执行文件内部会创建三个节点：主节点、左臂服务节点、右臂服务节点 + driver_node = Node( + package='lbot_driver', + executable='lbot_driver', + namespace=robot["name"], # 设置 namespace + parameters=[ + base_yaml_file, # 默认 YAML 文件 + { # 覆盖参数 + "arm_ip": robot["arm_ip"] + } + ], + output='screen', + emulate_tty=True, # 更好的日志输出格式 + ) + nodes.append(driver_node) + + return LaunchDescription(nodes) diff --git a/src/officer_sdk/lbot_driver/lib/lib_install.sh b/src/officer_sdk/lbot_driver/lib/lib_install.sh new file mode 100755 index 0000000..2a908fc --- /dev/null +++ b/src/officer_sdk/lbot_driver/lib/lib_install.sh @@ -0,0 +1,36 @@ +#!/bin/bash +set -e + +# 功能包 lib 目录（脚本所在目录就是 lib） +LIB_DIR="$(cd "$(dirname "$0")" && pwd)" + +echo "Target lib directory: $LIB_DIR" + +# 根据系统架构选择对应文件夹 +ARCH=$(uname -m) +if [ "$ARCH" = "x86_64" ]; then + SRC_DIR="$LIB_DIR/linux_x64" +elif [ "$ARCH" = "aarch64" ] || [ "$ARCH" = "arm64" ]; then + SRC_DIR="$LIB_DIR/linux_arm64" +else + echo "Unsupported architecture: $ARCH" + exit 1 +fi + +TARGET_SO="$LIB_DIR/liblbot_api_cpp.so.1.0.0" + +echo "Using source directory: $SRC_DIR" +echo "Removing old files..." +rm -f "$LIB_DIR/liblbot_api_cpp.so" \ + "$LIB_DIR/liblbot_api_cpp.so.1" \ + "$TARGET_SO" + +echo "Copying..." +cp "$SRC_DIR/liblbot_api_cpp.so" "$TARGET_SO" + +echo "Creating symlinks..." +ln -s "liblbot_api_cpp.so.1.0.0" "$LIB_DIR/liblbot_api_cpp.so.1" +ln -s "liblbot_api_cpp.so.1.0.0" "$LIB_DIR/liblbot_api_cpp.so" + +echo "[SUCCESS] Installed." + diff --git a/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so b/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so new file mode 120000 index 0000000..df3db5a --- /dev/null +++ b/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so @@ -0,0 +1 @@ +liblbot_api_cpp.so.1.0.0 \ No newline at end of file diff --git a/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so.1 b/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so.1 new file mode 120000 index 0000000..df3db5a --- /dev/null +++ b/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so.1 @@ -0,0 +1 @@ +liblbot_api_cpp.so.1.0.0 \ No newline at end of file diff --git a/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so.1.0.0 b/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so.1.0.0 new file mode 100755 index 0000000..0b8870e Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/liblbot_api_cpp.so.1.0.0 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so new file mode 100755 index 0000000..bc6ec60 Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so.1 b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so.1 new file mode 100755 index 0000000..bc6ec60 Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so.1 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so.1.0.5 b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so.1.0.5 new file mode 100755 index 0000000..bc6ec60 Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api.so.1.0.5 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so new file mode 100755 index 0000000..32a085c Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so.1 b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so.1 new file mode 100755 index 0000000..32a085c Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so.1 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so.1.0.5 b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so.1.0.5 new file mode 100755 index 0000000..32a085c Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_arm64/liblbot_api_cpp.so.1.0.5 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so new file mode 100755 index 0000000..d0ed1c1 Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so.1 b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so.1 new file mode 100755 index 0000000..d0ed1c1 Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so.1 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so.1.0.5 b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so.1.0.5 new file mode 100755 index 0000000..d0ed1c1 Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api.so.1.0.5 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so new file mode 100755 index 0000000..0b8870e Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so.1 b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so.1 new file mode 100755 index 0000000..0b8870e Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so.1 differ diff --git a/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so.1.0.5 b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so.1.0.5 new file mode 100755 index 0000000..0b8870e Binary files /dev/null and b/src/officer_sdk/lbot_driver/lib/linux_x64/liblbot_api_cpp.so.1.0.5 differ diff --git a/src/officer_sdk/lbot_driver/package.xml b/src/officer_sdk/lbot_driver/package.xml new file mode 100644 index 0000000..ccfb6f5 --- /dev/null +++ b/src/officer_sdk/lbot_driver/package.xml @@ -0,0 +1,26 @@ + + + + lbot_driver + 0.0.0 + TODO: Package description + wxp + TODO: License declaration + + ament_cmake + + rclcpp + std_msgs + sensor_msgs + geometry_msgs + tf2 + tf2_geometry_msgs + lbot_arm_interfaces + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/officer_sdk/lbot_driver/src/lbot_driver.cpp b/src/officer_sdk/lbot_driver/src/lbot_driver.cpp new file mode 100644 index 0000000..9d6affc --- /dev/null +++ b/src/officer_sdk/lbot_driver/src/lbot_driver.cpp @@ -0,0 +1,1782 @@ +// Copyright (c) 2025 LinkerRobot Tech +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "lbot_driver.h" +#include + +using namespace std::chrono_literals; +using lbot_driver::LBot; + +// 全局变量 +lbot::LbotApi lbot_api; +lbot_handle_t *lbot_handle{}; +LBot* LBot::g_instance = nullptr; +std::atomic g_conn_state{GlobalConnState::DISCONNECTED}; // 全局连接状态 + +// 静态状态回调函数 +void lbot_state_callback_wrapper(const lbot_full_state_t* state) { + if (state && lbot_driver::LBot::g_instance) { + std::lock_guard lock(lbot_driver::LBot::g_instance->state_mutex_); + lbot_driver::LBot::g_instance->current_lbot_state_ = *state; + } +} + +// 错误回调函数 +void lbot_error_callback_wrapper(int error_code, const char* error_msg) { + if (LBot::g_instance && LBot::g_instance->shutting_down_) return; + + if (error_msg) { + RCLCPP_ERROR( + rclcpp::get_logger("lbot_driver"), + "LBot Error [%d]: %s", + error_code, + error_msg + ); + } else { + RCLCPP_ERROR( + rclcpp::get_logger("lbot_driver"), + "LBot Error [%d]: (null message)", + error_code + ); + } +} + +namespace lbot_driver { + +// ========== 主节点实现 ========== +LBot::LBot(const std::string& node_name) : rclcpp::Node(node_name) { + // 设置独特的logger名称 + this->get_logger() = rclcpp::get_logger(node_name); + + // 参数初始化 + this->declare_parameter("arm_ip", "192.168.10.21"); + arm_ip_ = this->get_parameter("arm_ip").as_string(); + + // 全局指针 + g_instance = this; + + // 创建回调组 + callback_group_timer_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_subscribers_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + // 创建发布器 (50Hz状态发布) + left_joint_pub_ = this->create_publisher("left_arm/joint_states", 10); + right_joint_pub_ = this->create_publisher("right_arm/joint_states", 10); + left_pose_pub_ = this->create_publisher("left_arm/pose_states", 10); + right_pose_pub_ = this->create_publisher("right_arm/pose_states", 10); + + // 创建关节跟随订阅器（避免阻塞服务） + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_subscribers_; + + left_joint_follow_sub_ = this->create_subscription( + "left_arm/joint_follow", 10, + std::bind(&LBot::left_joint_follow_callback, this, std::placeholders::_1), + sub_opt); + + right_joint_follow_sub_ = this->create_subscription( + "right_arm/joint_follow", 10, + std::bind(&LBot::right_joint_follow_callback, this, std::placeholders::_1), + sub_opt); + + // 心跳与重连定时器 + heartbeat_timer_ = create_wall_timer(1s, std::bind(&LBot::heartbeat_timer_callback, this)); + reconnect_timer_ = create_wall_timer(2s, std::bind(&LBot::reconnect_timer_callback, this)); + + // 创建状态发布定时器 (50Hz) + state_publish_timer_ = this->create_wall_timer( + 20ms, + std::bind(&LBot::state_publish_timer_callback, this), + callback_group_timer_); + + // 连接机器人 + connect_robot(); + + RCLCPP_INFO(this->get_logger(), "LBot main node initialized successfully"); +} + +// 析构函数 +LBot::~LBot() { + shutting_down_ = true; + + // 停止 reconnect 线程 + { + std::lock_guard lock(reconnect_thread_mutex_); + if (reconnect_thread_.joinable()) + reconnect_thread_.join(); + } + + disconnect_robot(); +} + +bool LBot::connect_robot() { + std::lock_guard lock(conn_mutex_); + if(conn_state_ == GlobalConnState::CONNECTED) return true; + + g_conn_state.store(GlobalConnState::CONNECTING); // 更新全局状态 + + lbot_handle = lbot_api.lbot_init(arm_ip_.c_str()); + if (lbot_handle->id > 0) { + RCLCPP_INFO(this->get_logger(), "Connected to LBot at %s", arm_ip_.c_str()); + + // 获取机械臂信息 + get_robot_info(); + + // 启动状态监控 + if (lbot_api.lbot_start_state_monitor(lbot_state_callback_wrapper, lbot_error_callback_wrapper)) { + is_state_monitor_started_ = true; + RCLCPP_INFO(this->get_logger(), "State monitor started successfully"); + } else { + is_state_monitor_started_ = false; + RCLCPP_ERROR(this->get_logger(), "Failed to start state monitor"); + } + + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to connect to LBot"); + } + + bool success = lbot_handle->id > 0; + conn_state_ = success ? GlobalConnState::CONNECTED : GlobalConnState::DISCONNECTED; + g_conn_state.store(conn_state_); // 更新全局状态 + + return success; +} + +void LBot::disconnect_robot() { + std::lock_guard lock(conn_mutex_); + if (conn_state_ == GlobalConnState::DISCONNECTED) return; + + RCLCPP_INFO(this->get_logger(), "Disconnecting from LBot..."); + + // 先设置状态，避免其他线程继续操作 + conn_state_ = GlobalConnState::DISCONNECTED; + g_conn_state.store(GlobalConnState::DISCONNECTED); // 更新全局状态 + + // 停止状态监控 + if (is_state_monitor_started_) { + RCLCPP_INFO(this->get_logger(), "Stopping state monitor..."); + lbot_api.lbot_stop_state_monitor(); + is_state_monitor_started_ = false; + } + + // 清理连接 + RCLCPP_INFO(this->get_logger(), "Cleaning up LBot connection..."); + lbot_api.lbot_cleanup(); + + RCLCPP_INFO(this->get_logger(), "Disconnected from LBot successfully"); +} + +void LBot::get_robot_info() { + std::string robot_model, controller_version; + if (lbot_api.lbot_get_controller_info(lbot_handle, robot_model, controller_version)) { + RCLCPP_INFO(this->get_logger(), "Robot Model: %s, Controller Version: %s", robot_model.c_str(), controller_version.c_str()); + } +} + +// 状态发布定时器回调函数 (50Hz) +void LBot::state_publish_timer_callback() { + if (conn_state_ != GlobalConnState::CONNECTED || !is_state_monitor_started_) return; + + lbot_full_state_t state; + { + std::lock_guard lock(state_mutex_); + state = current_lbot_state_; + } + + rclcpp::Time time_now = this->get_clock()->now(); + + // ----------------------- 左臂 ----------------------- + + sensor_msgs::msg::JointState left_joint; + left_joint.header.stamp = time_now; + left_joint.header.frame_id = state.left_arm.frame_id; + + // 关节名称 + left_joint.name.resize(7); + for (size_t i = 0; i < 7; i++) + left_joint.name[i] = state.left_arm.name[i]; + + // 关节角度 + left_joint.position.resize(7); + for (size_t i = 0; i < 7; i++) + left_joint.position[i] = state.left_arm.joint_position[i]; + + // 关节速度 + left_joint.velocity.resize(7); + for (size_t i = 0; i < 7; i++) + left_joint.velocity[i] = state.left_arm.velocity[i]; + + // 关节力矩 effort + left_joint.effort.resize(7); + for (size_t i = 0; i < 7; i++) + left_joint.effort[i] = state.left_arm.effort[i]; + + left_joint_pub_->publish(left_joint); + + // PoseStamped（末端状态） + geometry_msgs::msg::PoseStamped left_pose; + left_pose.header.stamp = time_now; + left_pose.header.frame_id = state.left_arm.frame_id; + + left_pose.pose.position.x = state.left_arm.end_effector_position.x; + left_pose.pose.position.y = state.left_arm.end_effector_position.y; + left_pose.pose.position.z = state.left_arm.end_effector_position.z; + + left_pose.pose.orientation.x = state.left_arm.orientation.x; + left_pose.pose.orientation.y = state.left_arm.orientation.y; + left_pose.pose.orientation.z = state.left_arm.orientation.z; + left_pose.pose.orientation.w = state.left_arm.orientation.w; + + left_pose_pub_->publish(left_pose); + + // ----------------------- 右臂 ----------------------- + + sensor_msgs::msg::JointState right_joint; + right_joint.header.stamp = time_now; + right_joint.header.frame_id = state.right_arm.frame_id; + + right_joint.name.resize(7); + for (size_t i = 0; i < 7; i++) + right_joint.name[i] = state.right_arm.name[i]; + + right_joint.position.resize(7); + for (size_t i = 0; i < 7; i++) + right_joint.position[i] = state.right_arm.joint_position[i]; + + right_joint.velocity.resize(7); + for (size_t i = 0; i < 7; i++) + right_joint.velocity[i] = state.right_arm.velocity[i]; + + right_joint.effort.resize(7); + for (size_t i = 0; i < 7; i++) + right_joint.effort[i] = state.right_arm.effort[i]; + + right_joint_pub_->publish(right_joint); + + geometry_msgs::msg::PoseStamped right_pose; + right_pose.header.stamp = time_now; + right_pose.header.frame_id = state.right_arm.frame_id; + + right_pose.pose.position.x = state.right_arm.end_effector_position.x; + right_pose.pose.position.y = state.right_arm.end_effector_position.y; + right_pose.pose.position.z = state.right_arm.end_effector_position.z; + + right_pose.pose.orientation.x = state.right_arm.orientation.x; + right_pose.pose.orientation.y = state.right_arm.orientation.y; + right_pose.pose.orientation.z = state.right_arm.orientation.z; + right_pose.pose.orientation.w = state.right_arm.orientation.w; + + right_pose_pub_->publish(right_pose); +} + + +/****************************** 关节跟随订阅器（遥操作） ******************************/ + +// 左臂关节跟随回调 +void LBot::left_joint_follow_callback(const std::shared_ptr msg) { + if (!msg || msg->joints.empty()) return; + + if (g_conn_state.load() != GlobalConnState::CONNECTED) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Left joint follow: Robot not connected"); + return; + } + + std::vector joints(msg->joints.begin(), msg->joints.end()); + + if (!lbot_api.lbot_joint_follow(lbot_handle, LBOT_LEFT_ARM, joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to execute left_joint_follow"); + } +} + +// 右臂关节跟随回调 +void LBot::right_joint_follow_callback(const std::shared_ptr msg) { + if (!msg || msg->joints.empty()) return; + + if (g_conn_state.load() != GlobalConnState::CONNECTED) { + RCLCPP_WARN_THROTTLE(this->get_logger(), *this->get_clock(), 1000, "Right joint follow: Robot not connected"); + return; + } + + std::vector joints(msg->joints.begin(), msg->joints.end()); + + if (!lbot_api.lbot_joint_follow(lbot_handle, LBOT_RIGHT_ARM, joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to execute right_joint_follow"); + } +} + +/****************************** 心跳与重连机制 ******************************/ + +// 心跳机制回调函数 +void LBot::heartbeat_timer_callback() +{ + if (conn_state_ != GlobalConnState::CONNECTED) + return; + + std::string model, version; + // bool ok = lbot_api.lbot_get_controller_info(lbot_handle, model, version); + bool ok = true; + + if (!ok) + { + heartbeat_fail_count_++; + + RCLCPP_WARN(get_logger(), "Heartbeat failed (%d/3)", heartbeat_fail_count_); + + if (heartbeat_fail_count_ >= 3) + { + RCLCPP_ERROR(get_logger(), "Heartbeat lost. Disconnecting..."); + disconnect_robot(); + } + } + else + { + heartbeat_fail_count_ = 0; + } +} + +// 自动重连机制回调函数 +void LBot::reconnect_timer_callback() +{ + if (shutting_down_) return; + if (conn_state_ != GlobalConnState::DISCONNECTED) return; + + std::lock_guard lock(reconnect_thread_mutex_); + + if (reconnect_thread_running_) + return; // 线程正在运行，不重复启动 + + reconnect_thread_running_ = true; + + reconnect_thread_ = std::thread([this]() { + if (shutting_down_) { + reconnect_thread_running_ = false; + return; + } + + RCLCPP_INFO(this->get_logger(), "Trying to reconnect to LBot..."); + + bool success = connect_robot(); + + { + std::lock_guard lock(conn_mutex_); + if (success) { + conn_state_ = GlobalConnState::CONNECTED; + heartbeat_fail_count_ = 0; + + if (!shutting_down_) + RCLCPP_INFO(this->get_logger(), "Arm reconnected successfully"); + } else { + conn_state_ = GlobalConnState::DISCONNECTED; + + if (!shutting_down_) + RCLCPP_WARN(this->get_logger(), "Reconnection failed. Will retry..."); + } + } + + reconnect_thread_running_ = false; + }); +} + +// ========== 左臂服务节点实现 ========== +LeftArmServiceNode::LeftArmServiceNode(const std::string& node_name) : rclcpp::Node(node_name) { + // 设置独特的logger名称 + this->get_logger() = rclcpp::get_logger(node_name); + + // 创建互斥回调组确保串行执行 + callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_subscribers_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + create_services(); + RCLCPP_INFO(this->get_logger(), "Left arm service node initialized"); +} + +void LeftArmServiceNode::create_services() { + auto service_qos = rclcpp::QoS(rclcpp::ServicesQoS()); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_subscribers_; + + // 运动控制服务 + move_joint_service_ = this->create_service( + "left_arm/move_joint", + std::bind(&LeftArmServiceNode::move_joint_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + move_pose_service_ = this->create_service( + "left_arm/move_pose", + std::bind(&LeftArmServiceNode::move_pose_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + move_linear_service_ = this->create_service( + "left_arm/move_linear", + std::bind(&LeftArmServiceNode::move_linear_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 运动学计算服务 + forward_kinematics_service_ = this->create_service( + "left_arm/forward_kinematics", + std::bind(&LeftArmServiceNode::forward_kinematics_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + inverse_kinematics_service_ = this->create_service( + "left_arm/inverse_kinematics", + std::bind(&LeftArmServiceNode::inverse_kinematics_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 工具坐标系管理服务 + set_tool_frame_service_ = this->create_service( + "left_arm/set_tool_frame", + std::bind(&LeftArmServiceNode::set_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + get_tool_frame_service_ = this->create_service( + "left_arm/get_tool_frame", + std::bind(&LeftArmServiceNode::get_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + get_current_tool_frame_service_ = this->create_service( + "left_arm/get_current_tool_frame", + std::bind(&LeftArmServiceNode::get_current_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + change_tool_frame_service_ = this->create_service( + "left_arm/change_tool_frame", + std::bind(&LeftArmServiceNode::change_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + delete_tool_frame_service_ = this->create_service( + "left_arm/delete_tool_frame", + std::bind(&LeftArmServiceNode::delete_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + get_all_tool_frames_service_ = this->create_service( + "left_arm/get_all_tool_frames", + std::bind(&LeftArmServiceNode::get_all_tool_frames_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 零点设置服务 + set_zero_service_ = this->create_service( + "left_arm/set_zero", + std::bind(&LeftArmServiceNode::set_zero_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + set_enable_service_ = this->create_service( + "left_arm/set_enable", + std::bind(&LeftArmServiceNode::set_enable_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + set_emergency_service_ = this->create_service( + "left_arm/set_emergency_stop", + std::bind(&LeftArmServiceNode::set_emergency_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 灵巧手设置Topic回调函数 + left_hand_l6_joint_sub_ = this->create_subscription( + "left_hand/set_l6_joint", 10, + std::bind(&LeftArmServiceNode::left_hand_l6_set_joint_callback, this, std::placeholders::_1), + sub_opt); + left_hand_l6_force_sub_ = this->create_subscription( + "left_hand/set_l6_force", 10, + std::bind(&LeftArmServiceNode::left_hand_l6_set_force_callback, this, std::placeholders::_1), + sub_opt); + left_hand_l6_speed_sub_ = this->create_subscription( + "left_hand/set_l6_speed", 10, + std::bind(&LeftArmServiceNode::left_hand_l6_set_speed_callback, this, std::placeholders::_1), + sub_opt); + left_hand_l10_joint_sub_ = this->create_subscription( + "left_hand/set_l10_joint", 10, + std::bind(&LeftArmServiceNode::left_hand_l10_set_joint_callback, this, std::placeholders::_1), + sub_opt); + left_hand_l10_force_sub_ = this->create_subscription( + "left_hand/set_l10_force", 10, + std::bind(&LeftArmServiceNode::left_hand_l10_set_force_callback, this, std::placeholders::_1), + sub_opt); + left_hand_l10_speed_sub_ = this->create_subscription( + "left_hand/set_l10_speed", 10, + std::bind(&LeftArmServiceNode::left_hand_l10_set_speed_callback, this, std::placeholders::_1), + sub_opt); + left_hand_l20_joint_sub_ = this->create_subscription( + "left_hand/set_l20_joint", 10, + std::bind(&LeftArmServiceNode::left_hand_l20_set_joint_callback, this, std::placeholders::_1), + sub_opt); + left_hand_l20_series_position_sub_ = this->create_subscription( + "left_hand/set_l20_series_position", 10, + std::bind(&LeftArmServiceNode::left_hand_l20_set_series_position_callback, this, std::placeholders::_1), + sub_opt); +} + +/****************************** 左臂服务回调函数实现 ******************************/ + +// 关节运动回调函数 +void LeftArmServiceNode::move_joint_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + auto start_time = std::chrono::steady_clock::now(); + RCLCPP_INFO(this->get_logger(), "[%ld ms] Left arm move_joint started", + std::chrono::duration_cast(start_time.time_since_epoch()).count()); + + if (!check_connection_state("left_arm/move_joint")) { + response->success = false; + return; + } + + if (request->joints.size() != 7) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Left arm move_joint: size must be 7"); + return; + } + double joints[7]; + std::copy(request->joints.begin(), request->joints.end(), joints); + response->success = lbot_api.lbot_move_joint(lbot_handle, LBOT_LEFT_ARM, joints, request->speed, request->acce, request->block); + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Left arm move_joint executed successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Left arm move_joint failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } + + auto end_time = std::chrono::steady_clock::now(); + auto duration = std::chrono::duration_cast(end_time - start_time); + RCLCPP_INFO(this->get_logger(), "[%ld ms] Left arm move_joint returned after %ld ms", + std::chrono::duration_cast(end_time.time_since_epoch()).count(), + duration.count()); +} + +// 位姿运动回调函数 +void LeftArmServiceNode::move_pose_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/move_pose")) { + response->success = false; + return; + } + + lbot_position_t position; + lbot_euler_t euler; + + position.x = request->position.x; + position.y = request->position.y; + position.z = request->position.z; + + euler.x = request->euler.x; + euler.y = request->euler.y; + euler.z = request->euler.z; + + response->success = lbot_api.lbot_move_pose(lbot_handle, LBOT_LEFT_ARM, &position, &euler, request->speed, request->acce, request->block); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Left arm move_pose executed successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Left arm move_pose failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 线性运动回调函数 +void LeftArmServiceNode::move_linear_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/move_linear")) { + response->success = false; + return; + } + + lbot_position_t position; + lbot_euler_t euler; + + position.x = request->position.x; + position.y = request->position.y; + position.z = request->position.z; + + euler.x = request->euler.x; + euler.y = request->euler.y; + euler.z = request->euler.z; + + response->success = lbot_api.lbot_move_linear(lbot_handle, LBOT_LEFT_ARM, &position, &euler, request->speed, request->acce, request->block); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Left arm move_linear executed successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Left arm move_linear failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 正运动学回调函数 +void LeftArmServiceNode::forward_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/forward_kinematics")) { + response->success = false; + return; + } + + if (request->joints.size() != 7) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Left arm forward_kinematics: Joint array must contain exactly 7 values"); + return; + } + + double joints[7]; + std::copy(request->joints.begin(), request->joints.end(), joints); + + lbot_position_t position; + lbot_euler_t euler; + + response->success = lbot_api.lbot_forward_kinematics(lbot_handle, LBOT_LEFT_ARM, joints, &position, &euler); + + if (response->success) { + response->position.x = position.x; + response->position.y = position.y; + response->position.z = position.z; + + response->euler.x = euler.x; + response->euler.y = euler.y; + response->euler.z = euler.z; + + RCLCPP_INFO(this->get_logger(), "Left arm forward kinematics calculated successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Left arm forward kinematics calculation failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 逆运动学回调函数 +void LeftArmServiceNode::inverse_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/inverse_kinematics")) { + response->success = false; + return; + } + + double initial_joints[7]; + double* initial_ptr = nullptr; + + if (request->joints.empty()) { + initial_ptr = nullptr; + } else { + if (request->joints.size() != 7) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Left arm inverse_kinematics: Initial joint array must contain exactly 7 values"); + return; + } + std::copy(request->joints.begin(), request->joints.end(), initial_joints); + initial_ptr = initial_joints; + } + + lbot_position_t position; + lbot_euler_t euler; + + position.x = request->position.x; + position.y = request->position.y; + position.z = request->position.z; + + euler.x = request->euler.x; + euler.y = request->euler.y; + euler.z = request->euler.z; + + double result_joints[7]; + response->success = lbot_api.lbot_inverse_kinematics(lbot_handle, LBOT_LEFT_ARM, initial_ptr, &position, &euler, result_joints); + + if (response->success) { + response->joints.resize(7); + std::copy(result_joints, result_joints + 7, response->joints.begin()); + RCLCPP_INFO(this->get_logger(), "Left arm inverse kinematics calculated successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Left arm inverse kinematics calculation failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 设置工具坐标系回调函数 +void LeftArmServiceNode::set_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/set_tool_frame")) { + response->success = false; + return; + } + + const auto &frame = request->frame; + + lbot_position_t position; + position.x = frame.position.x; + position.y = frame.position.y; + position.z = frame.position.z; + + lbot_euler_t euler; + euler.x = frame.euler.x; + euler.y = frame.euler.y; + euler.z = frame.euler.z; + + response->success = lbot_api.lbot_set_tool_frame(lbot_handle, LBOT_LEFT_ARM, frame.name.c_str(), &position, &euler); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Left arm tool frame '%s' set successfully", frame.name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to set left arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 获取指定工具坐标系回调函数 +void LeftArmServiceNode::get_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/get_tool_frame")) { + response->success = false; + return; + } + + lbot_position_t position; + lbot_euler_t euler; + + response->success = lbot_api.lbot_get_tool_frame(lbot_handle, LBOT_LEFT_ARM, request->name.c_str(), &position, &euler); + + if (response->success) { + response->frame.name = request->name; + response->frame.position.x = position.x; + response->frame.position.y = position.y; + response->frame.position.z = position.z; + response->frame.euler.x = euler.x; + response->frame.euler.y = euler.y; + response->frame.euler.z = euler.z; + + RCLCPP_INFO(this->get_logger(), "Left arm tool frame '%s' retrieved successfully", request->name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get left arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 获取当前工具坐标系回调函数 +void LeftArmServiceNode::get_current_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("left_arm/get_current_tool_frame")) { + response->success = false; + return; + } + + std::string name = ""; + lbot_position_t position; + lbot_euler_t euler; + + response->success = lbot_api.lbot_get_current_tool_frame( + lbot_handle, + LBOT_LEFT_ARM, + name, + position, + euler + ); + + if (response->success) { + response->frame.name = name; + response->frame.position.x = position.x; + response->frame.position.y = position.y; + response->frame.position.z = position.z; + response->frame.euler.x = euler.x; + response->frame.euler.y = euler.y; + response->frame.euler.z = euler.z; + + RCLCPP_INFO(this->get_logger(), "Left arm current tool frame '%s' retrieved successfully", response->name.c_str() + ); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get current tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str() + ); + } +} + +// 切换工具坐标系回调函数 +void LeftArmServiceNode::change_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/change_tool_frame")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_change_tool_frame(lbot_handle, LBOT_LEFT_ARM, request->name.c_str()); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Left arm changed to tool frame '%s'", request->name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to change left arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 删除工具坐标系回调函数 +void LeftArmServiceNode::delete_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/delete_tool_frame")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_delete_tool_frame(lbot_handle, LBOT_LEFT_ARM, request->name.c_str()); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Left arm tool frame '%s' deleted successfully", request->name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to delete left arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 获取所有工具坐标系回调函数 +void LeftArmServiceNode::get_all_tool_frames_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("left_arm/get_all_tool_frames")) { + response->success = false; + return; + } + + std::vector frame_names; + response->success = lbot_api.lbot_get_all_tool_frames(lbot_handle, LBOT_LEFT_ARM, frame_names); + + if (response->success) { + response->names = frame_names; + RCLCPP_INFO(this->get_logger(), "Left arm retrieved %zu tool frames", frame_names.size()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get left arm tool frames: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 零点设置回调函数 +void LeftArmServiceNode::set_zero_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("left_arm/set_zero")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_set_zero(lbot_handle, LBOT_LEFT_ARM); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Left arm joint zero set successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to set left arm zero: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 使能设置回调函数 +void LeftArmServiceNode::set_enable_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("left_arm/set_enable")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_enable_arm(lbot_handle, LBOT_LEFT_ARM, request->enable); + + if (response->success) { + if (request->enable) { + RCLCPP_INFO(this->get_logger(), "Left arm enable ON successfully"); + } else { + RCLCPP_INFO(this->get_logger(), "Left arm enable OFF successfully"); + } + } else { + if (request->enable) { + RCLCPP_ERROR(this->get_logger(), + "Failed to enable (ON) left arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), + "Failed to disable (OFF) left arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } + } + +} + +// 急停设置回调函数 +void LeftArmServiceNode::set_emergency_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("left_arm/set_emergency_stop")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_emergency_stop(lbot_handle, LBOT_LEFT_ARM, request->emergency); + + if (response->success) { + if (request->emergency) { + RCLCPP_INFO(this->get_logger(), "Left arm emergency stop activated successfully"); + } else { + RCLCPP_INFO(this->get_logger(), "Left arm emergency stop released successfully"); + } + } else { + if (request->emergency) { + RCLCPP_ERROR(this->get_logger(), + "Failed to activate emergency stop for left arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), + "Failed to release emergency stop for left arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } + } +} + +void LeftArmServiceNode::left_hand_l6_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("left_hand/set_l6_joint")) { + return; + } + std::vector hand_joints(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l6_set_position(lbot_handle, LBOT_LEFT_ARM, hand_joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left hand joint"); + } +} + +void LeftArmServiceNode::left_hand_l6_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("left_hand/set_l6_force")) { + return; + } + std::vector hand_force(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l6_set_effort(lbot_handle, LBOT_LEFT_ARM, hand_force)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left hand force"); + } +} + +void LeftArmServiceNode::left_hand_l6_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("left_hand/set_l6_speed")) { + return; + } + std::vector hand_speed(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l6_set_velocity(lbot_handle, LBOT_LEFT_ARM, hand_speed)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left hand speed"); + } +} + +void LeftArmServiceNode::left_hand_l10_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("left_hand/set_l10_joint")) { + return; + } + std::vector hand_joints(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l10_set_position(lbot_handle, LBOT_LEFT_ARM, hand_joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left hand joint"); + } +} + +void LeftArmServiceNode::left_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("left_hand/set_l10_force")) { + return; + } + std::vector hand_force(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l10_set_effort(lbot_handle, LBOT_LEFT_ARM, hand_force)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left hand force"); + } +} + +void LeftArmServiceNode::left_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("left_hand/set_l10_speed")) { + return; + } + std::vector hand_speed(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l10_set_velocity(lbot_handle, LBOT_LEFT_ARM, hand_speed)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left hand speed"); + } +} + +void LeftArmServiceNode::left_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("left_hand/set_l20_joint")) { + return; + } + std::vector hand_joints(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l20_set_all_position(lbot_handle, LBOT_LEFT_ARM, hand_joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left L20 hand joint"); + } +} + +void LeftArmServiceNode::left_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) { + if (!msg || msg->data.size() < 7) return; + if (!check_connection_state("left_hand/set_l20_series_position")) { + return; + } + + lbot_l20_series_cmd_t cmd{}; + cmd.finger = static_cast(msg->data[0]); + for (size_t i = 0; i < 6; ++i) { + cmd.data[i] = static_cast(msg->data[i + 1]); + } + + if (!lbot_api.lbot_l20_set_series_position(lbot_handle, LBOT_LEFT_ARM, &cmd)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set left L20 hand series position"); + } +} + +// ========== 右臂服务节点实现 ========== +RightArmServiceNode::RightArmServiceNode(const std::string& node_name) : rclcpp::Node(node_name) { + // 设置独特的logger名称 + this->get_logger() = rclcpp::get_logger(node_name); + + // 创建互斥回调组确保串行执行 + callback_group_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + callback_group_subscribers_ = this->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + + create_services(); + RCLCPP_INFO(this->get_logger(), "Right arm service node initialized"); +} + +void RightArmServiceNode::create_services() { + auto service_qos = rclcpp::QoS(rclcpp::ServicesQoS()); + auto sub_opt = rclcpp::SubscriptionOptions(); + sub_opt.callback_group = callback_group_subscribers_; + + // 运动控制服务 + move_joint_service_ = this->create_service( + "right_arm/move_joint", + std::bind(&RightArmServiceNode::move_joint_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + move_pose_service_ = this->create_service( + "right_arm/move_pose", + std::bind(&RightArmServiceNode::move_pose_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + move_linear_service_ = this->create_service( + "right_arm/move_linear", + std::bind(&RightArmServiceNode::move_linear_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 运动学计算服务 + forward_kinematics_service_ = this->create_service( + "right_arm/forward_kinematics", + std::bind(&RightArmServiceNode::forward_kinematics_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + inverse_kinematics_service_ = this->create_service( + "right_arm/inverse_kinematics", + std::bind(&RightArmServiceNode::inverse_kinematics_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 工具坐标系管理服务 + set_tool_frame_service_ = this->create_service( + "right_arm/set_tool_frame", + std::bind(&RightArmServiceNode::set_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + get_tool_frame_service_ = this->create_service( + "right_arm/get_tool_frame", + std::bind(&RightArmServiceNode::get_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + get_current_tool_frame_service_ = this->create_service( + "right_arm/get_current_tool_frame", + std::bind(&RightArmServiceNode::get_current_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + change_tool_frame_service_ = this->create_service( + "right_arm/change_tool_frame", + std::bind(&RightArmServiceNode::change_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + delete_tool_frame_service_ = this->create_service( + "right_arm/delete_tool_frame", + std::bind(&RightArmServiceNode::delete_tool_frame_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + get_all_tool_frames_service_ = this->create_service( + "right_arm/get_all_tool_frames", + std::bind(&RightArmServiceNode::get_all_tool_frames_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 零点设置服务 + set_zero_service_ = this->create_service( + "right_arm/set_zero", + std::bind(&RightArmServiceNode::set_zero_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + set_enable_service_ = this->create_service( + "right_arm/set_enable", + std::bind(&RightArmServiceNode::set_enable_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + set_emergency_service_ = this->create_service( + "right_arm/set_emergency_stop", + std::bind(&RightArmServiceNode::set_emergency_callback, this, std::placeholders::_1, std::placeholders::_2), + service_qos, callback_group_); + + // 灵巧手设置Topic回调函数 + right_hand_l6_joint_sub_ = this->create_subscription( + "right_hand/set_l6_joint", 10, + std::bind(&RightArmServiceNode::right_hand_l6_set_joint_callback, this, std::placeholders::_1), + sub_opt); + right_hand_l6_force_sub_ = this->create_subscription( + "right_hand/set_l6_force", 10, + std::bind(&RightArmServiceNode::right_hand_l6_set_force_callback, this, std::placeholders::_1), + sub_opt); + right_hand_l6_speed_sub_ = this->create_subscription( + "right_hand/set_l6_speed", 10, + std::bind(&RightArmServiceNode::right_hand_l6_set_speed_callback, this, std::placeholders::_1), + sub_opt); + + right_hand_l10_joint_sub_ = this->create_subscription( + "right_hand/set_l10_joint", 10, + std::bind(&RightArmServiceNode::right_hand_l10_set_joint_callback, this, std::placeholders::_1), + sub_opt); + right_hand_l10_force_sub_ = this->create_subscription( + "right_hand/set_l10_force", 10, + std::bind(&RightArmServiceNode::right_hand_l10_set_force_callback, this, std::placeholders::_1), + sub_opt); + right_hand_l10_speed_sub_ = this->create_subscription( + "right_hand/set_l10_speed", 10, + std::bind(&RightArmServiceNode::right_hand_l10_set_speed_callback, this, std::placeholders::_1), + sub_opt); + right_hand_l20_joint_sub_ = this->create_subscription( + "right_hand/set_l20_joint", 10, + std::bind(&RightArmServiceNode::right_hand_l20_set_joint_callback, this, std::placeholders::_1), + sub_opt); + right_hand_l20_series_position_sub_ = this->create_subscription( + "right_hand/set_l20_series_position", 10, + std::bind(&RightArmServiceNode::right_hand_l20_set_series_position_callback, this, std::placeholders::_1), + sub_opt); +} + +/****************************** 右臂服务回调函数实现 ******************************/ + +// 关节运动回调函数 +void RightArmServiceNode::move_joint_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/move_joint")) { + response->success = false; + return; + } + + if (request->joints.size() != 7) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Right arm move_joint: size must be 7"); + return; + } + double joints[7]; + std::copy(request->joints.begin(), request->joints.end(), joints); + response->success = lbot_api.lbot_move_joint(lbot_handle, LBOT_RIGHT_ARM, joints, request->speed, request->acce, request->block); + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Right arm move_joint executed successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Right arm move_joint failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 位姿运动回调函数 +void RightArmServiceNode::move_pose_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/move_pose")) { + response->success = false; + return; + } + + lbot_position_t position; + lbot_euler_t euler; + + position.x = request->position.x; + position.y = request->position.y; + position.z = request->position.z; + + euler.x = request->euler.x; + euler.y = request->euler.y; + euler.z = request->euler.z; + + response->success = lbot_api.lbot_move_pose(lbot_handle, LBOT_RIGHT_ARM, &position, &euler, request->speed, request->acce, request->block); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Right arm move_pose executed successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Right arm move_pose failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 线性运动回调函数 +void RightArmServiceNode::move_linear_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/move_linear")) { + response->success = false; + return; + } + + lbot_position_t position; + lbot_euler_t euler; + + position.x = request->position.x; + position.y = request->position.y; + position.z = request->position.z; + + euler.x = request->euler.x; + euler.y = request->euler.y; + euler.z = request->euler.z; + + response->success = lbot_api.lbot_move_linear(lbot_handle, LBOT_RIGHT_ARM, &position, &euler, request->speed, request->acce, request->block); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Right arm move_linear executed successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Right arm move_linear failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 正运动学回调函数 +void RightArmServiceNode::forward_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/forward_kinematics")) { + response->success = false; + return; + } + + if (request->joints.size() != 7) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Right arm forward_kinematics: Joint array must contain exactly 7 values"); + return; + } + + double joints[7]; + std::copy(request->joints.begin(), request->joints.end(), joints); + + lbot_position_t position; + lbot_euler_t euler; + + response->success = lbot_api.lbot_forward_kinematics(lbot_handle, LBOT_RIGHT_ARM, joints, &position, &euler); + + if (response->success) { + response->position.x = position.x; + response->position.y = position.y; + response->position.z = position.z; + + response->euler.x = euler.x; + response->euler.y = euler.y; + response->euler.z = euler.z; + + RCLCPP_INFO(this->get_logger(), "Right arm forward kinematics calculated successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Right arm forward kinematics calculation failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 逆运动学回调函数 +void RightArmServiceNode::inverse_kinematics_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/inverse_kinematics")) { + response->success = false; + return; + } + + double initial_joints[7]; + double* initial_ptr = nullptr; + + if (request->joints.empty()) { + initial_ptr = nullptr; + } else { + if (request->joints.size() != 7) { + response->success = false; + RCLCPP_ERROR(this->get_logger(), "Right arm inverse_kinematics: Initial joint array must contain exactly 7 values"); + return; + } + std::copy(request->joints.begin(), request->joints.end(), initial_joints); + initial_ptr = initial_joints; + } + + lbot_position_t position; + lbot_euler_t euler; + + position.x = request->position.x; + position.y = request->position.y; + position.z = request->position.z; + + euler.x = request->euler.x; + euler.y = request->euler.y; + euler.z = request->euler.z; + + double result_joints[7]; + response->success = lbot_api.lbot_inverse_kinematics(lbot_handle, LBOT_RIGHT_ARM, initial_ptr, &position, &euler, result_joints); + + if (response->success) { + response->joints.resize(7); + std::copy(result_joints, result_joints + 7, response->joints.begin()); + RCLCPP_INFO(this->get_logger(), "Right arm inverse kinematics calculated successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Right arm inverse kinematics calculation failed: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 设置工具坐标系回调函数 +void RightArmServiceNode::set_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/set_tool_frame")) { + response->success = false; + return; + } + + const auto &frame = request->frame; + + lbot_position_t position; + position.x = frame.position.x; + position.y = frame.position.y; + position.z = frame.position.z; + + lbot_euler_t euler; + euler.x = frame.euler.x; + euler.y = frame.euler.y; + euler.z = frame.euler.z; + + response->success = lbot_api.lbot_set_tool_frame(lbot_handle, LBOT_RIGHT_ARM, frame.name.c_str(), &position, &euler); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Right arm tool frame '%s' set successfully", frame.name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to set right arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 获取指定工具坐标系回调函数 +void RightArmServiceNode::get_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/get_tool_frame")) { + response->success = false; + return; + } + + lbot_position_t position; + lbot_euler_t euler; + + response->success = lbot_api.lbot_get_tool_frame(lbot_handle, LBOT_RIGHT_ARM, request->name.c_str(), &position, &euler); + + if (response->success) { + response->frame.name = request->name; + response->frame.position.x = position.x; + response->frame.position.y = position.y; + response->frame.position.z = position.z; + response->frame.euler.x = euler.x; + response->frame.euler.y = euler.y; + response->frame.euler.z = euler.z; + + RCLCPP_INFO(this->get_logger(), "Right arm tool frame '%s' retrieved successfully", request->name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get right arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + + +// 获取当前工具坐标系回调函数 +void RightArmServiceNode::get_current_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("right_arm/get_current_tool_frame")) { + response->success = false; + return; + } + + std::string name = ""; + lbot_position_t position; + lbot_euler_t euler; + + response->success = lbot_api.lbot_get_current_tool_frame( + lbot_handle, + LBOT_RIGHT_ARM, + name, + position, + euler + ); + + if (response->success) { + response->frame.name = name; + response->frame.position.x = position.x; + response->frame.position.y = position.y; + response->frame.position.z = position.z; + response->frame.euler.x = euler.x; + response->frame.euler.y = euler.y; + response->frame.euler.z = euler.z; + + RCLCPP_INFO(this->get_logger(), "Right arm current tool frame '%s' retrieved successfully", response->name.c_str() + ); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get current tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str() + ); + } +} + +// 切换工具坐标系回调函数 +void RightArmServiceNode::change_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/change_tool_frame")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_change_tool_frame(lbot_handle, LBOT_RIGHT_ARM, request->name.c_str()); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Right arm changed to tool frame '%s'", request->name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to change right arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 删除工具坐标系回调函数 +void RightArmServiceNode::delete_tool_frame_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/delete_tool_frame")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_delete_tool_frame(lbot_handle, LBOT_RIGHT_ARM, request->name.c_str()); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Right arm tool frame '%s' deleted successfully", request->name.c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to delete right arm tool frame: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 获取所有工具坐标系回调函数 +void RightArmServiceNode::get_all_tool_frames_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("right_arm/get_all_tool_frames")) { + response->success = false; + return; + } + + std::vector frame_names; + response->success = lbot_api.lbot_get_all_tool_frames(lbot_handle, LBOT_RIGHT_ARM, frame_names); + + if (response->success) { + response->names = frame_names; + RCLCPP_INFO(this->get_logger(), "Right arm retrieved %zu tool frames", frame_names.size()); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to get right arm tool frames: %s", lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 零点设置回调函数 +void RightArmServiceNode::set_zero_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("right_arm/set_zero")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_set_zero(lbot_handle, LBOT_RIGHT_ARM); + + if (response->success) { + RCLCPP_INFO(this->get_logger(), "Right arm joint zero set successfully"); + } else { + RCLCPP_ERROR(this->get_logger(), "Failed to set right arm zero: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } +} + +// 使能设置回调函数 +void RightArmServiceNode::set_enable_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + if (!check_connection_state("right_arm/set_enable")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_enable_arm(lbot_handle, LBOT_RIGHT_ARM, request->enable); + + if (response->success) { + if (request->enable) { + RCLCPP_INFO(this->get_logger(), "Right arm enable ON successfully"); + } else { + RCLCPP_INFO(this->get_logger(), "Right arm enable OFF successfully"); + } + } else { + if (request->enable) { + RCLCPP_ERROR(this->get_logger(), + "Failed to enable (ON) right arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), + "Failed to disable (OFF) right arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } + } +} + +// 急停设置回调函数 +void RightArmServiceNode::set_emergency_callback( + const std::shared_ptr request, + std::shared_ptr response) +{ + (void)request; + + if (!check_connection_state("right_arm/set_emergency_stop")) { + response->success = false; + return; + } + + response->success = lbot_api.lbot_emergency_stop(lbot_handle, LBOT_RIGHT_ARM, request->emergency); + + if (response->success) { + if (request->emergency) { + RCLCPP_INFO(this->get_logger(), "Right arm emergency stop activated successfully"); + } else { + RCLCPP_INFO(this->get_logger(), "Right arm emergency stop released successfully"); + } + } else { + if (request->emergency) { + RCLCPP_ERROR(this->get_logger(), + "Failed to activate emergency stop for right arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } else { + RCLCPP_ERROR(this->get_logger(), + "Failed to release emergency stop for right arm: %s", + lbot_api.lbot_get_last_error(lbot_handle).c_str()); + } + } +} + +void RightArmServiceNode::right_hand_l6_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("right_hand/set_l6_joint")) { + return; + } + std::vector hand_joints(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l6_set_position(lbot_handle, LBOT_RIGHT_ARM, hand_joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right hand joint"); + } +} + +void RightArmServiceNode::right_hand_l6_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("right_hand/set_l6_force")) { + return; + } + std::vector hand_force(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l6_set_effort(lbot_handle, LBOT_RIGHT_ARM, hand_force)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right hand force"); + } +} + +void RightArmServiceNode::right_hand_l6_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("right_hand/set_l6_speed")) { + return; + } + std::vector hand_speed(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l6_set_velocity(lbot_handle, LBOT_RIGHT_ARM, hand_speed)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right hand speed"); + } +} + +void RightArmServiceNode::right_hand_l10_set_joint_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("right_hand/set_l10_joint")) { + return; + } + std::vector hand_joints(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l10_set_position(lbot_handle, LBOT_RIGHT_ARM, hand_joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right hand joint"); + } +} + +void RightArmServiceNode::right_hand_l10_set_force_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("right_hand/set_l10_force")) { + return; + } + std::vector hand_force(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l10_set_effort(lbot_handle, LBOT_RIGHT_ARM, hand_force)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right hand force"); + } +} + +void RightArmServiceNode::right_hand_l10_set_speed_callback(const std_msgs::msg::UInt8MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("right_hand/set_l10_speed")) { + return; + } + std::vector hand_speed(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l10_set_velocity(lbot_handle, LBOT_RIGHT_ARM, hand_speed)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right hand speed"); + } +} + +void RightArmServiceNode::right_hand_l20_set_joint_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) { + if (!msg || msg->data.empty()) return; + if (!check_connection_state("right_hand/set_l20_joint")) { + return; + } + std::vector hand_joints(msg->data.begin(), msg->data.end()); + + if (!lbot_api.lbot_l20_set_all_position(lbot_handle, LBOT_RIGHT_ARM, hand_joints)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right L20 hand joint"); + } +} + +void RightArmServiceNode::right_hand_l20_set_series_position_callback(const std_msgs::msg::Int32MultiArray::SharedPtr msg) { + if (!msg || msg->data.size() < 7) return; + if (!check_connection_state("right_hand/set_l20_series_position")) { + return; + } + + lbot_l20_series_cmd_t cmd{}; + cmd.finger = static_cast(msg->data[0]); + for (size_t i = 0; i < 6; ++i) { + cmd.data[i] = static_cast(msg->data[i + 1]); + } + + if (!lbot_api.lbot_l20_set_series_position(lbot_handle, LBOT_RIGHT_ARM, &cmd)) { + RCLCPP_ERROR(this->get_logger(), "Failed to set right L20 hand series position"); + } +} + +} // namespace lbot_driver + +/****************************** 主函数 ******************************/ + +int main(int argc, char ** argv) { + rclcpp::init(argc, argv); + + RCLCPP_INFO(rclcpp::get_logger("lbot_driver"), "Starting LBot Driver with separate service nodes..."); + + // 创建三个节点 + auto main_node = std::make_shared("lbot_main_node"); // 主节点：连接管理+状态发布+关节跟随 + auto left_arm_node = std::make_shared("lbot_left_arm_node"); // 左臂服务节点 + auto right_arm_node = std::make_shared("lbot_right_arm_node"); // 右臂服务节点 + + rclcpp::on_shutdown([&]() { + RCLCPP_INFO(rclcpp::get_logger("lbot_driver"), "Shutdown signal received, stopping..."); + if (main_node) { + main_node->shutting_down_ = true; + main_node->disconnect_robot(); + } + RCLCPP_INFO(rclcpp::get_logger("lbot_driver"), "Cleanup completed"); + }); + + // 创建三个单线程执行器 + rclcpp::executors::SingleThreadedExecutor main_executor; // 主节点执行器 + rclcpp::executors::SingleThreadedExecutor left_arm_executor; // 左臂服务执行器 + rclcpp::executors::SingleThreadedExecutor right_arm_executor; // 右臂服务执行器 + + // 将节点添加到对应执行器 + main_executor.add_node(main_node); + left_arm_executor.add_node(left_arm_node); + right_arm_executor.add_node(right_arm_node); + + // 创建三个线程分别运行执行器 + std::thread main_thread([&main_executor]() { + try { + RCLCPP_INFO(rclcpp::get_logger("main_executor"), "Main node executor spinning..."); + main_executor.spin(); + } catch (const std::exception& e) { + RCLCPP_ERROR(rclcpp::get_logger("main_executor"), "Exception in main executor: %s", e.what()); + } + }); + + std::thread left_arm_thread([&left_arm_executor]() { + try { + RCLCPP_INFO(rclcpp::get_logger("left_arm_executor"), "Left arm service executor spinning..."); + left_arm_executor.spin(); + } catch (const std::exception& e) { + RCLCPP_ERROR(rclcpp::get_logger("left_arm_executor"), "Exception in left arm executor: %s", e.what()); + } + }); + + std::thread right_arm_thread([&right_arm_executor]() { + try { + RCLCPP_INFO(rclcpp::get_logger("right_arm_executor"), "Right arm service executor spinning..."); + right_arm_executor.spin(); + } catch (const std::exception& e) { + RCLCPP_ERROR(rclcpp::get_logger("right_arm_executor"), "Exception in right arm executor: %s", e.what()); + } + }); + + RCLCPP_INFO(rclcpp::get_logger("lbot_driver"), "All nodes ready, spinning..."); + + // 等待所有线程结束 + main_thread.join(); + left_arm_thread.join(); + right_arm_thread.join(); + + RCLCPP_INFO(rclcpp::get_logger("lbot_driver"), "LBot Driver stopped"); + return 0; +}