修复了rosbag录制服务崩溃的bug

已加入对机械臂反馈值和控制量的参数录制
加入随sh脚本生命周期的使能和失能功能
2026-09-17 16:34:27 +08:00 · 2026-09-17 16:17:50 +08:00 · 2026-09-17 15:30:07 +08:00 · 2026-09-17 13:44:10 +08:00 · 2026-09-17 13:42:11 +08:00 · 2026-09-17 10:50:14 +08:00
55 changed files with 4493 additions and 0 deletions
@@ -0,0 +1,3 @@
+build/
+install/
+log/
@@ -0,0 +1,288 @@
+# 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 原生加速度反馈接口。
+
+## 机器人健康/故障状态
+
+当前 `officer_sdk/lbot_driver` 已发布系统错误信息 topic：
+
+```text
+/robot1/system_error
+```
+
+消息类型：
+
+```text
+lbot_arm_interfaces/msg/SystemError
+```
+
+消息定义：
+
+```text
+std_msgs/Header header
+int32 error_code
+string error_msg
+bool connected
+```
+
+字段含义：
+
+```text
+error_code  SDK 或 driver 上报的错误码
+error_msg   错误信息
+connected   发布错误时 driver 是否认为机器人处于连接状态
+```
+
+示例：
+
+```bash
+ros2 topic echo /robot1/system_error
+```
+
+目前该 topic 会在以下情况发布：
+
+```text
+SDK error callback 触发时
+连接机器人失败时，error_code = -1
+状态监控启动失败时，error_code = -2
+```
+
+注意：SDK 当前状态结构 `lbot_full_state_t` 只包含左右臂关节状态、末端位姿、时间戳和 IP；没有完整的故障码、告警码、使能状态、急停状态等字段。因此 `/robot1/system_error` 当前是错误事件 topic，不是完整健康状态快照。
+
+仍然可以辅助查看 ROS 日志：
+
+```text
+/rosout
+```
+
+示例：
+
+```bash
+ros2 topic echo /rosout
+```
+
+也可以查看启动脚本日志：
+
+```bash
+tail -f ~/workspace/wpz_LbotArm/log/lbot_arm_control.log
+```
+
+如果后续需要稳定给外部软件使用的完整健康状态快照，建议在 `SystemError.msg` 之外再新增 `RobotHealth.msg`，并在连接/重连逻辑、使能/急停服务回调中维护并周期发布该状态。需要修改的位置：
+
+```text
+src/officer_sdk/lbot_arm_interfaces/msg/RobotHealth.msg
+src/officer_sdk/lbot_arm_interfaces/CMakeLists.txt
+src/officer_sdk/lbot_driver/include/lbot_driver/lbot_driver.h
+src/officer_sdk/lbot_driver/src/lbot_driver.cpp
+```
+
+## 机械臂关节空间控制
+
+外部软件如果需要发送机械臂各关节目标位置，使用 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]}"
+```
+
+## 相关代码位置
+
+反馈发布：
+
+```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()
+```
+
+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
+```
@@ -0,0 +1,46 @@
+arm_joint_limits:
+  right_arm:
+    joint_1:
+      vec: 4
+      acc: 4
+    joint_2:
+      vec: 4
+      acc: 4
+    joint_3:
+      vec: 4
+      acc: 4
+    joint_4:
+      vec: 4
+      acc: 4
+    joint_5:
+      vec: 4
+      acc: 4
+    joint_6:
+      vec: 4
+      acc: 4
+    joint_7:
+      vec: 4
+      acc: 4
+
+  left_arm:
+    joint_1:
+      vec: 4
+      acc: 4
+    joint_2:
+      vec: 4
+      acc: 4
+    joint_3:
+      vec: 4
+      acc: 4
+    joint_4:
+      vec: 4
+      acc: 4
+    joint_5:
+      vec: 4
+      acc: 4
+    joint_6:
+      vec: 4
+      acc: 4
+    joint_7:
+      vec: 4
+      acc: 4
@@ -0,0 +1,47 @@
+arm_joint_limits:
+  right_arm:
+    joint_1:
+      min: -167
+      max: 167
+    joint_2:
+      min: -4
+      max: 183
+    joint_3:
+      min: -154
+      max: 154
+    joint_4:
+      min: -117
+      max: 117
+    joint_5:
+      min: -154
+      max: 154
+    joint_6:
+      min: -91
+      max: 91
+    joint_7:
+      min: -91
+      max: 91
+
+  left_arm:
+    joint_1:
+      min: -167
+      max: 167
+    joint_2:
+      min: -183
+      max: 4
+    joint_3:
+      min: -154
+      max: 154
+    joint_4:
+      min: -117
+      max: 117
+    joint_5:
+      min: -154
+      max: 154
+    joint_6:
+      min: -91
+      max: 91
+    joint_7:
+      min: -91
+      max: 91
+      
@@ -0,0 +1,258 @@
+#!/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"
+BAG_PID_FILE="${STATE_DIR}/rosbag.pid"
+BAG_PGID_FILE="${STATE_DIR}/rosbag.pgid"
+LOG_FILE="${WS_DIR}/log/lbot_arm_control.log"
+BAG_LOG_FILE="${WS_DIR}/log/lbot_arm_rosbag.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
+}
+
+is_bag_running() {
+  [[ -f "${BAG_PID_FILE}" ]] || return 1
+  local pid
+  pid="$(cat "${BAG_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_bag_recorder() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${BAG_LOG_FILE}")"
+
+  if is_bag_running; then
+    echo "rosbag recorder is already running, pid=$(cat "${BAG_PID_FILE}")"
+    return 0
+  fi
+
+  echo "Starting rosbag recorder..."
+  echo "Rosbag log: ${BAG_LOG_FILE}"
+
+  : > "${BAG_LOG_FILE}"
+  setsid env WS_DIR="${WS_DIR}" bash -lc '
+    set -euo pipefail
+    set +u
+    source "${WS_DIR}/install/setup.bash"
+    set -u
+    while true; do
+      start_time=$(date +%Y%m%d_%H%M%S)
+      output=/tmp/lbot_arm_${start_time}.mcap
+      echo "Starting rosbag: ${output}"
+      set +e
+      timeout 3600 ros2 bag record \
+        --storage mcap \
+        --output "${output}" \
+        --polling-interval 2 \
+        --include-unpublished-topics \
+        --disable-keyboard-controls \
+        --log-level warn \
+        --topics \
+          /robot1/left_arm/joint_states \
+          /robot1/right_arm/joint_states \
+          /robot1/system_error \
+          /robot1/left_hand/set_l20_joint \
+          /robot1/right_hand/set_l20_joint \
+        --services \
+          /robot1/left_arm/move_joint \
+          /robot1/right_arm/move_joint
+      rc=$?
+      set -e
+      if [[ ${rc} -ne 124 ]]; then
+        exit ${rc}
+      fi
+    done
+  ' >> "${BAG_LOG_FILE}" 2>&1 &
+
+  local pid=$!
+  local pgid
+  pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')"
+  echo "${pid}" > "${BAG_PID_FILE}"
+  echo "${pgid:-${pid}}" > "${BAG_PGID_FILE}"
+
+  sleep 1
+  if is_bag_running; then
+    echo "rosbag recorder started, pid=${pid}, pgid=$(cat "${BAG_PGID_FILE}")"
+  else
+    echo "rosbag recorder failed to start. Check log: ${BAG_LOG_FILE}"
+    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+    return 1
+  fi
+}
+
+stop_bag_recorder() {
+  if ! [[ -f "${BAG_PID_FILE}" ]]; then
+    return 0
+  fi
+
+  local pid pgid
+  pid="$(cat "${BAG_PID_FILE}")"
+  pgid="${pid}"
+  if [[ -f "${BAG_PGID_FILE}" ]]; then
+    pgid="$(cat "${BAG_PGID_FILE}")"
+  fi
+
+  if ! kill -0 "${pid}" 2>/dev/null; then
+    rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+    return 0
+  fi
+
+  echo "Stopping rosbag recorder, 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 "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+      echo "rosbag recorder stopped"
+      return 0
+    fi
+    sleep 0.2
+  done
+
+  echo "rosbag recorder did not exit after SIGTERM, forcing stop..."
+  kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true
+  rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}"
+  echo "rosbag recorder stopped"
+}
+
+start_driver() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")"
+
+  if is_running; then
+    echo "lbot_driver is already running, pid=$(cat "${PID_FILE}")"
+    start_bag_recorder
+    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}")"
+    start_bag_recorder
+  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"
+    stop_bag_recorder
+    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}"
+    stop_bag_recorder
+    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"
+      stop_bag_recorder
+      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"
+  stop_bag_recorder
+}
+
+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
+
+  if is_bag_running; then
+    echo "rosbag recorder is running, pid=$(cat "${BAG_PID_FILE}"), pgid=$(cat "${BAG_PGID_FILE}" 2>/dev/null || cat "${BAG_PID_FILE}")"
+  else
+    echo "rosbag recorder is not running"
+  fi
+}
+
+case "${1:-}" in
+  start)
+    start_driver
+    ;;
+  stop)
+    stop_driver
+    ;;
+  status)
+    status_driver
+    ;;
+  *)
+    usage
+    exit 1
+    ;;
+esac
@@ -0,0 +1,6 @@
+std_msgs/Header header
+
+uint64 system_timestamp
+string arm_ip
+
+float64[12] 
@@ -0,0 +1,72 @@
+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"
+    "msg/SystemError.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(<dependency> 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()
@@ -0,0 +1,3 @@
+float32[] joints
+geometry_msgs/Vector3 euler
+geometry_msgs/Pose pose
@@ -0,0 +1,2 @@
+float32[] joints
+bool follow
@@ -0,0 +1,3 @@
+string name
+geometry_msgs/Vector3 euler
+geometry_msgs/Vector3 position
@@ -0,0 +1,2 @@
+geometry_msgs/Vector3 euler
+geometry_msgs/Vector3 position
@@ -0,0 +1,4 @@
+std_msgs/Header header
+int32 error_code
+string error_msg
+bool connected
@@ -0,0 +1,25 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+    <name>lbot_arm_interfaces</name>
+    <version>0.0.0</version>
+    <description>TODO: Package description</description>
+    <maintainer email="mengfanjiwork@163.com">Ross</maintainer>
+    <license>TODO: License declaration</license>
+
+    <buildtool_depend>ament_cmake</buildtool_depend>
+
+    <build_depend>rosidl_default_generators</build_depend>
+    <exec_depend>rosidl_default_runtime</exec_depend>
+    <member_of_group>rosidl_interface_packages</member_of_group>
+
+    <test_depend>ament_lint_auto</test_depend>
+    <test_depend>ament_lint_common</test_depend>
+
+    <export>
+        <build_type>ament_cmake</build_type>
+    </export>
+    <depend>std_msgs</depend>
+    <depend>geometry_msgs</depend>
+    <depend>sensor_msgs</depend>
+</package>
@@ -0,0 +1,3 @@
+string name
+---
+bool success
@@ -0,0 +1,3 @@
+string name
+---
+bool success
@@ -0,0 +1,7 @@
+float32[] joints
+
+---
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+
+bool success
@@ -0,0 +1,4 @@
+
+---
+string[] names
+bool success
@@ -0,0 +1,5 @@
+---
+string name
+lbot_arm_interfaces/LbotFrame frame
+bool success
+
@@ -0,0 +1,4 @@
+string name
+---
+lbot_arm_interfaces/LbotFrame frame
+bool success
@@ -0,0 +1,6 @@
+float32[] joints                      # 此关节角度不设置会默认从机械臂读取当前角度，如果设置则基于此值为初始角度进行逆解
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+---
+float32[] joints
+bool success
@@ -0,0 +1,9 @@
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+float32 speed
+float32 acce
+bool block
+
+---
+
+bool success
@@ -0,0 +1,7 @@
+float32[] joints
+float32 speed
+float32 acce
+bool block
+
+---
+bool success
@@ -0,0 +1,9 @@
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+float32 speed
+float32 acce
+bool block
+
+---
+
+bool success
@@ -0,0 +1,9 @@
+geometry_msgs/Vector3 position
+geometry_msgs/Vector3 euler
+float32 speed
+float32 acce
+bool block
+
+---
+
+bool success
@@ -0,0 +1,5 @@
+bool emergency
+
+---
+
+bool success
@@ -0,0 +1,5 @@
+bool enable
+
+---
+
+bool success
@@ -0,0 +1,3 @@
+lbot_arm_interfaces/LbotFrame frame
+---
+bool success
@@ -0,0 +1,3 @@
+string name
+---
+bool success
@@ -0,0 +1,3 @@
+
+---
+bool success
@@ -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()
@@ -0,0 +1,4 @@
+lbot_driver:
+  ros__parameters:
+    #robot param
+    arm_ip: "192.168.10.21"        #设置TCP连接时的IP
@@ -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
@@ -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 <string>
+#include <vector>
+#include <functional>
+
+/**
+ * @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<double>& 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<double>& position, const std::vector<double>& 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<double>& position, const std::vector<double>& 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<double>& 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<uint8_t>& 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<uint8_t>& 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<uint8_t>& 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<uint8_t>& 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<uint8_t>& 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<uint8_t>& 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<int>& 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<double>& joints,
+                                std::vector<double>& position, std::vector<double>& 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<double>& initial_joints,
+                                const std::vector<double>& position, const std::vector<double>& euler,
+                                std::vector<double>& 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<double>& position, const std::vector<double>& 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<double>& position, std::vector<double>& 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<double>& position,
+                                    std::vector<double>& 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<std::string>& 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<double>& position, const std::vector<double>& 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<double>& position, std::vector<double>& 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<std::string>& 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
@@ -0,0 +1,430 @@
+// 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 <iostream>
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp/clock.hpp" 
+#include <memory>
+#include <string>
+#include <thread>
+#include <chrono>
+#include <functional>
+#include <atomic>
+#include <unistd.h>             
+#include <signal.h>             
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <netinet/in.h>
+#include <arpa/inet.h>
+#include <sys/ioctl.h>
+#include <sys/time.h>
+#include <sys/select.h>
+#include <fcntl.h>
+#include <rmw/qos_profiles.h>
+
+#include "lbot_api_cpp.h"
+
+// ROS2 标准消息类型
+#include <std_msgs/msg/empty.hpp>
+#include <std_msgs/msg/bool.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <std_srvs/srv/empty.hpp>
+#include <sensor_msgs/msg/joint_state.hpp>
+#include <geometry_msgs/msg/pose.hpp>
+#include <geometry_msgs/msg/pose_stamped.hpp>
+#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"
+#include "lbot_arm_interfaces/msg/system_error.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<GlobalConnState> 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<bool> shutting_down_{false};
+    // 单例类指针
+    static LBot* g_instance;
+
+    // 重连机制相关线程与变量
+    std::thread reconnect_thread_;
+    std::atomic<bool> reconnect_thread_running_{false};
+    std::mutex reconnect_thread_mutex_;
+
+    void disconnect_robot();
+    void publish_system_error(int error_code, const char* error_msg);
+
+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<sensor_msgs::msg::JointState>::SharedPtr left_joint_pub_;
+    rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr right_joint_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr left_pose_pub_;
+    rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr right_pose_pub_;
+    rclcpp::Publisher<lbot_arm_interfaces::msg::SystemError>::SharedPtr system_error_pub_;
+
+    /****************************** 订阅器 ******************************/
+    
+    // 关节跟随订阅器 (用于遥操作) - 移到主节点
+    rclcpp::Subscription<lbot_arm_interfaces::msg::FollowJoint>::SharedPtr left_joint_follow_sub_;
+    rclcpp::Subscription<lbot_arm_interfaces::msg::FollowJoint>::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);
+    
+    /****************************** 连接状态检查函数 ******************************/
+    
+    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<lbot_arm_interfaces::srv::MoveJ::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJ::Response> response);
+    
+    void move_pose_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Response> response);
+    
+    void move_linear_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Response> response);
+
+    // 运动学计算
+    void forward_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Response> response);
+    
+    void inverse_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Response> response);
+
+    // 工具坐标系管理
+    void set_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Response> response);
+    
+    void get_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Response> response);
+
+    void get_current_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Response> response);
+    
+    void change_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Response> response);
+    
+    void delete_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Response> response);
+    
+    void get_all_tool_frames_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Response> response);
+
+    // 系统设置
+    void set_zero_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Response> response);
+    void set_enable_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Response> response);
+    void set_emergency_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Response> response);
+
+    /****************************** 服务器 ******************************/
+    
+    // 运动控制服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJ>::SharedPtr move_joint_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJP>::SharedPtr move_pose_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveL>::SharedPtr move_linear_service_;
+
+    // 运动学计算服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::ForwardKinematics>::SharedPtr forward_kinematics_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::InverseKinematics>::SharedPtr inverse_kinematics_service_;
+
+    // 工具坐标系管理服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetFrame>::SharedPtr set_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetFrame>::SharedPtr get_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetCurrentFrame>::SharedPtr get_current_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::ChangeFrame>::SharedPtr change_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::DeleteFrame>::SharedPtr delete_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetAllFrames>::SharedPtr get_all_tool_frames_service_;
+
+    // 系统设置服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetZero>::SharedPtr set_zero_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEnable>::SharedPtr set_enable_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEmergency>::SharedPtr set_emergency_service_;
+
+    /****************************** 灵巧手Topic ******************************/
+
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l6_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l6_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l6_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr left_hand_l10_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr left_hand_l20_joint_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);
+    
+    /****************************** 连接状态检查函数 ******************************/
+    
+    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<lbot_arm_interfaces::srv::MoveJ::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJ::Response> response);
+    
+    void move_pose_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveJP::Response> response);
+    
+    void move_linear_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::MoveL::Response> response);
+
+    // 运动学计算
+    void forward_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ForwardKinematics::Response> response);
+    
+    void inverse_kinematics_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::InverseKinematics::Response> response);
+
+    // 工具坐标系管理
+    void set_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetFrame::Response> response);
+    
+    void get_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetFrame::Response> response);
+
+    void get_current_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetCurrentFrame::Response> response);
+    
+    void change_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::ChangeFrame::Response> response);
+    
+    void delete_tool_frame_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::DeleteFrame::Response> response);
+    
+    void get_all_tool_frames_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::GetAllFrames::Response> response);
+
+    // 系统设置
+    void set_zero_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetZero::Response> response);
+    void set_enable_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEnable::Response> response);
+    void set_emergency_callback(
+        const std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Request> request,
+        std::shared_ptr<lbot_arm_interfaces::srv::SetEmergency::Response> response);
+
+    /****************************** 服务器 ******************************/
+    
+    // 运动控制服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJ>::SharedPtr move_joint_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveJP>::SharedPtr move_pose_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::MoveL>::SharedPtr move_linear_service_;
+
+    // 运动学计算服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::ForwardKinematics>::SharedPtr forward_kinematics_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::InverseKinematics>::SharedPtr inverse_kinematics_service_;
+
+    // 工具坐标系管理服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetFrame>::SharedPtr set_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetFrame>::SharedPtr get_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetCurrentFrame>::SharedPtr get_current_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::ChangeFrame>::SharedPtr change_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::DeleteFrame>::SharedPtr delete_tool_frame_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::GetAllFrames>::SharedPtr get_all_tool_frames_service_;
+
+    // 系统设置服务器
+    rclcpp::Service<lbot_arm_interfaces::srv::SetZero>::SharedPtr set_zero_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEnable>::SharedPtr set_enable_service_;
+    rclcpp::Service<lbot_arm_interfaces::srv::SetEmergency>::SharedPtr set_emergency_service_;
+
+    /****************************** 灵巧手Topic ******************************/
+
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l6_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l6_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l6_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_joint_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_force_sub_;
+    rclcpp::Subscription<std_msgs::msg::UInt8MultiArray>::SharedPtr right_hand_l10_speed_sub_;
+    rclcpp::Subscription<std_msgs::msg::Int32MultiArray>::SharedPtr right_hand_l20_joint_sub_;
+};
+
+}  // namespace lbot_driver
+
+#endif // LBOT_DRIVER_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 <stdbool.h>
+#include <stdint.h>
+
+
+// 机械臂类型枚举
+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
@@ -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
+                        
@@ -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)
@@ -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."
+
@@ -0,0 +1 @@
+liblbot_api_cpp.so.1.0.0
@@ -0,0 +1 @@
+liblbot_api_cpp.so.1.0.0
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>lbot_driver</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="miracove@163.com">wxp</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>geometry_msgs</depend>
+  <depend>tf2</depend>
+  <depend>tf2_geometry_msgs</depend>
+  <depend>lbot_arm_interfaces</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
Author	SHA1	Message	Date
weipengzhe	9b2279f2d2	修复了rosbag录制服务崩溃的bug	2026-09-17 16:34:27 +08:00
weipengzhe	c3613d9e41	已加入对机械臂反馈值和控制量的参数录制	2026-09-17 16:17:50 +08:00
weipengzhe	8b997e1e79	加入随sh脚本生命周期的使能和失能功能	2026-09-17 15:30:07 +08:00
weipengzhe	f0633b6ff7	完善提交规则	2026-09-17 13:44:10 +08:00
weipengzhe	50aba5217f	删除手部串联控制驱动,加入系统报错信息话题发布	2026-09-17 13:42:11 +08:00
weipengzhe	9479a8ccb2	机械臂与灵巧手位控逻辑已完备	2026-09-17 10:50:14 +08:00
weipengzhe	c649b25845	删掉了官方sdk	2026-09-17 09:48:36 +08:00
weipengzhe	3955956604	加入官方sdk,后续将在此基础上修改	2026-09-17 09:44:19 +08:00
weipengzhe	62b8e72e65	加入了关节软限位和控制话题字段	2026-09-17 09:36:03 +08:00