del step

.gitignore

@@ -0,0 +1,12 @@
+# 忽略所有层级的 build 文件夹及其内容
+**/build/
+
+# 忽略所有层级的 install 文件夹及其内容
+**/install/
+
+# 忽略所有层级的 log 文件夹及其内容
+**/log/
+
+ethercat_dev/csp_log.csv
+
+csp_log.csv

.vscode/c_cpp_properties.json

@@ -0,0 +1,18 @@
+{
+  "configurations": [
+    {
+      "name": "linux-gcc-x64",
+      "includePath": [
+        "${workspaceFolder}/**"
+      ],
+      "compilerPath": "/usr/bin/gcc",
+      "cStandard": "${default}",
+      "cppStandard": "${default}",
+      "intelliSenseMode": "linux-gcc-x64",
+      "compilerArgs": [
+        ""
+      ]
+    }
+  ],
+  "version": 4
+}

.vscode/launch.json

@@ -0,0 +1,24 @@
+{
+  "version": "0.2.0",
+  "configurations": [
+    {
+      "name": "C/C++ Runner: Debug Session",
+      "type": "cppdbg",
+      "request": "launch",
+      "args": [],
+      "stopAtEntry": false,
+      "externalConsole": false,
+      "cwd": "/home/demo/hivecore_robot_os1/hivecore_robot_drivers/motor_dev/src",
+      "program": "/home/demo/hivecore_robot_os1/hivecore_robot_drivers/motor_dev/src/build/Debug/outDebug",
+      "MIMode": "gdb",
+      "miDebuggerPath": "gdb",
+      "setupCommands": [
+        {
+          "description": "Enable pretty-printing for gdb",
+          "text": "-enable-pretty-printing",
+          "ignoreFailures": true
+        }
+      ]
+    }
+  ]
+}

.vscode/settings.json

@@ -0,0 +1,136 @@
+{
+  "files.associations": {
+    "cctype": "cpp",
+    "clocale": "cpp",
+    "cmath": "cpp",
+    "csignal": "cpp",
+    "cstdarg": "cpp",
+    "cstddef": "cpp",
+    "cstdio": "cpp",
+    "cstdlib": "cpp",
+    "cstring": "cpp",
+    "ctime": "cpp",
+    "cwchar": "cpp",
+    "cwctype": "cpp",
+    "any": "cpp",
+    "array": "cpp",
+    "atomic": "cpp",
+    "bit": "cpp",
+    "bitset": "cpp",
+    "chrono": "cpp",
+    "codecvt": "cpp",
+    "compare": "cpp",
+    "complex": "cpp",
+    "concepts": "cpp",
+    "condition_variable": "cpp",
+    "cstdint": "cpp",
+    "deque": "cpp",
+    "forward_list": "cpp",
+    "list": "cpp",
+    "map": "cpp",
+    "set": "cpp",
+    "string": "cpp",
+    "unordered_map": "cpp",
+    "unordered_set": "cpp",
+    "vector": "cpp",
+    "exception": "cpp",
+    "algorithm": "cpp",
+    "functional": "cpp",
+    "iterator": "cpp",
+    "memory": "cpp",
+    "memory_resource": "cpp",
+    "numeric": "cpp",
+    "optional": "cpp",
+    "random": "cpp",
+    "ratio": "cpp",
+    "regex": "cpp",
+    "string_view": "cpp",
+    "system_error": "cpp",
+    "tuple": "cpp",
+    "type_traits": "cpp",
+    "utility": "cpp",
+    "fstream": "cpp",
+    "future": "cpp",
+    "initializer_list": "cpp",
+    "iomanip": "cpp",
+    "iosfwd": "cpp",
+    "iostream": "cpp",
+    "istream": "cpp",
+    "limits": "cpp",
+    "mutex": "cpp",
+    "new": "cpp",
+    "numbers": "cpp",
+    "ostream": "cpp",
+    "semaphore": "cpp",
+    "sstream": "cpp",
+    "stdexcept": "cpp",
+    "stop_token": "cpp",
+    "streambuf": "cpp",
+    "thread": "cpp",
+    "cfenv": "cpp",
+    "cinttypes": "cpp",
+    "typeindex": "cpp",
+    "typeinfo": "cpp",
+    "valarray": "cpp",
+    "variant": "cpp",
+    "filesystem": "cpp"
+  },
+  "C_Cpp_Runner.cCompilerPath": "gcc",
+  "C_Cpp_Runner.cppCompilerPath": "g++",
+  "C_Cpp_Runner.debuggerPath": "gdb",
+  "C_Cpp_Runner.cStandard": "",
+  "C_Cpp_Runner.cppStandard": "",
+  "C_Cpp_Runner.msvcBatchPath": "",
+  "C_Cpp_Runner.useMsvc": false,
+  "C_Cpp_Runner.warnings": [
+    "-Wall",
+    "-Wextra",
+    "-Wpedantic",
+    "-Wshadow",
+    "-Wformat=2",
+    "-Wcast-align",
+    "-Wconversion",
+    "-Wsign-conversion",
+    "-Wnull-dereference"
+  ],
+  "C_Cpp_Runner.msvcWarnings": [
+    "/W4",
+    "/permissive-",
+    "/w14242",
+    "/w14287",
+    "/w14296",
+    "/w14311",
+    "/w14826",
+    "/w44062",
+    "/w44242",
+    "/w14905",
+    "/w14906",
+    "/w14263",
+    "/w44265",
+    "/w14928"
+  ],
+  "C_Cpp_Runner.enableWarnings": true,
+  "C_Cpp_Runner.warningsAsError": false,
+  "C_Cpp_Runner.compilerArgs": [],
+  "C_Cpp_Runner.linkerArgs": [],
+  "C_Cpp_Runner.includePaths": [],
+  "C_Cpp_Runner.includeSearch": [
+    "*",
+    "**/*"
+  ],
+  "C_Cpp_Runner.excludeSearch": [
+    "**/build",
+    "**/build/**",
+    "**/.*",
+    "**/.*/**",
+    "**/.vscode",
+    "**/.vscode/**"
+  ],
+  "C_Cpp_Runner.useAddressSanitizer": false,
+  "C_Cpp_Runner.useUndefinedSanitizer": false,
+  "C_Cpp_Runner.useLeakSanitizer": false,
+  "C_Cpp_Runner.showCompilationTime": false,
+  "C_Cpp_Runner.useLinkTimeOptimization": false,
+  "C_Cpp_Runner.msvcSecureNoWarnings": false,
+  "Codegeex.RepoIndex": true
+}

aikit_tts/CMakeLists.txt

@@ -0,0 +1,123 @@
+cmake_minimum_required(VERSION 3.8)
+project(aikit_tts)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# 查找依赖
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+
+# 科大讯飞SDK路径配置 - 关键修改
+set(XUNFEI_SDK_LIB_DIRS 
+  ${CMAKE_CURRENT_SOURCE_DIR}/libs
+)
+
+# -------------------------- 新增：资源路径配置 --------------------------
+# 1. 定义资源源路径（相对当前CMakeLists.txt的路径，即resources/tts）
+set(TTS_RESOURCE_SRC_DIR ${CMAKE_CURRENT_SOURCE_DIR}/resources/tts)
+# 2. 定义资源安装路径（ROS 2标准路径：share/包名/tts）
+# 安装后实际路径：/opt/ros/humble/share/aikit_tts/tts/
+# 显式指定资源安装路径，避免CMAKE_INSTALL_DATADIR解析异常
+set(TTS_RESOURCE_INSTALL_DIR share/${PROJECT_NAME}/tts)
+
+# -----------------------------------------------------------------------
+
+
+# 添加头文件路径
+include_directories(
+  include
+  ${CMAKE_CURRENT_SOURCE_DIR}/include  # 项目自身头文件
+)
+
+# 添加库文件路径
+link_directories(
+  ${XUNFEI_SDK_LIB_DIRS}
+)
+
+# 打印库路径用于调试
+foreach(LIB_DIR ${XUNFEI_SDK_LIB_DIRS})
+  if(EXISTS "${LIB_DIR}/ebd1bade4_v1025_aee.so")
+    message(STATUS "找到ebd1bade4_v1025_aee.so: ${LIB_DIR}/ebd1bade4_v1025_aee.so")
+  endif()
+  if(EXISTS "${LIB_DIR}/libaikit.so")
+    message(STATUS "找到libaikit.so: ${LIB_DIR}/libaikit.so")
+  endif()
+endforeach()
+
+# 编译节点
+add_executable(aikit_tts_node 
+  src/aikit_tts_node.cpp 
+)
+
+# 依赖项链接
+ament_target_dependencies(aikit_tts_node
+  rclcpp
+  std_msgs
+)
+
+# -------------------------- 新增：传递资源路径给程序 --------------------------
+# 通过宏定义TTS_RESOURCE_PATH，把安装路径传给cpp代码（末尾加/方便拼接文件名）
+target_compile_definitions(aikit_tts_node
+  PRIVATE
+  TTS_RESOURCE_PATH="${TTS_RESOURCE_INSTALL_DIR}/"
+)
+# -----------------------------------------------------------------------
+
+# 链接库
+target_link_libraries(aikit_tts_node
+  -L${XUNFEI_SDK_LIB_DIRS}  # 显式指定库路径
+  aikit  # 科大讯飞主库
+  dl pthread m rt asound
+)
+
+# 安装可执行文件
+install(TARGETS
+  aikit_tts_node
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+# 安装SDK库文件到系统能找到的地方
+install(DIRECTORY ${XUNFEI_SDK_LIB_DIRS}/
+  DESTINATION lib
+  FILES_MATCHING PATTERN "*.so*"
+)
+
+# -------------------------- 新增：安装资源文件 --------------------------
+# 把resources/tts下的.dat和.irf文件，安装到前面定义的TTS_RESOURCE_INSTALL_DIR
+# 安装资源文件（修改后）
+install(
+  DIRECTORY ${TTS_RESOURCE_SRC_DIR}/
+  # 拼接安装根目录，最终路径是 install/share/aikit_tts/tts/
+  DESTINATION ${CMAKE_INSTALL_PREFIX}/${TTS_RESOURCE_INSTALL_DIR}
+  FILES_MATCHING
+  PATTERN "*.dat"
+  PATTERN "*.irf"
+)
+
+# -----------------------------------------------------------------------
+
+# 安装头文件
+install(DIRECTORY include/
+  DESTINATION include
+)
+
+# 安装config和launch目录（原有配置）
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/config)
+  install(DIRECTORY config/
+    DESTINATION share/${PROJECT_NAME}/config
+  )
+endif()
+
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/launch)
+  install(DIRECTORY launch/
+    DESTINATION share/${PROJECT_NAME}/launch
+  )
+endif()
+
+ament_export_include_directories(include)
+ament_export_dependencies(rclcpp std_msgs)
+
+ament_package()

aikit_tts/include/aikit_biz_api.h

@@ -0,0 +1,337 @@
+//
+// Created by xkzhang9 on 2020/10/16.
+//
+
+#ifndef AIKIT_BIZ_API_H
+#define AIKIT_BIZ_API_H
+
+#include "aikit_biz_type.h"
+#include "aikit_biz_builder.h"
+
+namespace AIKIT {
+
+/**
+ * SDK初始化函数用以初始化整个SDK
+ * 初始化相关配置参数通过AIKIT_Congigurator配置
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_Init();
+
+
+/**[deprecated]
+ * SDK初始化函数用以初始化整个SDK
+ * @param param  SDK配置参数
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_Init(AIKIT_InitParam* param);
+
+/** 
+ * SDK逆初始化函数用以释放SDK所占资源
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_UnInit();
+
+/**
+ * 注册回调函数用以返回执行结果
+ * @param onOutput  能力实际输出回调
+ * @param onEvent   能力执行事件回调
+ * @param onError   能力执行错误回调
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_RegisterCallback(AIKIT_Callbacks cbs);
+
+/**
+ * 注册回调函数用以返回执行结果
+ * @param ability   [in]  能力唯一标识
+ * @param onOutput  能力实际输出回调
+ * @param onEvent   能力执行事件回调
+ * @param onError   能力执行错误回调
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_RegisterAbilityCallback(const char* ability, AIKIT_Callbacks cbs);
+
+/**
+ * 初始化能力引擎
+ * @param ability   [in]  能力唯一标识
+ * @param param     [in] 初始化参数
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_EngineInit(const char* ability, AIKIT_BizParam* param);
+
+/**
+ * 能力引擎逆初始化,释放能力及对应引擎占用所有资源
+ * @param ability   [in]  能力唯一标识
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_EngineUnInit(const char* ability);
+
+/**
+ * 个性化数据预处理
+ * @param   ability     [in]  能力唯一标识
+ * @param   srcData     [in]  原始数据输入
+ * @param   data        [out] 结果数据输出
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_PreProcess(const char* ability, AIKIT_CustomData* srcData, AIKIT_CustomData** data);
+
+/**
+ * 离线个性化数据加载
+ * @param ability    能力唯一标识
+ * @param data      个性化数据
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_LoadData(const char* ability, AIKIT_CustomData* data);
+
+/**
+ * 在线个性化数据上传
+ * @param ability   能力唯一标识
+ * @param params    个性化参数
+ * @param data      个性化数据
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_LoadDataAsync(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* data, void* usrContext, AIKIT_HANDLE** outHandle);
+
+/**
+ * 个性化数据查询
+ * @param ability    能力唯一标识
+ * @param data      个性化数据
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_QueryData(const char* ability, AIKIT_CustomData* data);
+
+/**
+ * @brief 可见即可说（AIUI定制接口）
+ * @param  ability  能力唯一标识
+ * @param params    个性化参数
+ * @param data      个性化数据
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_LoadDataSpeakableAsync(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* data, void* usrContext, AIKIT_HANDLE** outHandle);
+
+/**
+ * 个性化数据卸载
+ * @param  ability      能力唯一标识
+ * @param  key          个性化数据唯一标识
+ * @param  index        个性化数据索引
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_UnLoadData(const char* ability, const char* key, int index);
+
+/**
+ * 指定要使用的个性化数据集合，未调用，则默认使用所有AIKIT_LoadData加载的数据
+ * 可调用多次以使用不同key集合
+ * @param  abilityId  能力唯一标识
+ * @param  key        个性化数据唯一标识数组
+ * @param  index      个性化数据索引数组
+ * @param  count      个性化数据索引数组成员个数
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t  AIKIT_SpecifyDataSet(const char* ability, const char* key, int index[], int count);
+
+/**
+ * 启动one-shot模式能力同步模式调用
+ * @param   ability    能力唯一标识
+ * @param   param      能力参数
+ * @param   inputData  能力数据输入
+ * @param   outputData 能力数据输出
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_OneShot(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* inputData, AIKIT_OutputData** outputData);
+
+/**
+ * 启动one-shot模式能力异步模式调用
+ * @param ability    能力唯一标识d
+ * @param param      能力参数
+ * @param data       能力数据输入
+ * @param usrContext 上下文指针
+ * @param outHandle  生成的引擎会话句柄
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_OneShotAsync(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* data, void* usrContext, AIKIT_HANDLE** outHandle);
+
+/**
+ * 启动会话模式能力调用实例，若引擎未初始化，则内部首先完成引擎初始化
+ * @param ability   能力唯一标识
+ * @param len       ability长度
+ * @param param     初始化参数
+ * @param usrContext上下文指针
+ * @param outHandle 生成的引擎会话句柄
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_Start(const char* ability, AIKIT_BizParam* param, void* usrContext, AIKIT_HANDLE** outHandle);
+
+/**
+ * 会话模式输入数据
+ * @param handle    会话实例句柄
+ * @param input     输入数据
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_Write(AIKIT_HANDLE* handle, AIKIT_InputData* input);
+
+/**
+ * 会话模式同步读取数据
+ * @param handle    会话实例句柄
+ * @param output     输入数据
+ * @return 结果错误码，0=成功
+ * @note  output内存由SDK自行维护,无需主动释放
+ */
+AIKITAPI int32_t AIKIT_Read(AIKIT_HANDLE* handle, AIKIT_OutputData** output);
+
+/**
+ * 结束会话实例
+ * @param handle    会话实例句柄
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_End(AIKIT_HANDLE* handle);
+
+
+/**
+ * 释放能力占用资源,注意不会释放引擎实例
+ * 若要释放能力及能力所有资源，需调用AIKIT_EngineUnInit()
+ * @param   ability    能力唯一标识
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_FreeAbility(const char* ability);
+
+/**
+ * 设置日志级别
+ * @param  level    日志级别
+ * @return 错误码 0=成功，其他表示失败
+*/
+AIKITAPI int32_t AIKIT_SetLogLevel(int32_t level);
+
+/**
+ * 设置日志输出模式
+ * @param  mode    日志输出模式
+ * @return 错误码 0=成功，其他表示失败
+*/
+AIKITAPI int32_t AIKIT_SetLogMode(int32_t mode);
+
+/**
+ * 输出模式为文件时，设置日志文件名称
+ * @param  path    日志名称
+ * @return 错误码 0=成功，其他表示失败
+*/
+AIKITAPI int32_t AIKIT_SetLogPath(const char* path);
+
+/** 
+ * 获取设备ID
+ * @param deviceID    设备指纹输出字符串
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_GetDeviceID(const char** deviceID);
+
+/** 
+ * 设置授权更新间隔，单位为秒，默认为300秒
+ * AIKIT_Init前设置
+ * @param interval    间隔秒数
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_SetAuthCheckInterval(uint32_t interval);
+
+/** 
+ * 强制更新授权
+ * 注意：注意需在AIKIT_Init调用成功后，方可调用
+ * @param timeout    超时时间 单位为秒 
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_UpdateAuth(uint32_t timeout);
+
+/** 
+ * 获取能力授权剩余秒数
+ * 能力输入参数为空时，默认返回最接近授权过期的能力剩余秒数
+ * 注意：注意需在AIKIT_Init调用成功后，方可调用
+ * @param leftTime   返回的能力授权剩余秒数，0 表示永久授权， 负值表示已过期的秒数
+ * @param ability    能力id标识
+ * @return 返回调用结果，0 = 成功， 其他值表示调用失败
+ */
+AIKITAPI int AIKIT_GetAuthLeftTime(int64_t& leftTime,int64_t& authEndTime, const char* ability = nullptr);
+
+/** 
+ * 获取SDK版本号
+ * @return SDK版本号
+ */
+AIKITAPI const char* AIKIT_GetVersion();
+
+/**
+ * @brief 获取能力对应的引擎版本
+ * 
+ * @param ability 能力唯一标识
+ * @return const* 引擎版本号
+ */
+AIKITAPI const char* AIKIT_GetEngineVersion(const char* ability);
+
+/**
+ * 本地日志是否开启
+ * @param open
+ * @return
+ */
+AIKITAPI int32_t AIKIT_SetILogOpen(bool open);
+
+/**
+ * 本地日志最大存储个数（【1，300】）
+ * @param count
+ * @return
+ */
+AIKITAPI int32_t AIKIT_SetILogMaxCount(uint32_t count);
+
+/**
+ * 设置单日志文件大小（(0，10M】）
+ * @param bytes
+ * @return
+ */
+AIKITAPI int32_t AIKIT_SetILogMaxSize(long long bytes);
+
+/**
+ * 设置SDK相关配置
+ * @param key    参数名字
+ * @param value  参数值
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_SetConfig(const char* key, const void* value);
+
+/**
+ * 设置SDK内存模式
+ * @param ability 能力id
+ * @param mode    模式，取值参见 AIKIT_MEMORY_MODE
+ * @return AIKITAPI 
+ */
+AIKITAPI int32_t AIKIT_SetMemoryMode(const char* ability,int32_t mode);
+
+
+/**自2.1.6版本开始已废弃接口，当前仅保留空实现接口声明
+ * 自2.1.6版本开始AIKIT_OneShot响应数据由SDK内部自己维护，无需用户维护
+ * 同步接口响应数据缓存释放接口
+ * @param  outputData    由同步接口AIKIT_OneShot获取的响应结果数据
+ */
+AIKITAPI int32_t AIKIT_Free(AIKIT_OutputData** outputData);
+
+/**自2.1.6版本开始已废弃接口，由AIKIT_FreeAbility替代
+ * 释放能力占用资源
+ * @param   ability    能力唯一标识
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_GC(const char* ability);
+
+/**
+ * 设置日志级别，模式，以及保存路径,旧版日志接口，不推荐使用
+ * @param  level    日志级别
+ * @param  mode     日志输出模式
+ * @param  level    输出模式为文件时的文件名称
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_SetLogInfo(int32_t level, int32_t mode, const char* path);
+
+/**
+ * 获取能力授权剩余秒数
+ * 能力输入参数为空时，默认返回最接近授权过期的能力剩余秒数
+ * 注意：注意需在AEE_Init调用成功后，方可调用
+ * @param leftTime   返回的能力授权剩余秒数，0 表示永久授权， 负值表示已过期的秒数
+ * @param ability    能力id标识
+ * @return 返回调用结果，0 = 成功， 其他值表示调用失败
+ */
+AIKITAPI int AIKIT_GetAuthLeftTime(int64_t& leftTime, const char* ability = nullptr);
+
+}  // namespace AIKIT
+
+#endif  //AIKIT_BIZ_API_H

aikit_tts/include/aikit_biz_api_c.h

@@ -0,0 +1,437 @@
+//
+// Created by chaoxu8 on 2021/07/01.
+//
+
+#ifndef AIKIT_BIZ_API_C_H
+#define AIKIT_BIZ_API_C_H
+
+#ifndef __cplusplus
+#include <stdbool.h>
+#endif
+#include "aikit_biz_type.h"
+#include "../api_aee/aee_biz_api_c.h"
+
+#ifndef AEE_BIZ_API_C_H
+// 构造器类型
+typedef enum BuilderType_ {
+    BUILDER_TYPE_PARAM,       // 参数输入
+    BUILDER_TYPE_DATA         // 数据输入
+} BuilderType;
+
+// 构造器输入数据类型
+typedef enum BuilderDataType_ {
+    DATA_TYPE_TEXT,           // 文本
+    DATA_TYPE_AUDIO,          // 音频
+    DATA_TYPE_IMAGE,          // 图片
+    DATA_TYPE_VIDEO           // 视频
+} BuilderDataType;
+// 构造器输入数据结构体
+typedef struct BuilderData_ {
+    int type;                 // 数据类型
+    const char* name;         // 数据段名
+    void* data;               // 数据段实体（当送入路径时，此处传入路径地址字符串指针即可；
+                              //            当送入文件句柄时，此处传入文件句柄指针即可）
+    int len;                  // 数据段长度（当送入路径或文件句柄时，此处传0即可）
+    int status;               // 数据段状态，参考AIKIT_DataStatus枚举
+} BuilderData;
+#endif
+
+// 构造器上下文及句柄
+typedef struct AIKITBuilderContext_ {
+    void* builderInst;        // 构造器内部类句柄
+    BuilderType type;         // 构造器类型
+} AIKITBuilderContext, *AIKITBuilderHandle;
+
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief 初始化输入构造器
+ * @param type 构造器类型
+ * @return 构造器句柄，失败返回nullptr
+ */
+AIKITAPI AIKITBuilderHandle AIKITBuilder_Create(BuilderType type);
+typedef AIKITBuilderHandle(*AIKITBuilder_Create_Ptr)(BuilderType type);
+
+/**
+ * @brief 添加整型参数
+ * @param handle 构造器句柄
+ * @param name 参数名称
+ * @param value 参数值
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int AIKITBuilder_AddInt(AIKITBuilderHandle handle, const char *key, int value);
+typedef int(*AIKITBuilder_AddInt_Ptr)(AIKITBuilderHandle handle, const char *key, int value);
+
+/**
+ * @brief 添加字符串型参数
+ * @param handle 构造器句柄
+ * @param name 参数名称
+ * @param str 参数值
+ * @param len 字符串长度
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int AIKITBuilder_AddString(AIKITBuilderHandle handle, const char *key, const char *value, int len);
+typedef int(*AIKITBuilder_AddString_Ptr)(AIKITBuilderHandle handle, const char *key, const char *value, int len);
+
+/**
+ * @brief 添加布尔型参数
+ * @param handle 构造器句柄
+ * @param name 参数名称
+ * @param value 参数值
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int AIKITBuilder_AddBool(AIKITBuilderHandle handle, const char *key, bool value);
+typedef int(*AIKITBuilder_AddBool_Ptr)(AIKITBuilderHandle handle, const char *key, bool value);
+
+/**
+ * @brief 添加浮点型参数
+ * @param handle 构造器句柄
+ * @param name 参数名称
+ * @param value 参数值
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int AIKITBuilder_AddDouble(AIKITBuilderHandle handle, const char *key, double value);
+typedef int(*AIKITBuilder_AddDouble_Ptr)(AIKITBuilderHandle handle, const char *key, double value);
+
+/**
+ * @brief 添加输入数据
+ * @param handle 构造器句柄
+ * @param data 数据结构体实例
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int AIKITBuilder_AddBuf(AIKITBuilderHandle handle, BuilderData *data);
+typedef int(*AIKITBuilder_AddBuf_Ptr)(AIKITBuilderHandle handle, BuilderData *data);
+
+/**
+ * @brief 添加输入数据（以路径方式）
+ * @param handle 构造器句柄
+ * @param data 数据结构体实例
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int AIKITBuilder_AddPath(AIKITBuilderHandle handle, BuilderData *data);
+typedef int(*AIKITBuilder_AddPath_Ptr)(AIKITBuilderHandle handle, BuilderData *data);
+
+/**
+ * @brief 添加输入数据（以文件对象方式）
+ * @param handle 构造器句柄
+ * @param data 数据结构体实例
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int AIKITBuilder_AddFile(AIKITBuilderHandle handle, BuilderData *data);
+typedef int(*AIKITBuilder_AddFile_Ptr)(AIKITBuilderHandle handle, BuilderData *data);
+
+/**
+ * @brief 构建输入参数
+ * @param handle 构造器句柄
+ * @return 参数结构化指针，失败返回nullptr
+ */
+AIKITAPI AIKIT_BizParam* AIKITBuilder_BuildParam(AIKITBuilderHandle handle);
+typedef AIKIT_BizParam*(*AIKITBuilder_BuildParam_Ptr)(AIKITBuilderHandle handle);
+
+/**
+ * @brief 构建输入数据
+ * @param handle 构造器句柄
+ * @return 数据结构化指针，失败返回nullptr
+ */
+AIKITAPI AIKIT_InputData* AIKITBuilder_BuildData(AIKITBuilderHandle handle);
+typedef AIKIT_InputData*(*AIKITBuilder_BuildData_Ptr)(AIKITBuilderHandle handle);
+
+/**
+ * @brief 清空输入构造器
+ * @param handle 构造器句柄
+ * @return 无
+ */
+AIKITAPI void AIKITBuilder_Clear(AIKITBuilderHandle handle);
+typedef void(*AIKITBuilder_Clear_Ptr)(AIKITBuilderHandle handle);
+
+/**
+ * @brief 销毁输入构造器
+ * @param handle 构造器句柄
+ * @return 无
+ */
+AIKITAPI void AIKITBuilder_Destroy(AIKITBuilderHandle handle);
+typedef void(*AIKITBuilder_Destroy_Ptr)(AIKITBuilderHandle handle);
+
+/**
+ * SDK初始化函数用以初始化整个SDK
+ * @param param  SDK配置参数
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_Init(AIKIT_InitParam* param);
+typedef int32_t(*AIKIT_Init_Ptr)(AIKIT_InitParam* param);
+
+/** 
+ * SDK逆初始化函数用以释放SDK所占资源
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_UnInit();
+typedef int32_t(*AIKIT_UnInit_Ptr)();
+
+/**
+ * 注册回调函数用以返回执行结果
+ * @param onOutput  能力实际输出回调
+ * @param onEvent   能力执行事件回调
+ * @param onError   能力执行错误回调
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_RegisterCallback(AIKIT_Callbacks cbs);
+typedef int32_t(*AIKIT_RegisterCallback_Ptr)(AIKIT_Callbacks cbs);
+
+/**
+ * 注册回调函数用以返回执行结果
+ * @param ability   [in]  能力唯一标识
+ * @param onOutput  能力实际输出回调
+ * @param onEvent   能力执行事件回调
+ * @param onError   能力执行错误回调
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_RegisterAbilityCallback(const char* ability, AIKIT_Callbacks cbs);
+typedef int32_t(*AIKIT_RegisterAbilityCallback_Ptr)(const char* ability, AIKIT_Callbacks cbs);
+
+/**
+ * 初始化能力引擎
+ * @param ability   [in]  能力唯一标识
+ * @param param     [in] 初始化参数
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_EngineInit(const char* ability, AIKIT_BizParam* param);
+typedef int32_t(*AIKIT_EngineInit_Ptr)(const char* ability, AIKIT_BizParam* param);
+
+/**
+ * 能力引擎逆初始化
+ * @param ability   [in]  能力唯一标识
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_EngineUnInit(const char* ability);
+typedef int32_t(*AIKIT_EngineUnInit_Ptr)(const char* ability);
+
+/**
+ * 个性化数据预处理
+ * @param   ability     [in]  能力唯一标识
+ * @param   srcData     [in]  原始数据输入
+ * @param   data        [out] 结果数据输出
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_PreProcess(const char* ability, AIKIT_CustomData* srcData, AIKIT_CustomData** data);
+typedef int32_t(*AIKIT_PreProcess_Ptr)(const char* ability, AIKIT_CustomData* srcData, AIKIT_CustomData** data);
+
+/**
+ * 个性化数据加载
+ * @param ability    能力唯一标识
+ * @param data      个性化数据
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_LoadData(const char* ability, AIKIT_CustomData* data);
+typedef int32_t(*AIKIT_LoadData_Ptr)(const char* ability, AIKIT_CustomData* data);
+
+/**
+ * 个性化数据加载
+ * @param  ability      能力唯一标识
+ * @param  key          个性化数据唯一标识
+ * @param  index        个性化数据索引
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_UnLoadData(const char* ability, const char* key, int index);
+typedef int32_t(*AIKIT_UnLoadData_Ptr)(const char* ability, const char* key, int index);
+
+/**
+ * 指定要使用的个性化数据集合，未调用，则默认使用所有AIKIT_LoadData加载的数据
+ * 可调用多次以使用不同key集合
+ * @param  abilityId  能力唯一标识
+ * @param  key        个性化数据唯一标识数组
+ * @param  index      个性化数据索引数组
+ * @param  count      个性化数据索引数组成员个数
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t  AIKIT_SpecifyDataSet(const char* ability, const char* key, int index[], int count);
+typedef int32_t(*AIKIT_SpecifyDataSet_Ptr)(const char* ability, const char* key, int index[], int count);
+
+/**
+ * 启动one-shot模式能力同步模式调用
+ * @param   ability    能力唯一标识
+ * @param   param      能力参数
+ * @param   inputData  能力数据输入
+ * @param   outputData 能力数据输出
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_OneShot(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* inputData, AIKIT_OutputData** outputData);
+typedef int32_t(*AIKIT_OneShot_Ptr)(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* inputData, AIKIT_OutputData** outputData);
+
+
+/**
+ * 启动one-shot模式能力异步模式调用
+ * @param ability    能力唯一标识d
+ * @param param      能力参数
+ * @param data       能力数据输入
+ * @param usrContext 上下文指针
+ * @param outHandle  生成的引擎会话句柄
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_OneShotAsync(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* data, void* usrContext, AIKIT_HANDLE** outHandle);
+typedef int32_t(*AIKIT_OneShotAsync_Ptr)(const char* ability, AIKIT_BizParam* params, AIKIT_InputData* data, void* usrContext, AIKIT_HANDLE** outHandle);
+
+/**
+ * 启动会话模式能力调用实例，若引擎未初始化，则内部首先完成引擎初始化
+ * @param ability   能力唯一标识
+ * @param len       ability长度
+ * @param param     初始化参数
+ * @param usrContext上下文指针
+ * @param outHandle 生成的引擎会话句柄
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_Start(const char* ability, AIKIT_BizParam* param, void* usrContext, AIKIT_HANDLE** outHandle);
+typedef int32_t(*AIKIT_Start_Ptr)(const char* ability, AIKIT_BizParam* param, void* usrContext, AIKIT_HANDLE** outHandle);
+
+/**
+ * 会话模式输入数据
+ * @param handle    会话实例句柄
+ * @param input     输入数据
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_Write(AIKIT_HANDLE* handle, AIKIT_InputData* input);
+typedef int32_t(*AIKIT_Write_Ptr)(AIKIT_HANDLE* handle, AIKIT_InputData* input);
+
+/**
+ * 会话模式同步读取数据
+ * @param handle    会话实例句柄
+ * @param output     输入数据
+ * @return 结果错误码，0=成功
+ * @note  output内存由SDK自行维护,无需主动释放
+ */
+AIKITAPI int32_t AIKIT_Read(AIKIT_HANDLE* handle, AIKIT_OutputData** output);
+typedef int32_t(*AIKIT_Read_Ptr)(AIKIT_HANDLE* handle, AIKIT_OutputData** output);
+
+/**
+ * 结束会话实例
+ * @param handle    会话实例句柄
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_End(AIKIT_HANDLE* handle);
+typedef int32_t(*AIKIT_End_Ptr)(AIKIT_HANDLE* handle);
+
+
+/**
+ * 释放能力占用资源
+ * @param   ability    能力唯一标识
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_FreeAbility(const char* ability);
+typedef int32_t(*AIKIT_FreeAbility_Ptr)(const char* ability);
+
+/**
+ * 设置日志级别
+ * @param  level    日志级别
+ * @return 错误码 0=成功，其他表示失败
+*/
+AIKITAPI int32_t AIKIT_SetLogLevel(int32_t level);
+typedef int32_t(*AIKIT_SetLogLevel_Ptr)(int32_t level);
+
+/**
+ * 设置日志输出模式
+ * @param  mode    日志输出模式
+ * @return 错误码 0=成功，其他表示失败
+*/
+AIKITAPI int32_t AIKIT_SetLogMode(int32_t mode);
+typedef int32_t(*AIKIT_SetLogMode_Ptr)(int32_t mode);
+
+/**
+ * 输出模式为文件时，设置日志文件名称
+ * @param  path    日志名称
+ * @return 错误码 0=成功，其他表示失败
+*/
+AIKITAPI int32_t AIKIT_SetLogPath(const char* path);
+typedef int32_t(*AIKIT_SetLogPath_Ptr)(const char* path);
+
+/** 
+ * 获取设备ID
+ * @param deviceID    设备指纹输出字符串
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_GetDeviceID(const char** deviceID);
+typedef int32_t(*AIKIT_GetDeviceID_Ptr)(const char** deviceID);
+
+/** 
+ * 设置授权更新间隔，单位为秒，默认为300秒
+ * AIKIT_Init前设置
+ * @param interval    间隔秒数
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_SetAuthCheckInterval(uint32_t interval);
+typedef int32_t(*AIKIT_SetAuthCheckInterval_Ptr)(uint32_t interval);
+
+/** 
+ * 获取SDK版本号
+ * @return SDK版本号
+ */
+AIKITAPI const char* AIKIT_GetVersion();
+typedef const char*(*AIKIT_GetVersion_Ptr)();
+
+/**
+ * @brief 获取能力对应的引擎版本
+ * 
+ * @param ability 能力唯一标识
+ * @return const* 引擎版本号
+ */
+AIKITAPI const char* AIKIT_GetEngineVersion(const char* ability);
+typedef const char*(*AIKIT_GetEngineVersion_Ptr)(const char* ability);
+
+/**
+ * 本地日志是否开启
+ * @param open
+ * @return
+ */
+AIKITAPI int32_t AIKIT_SetILogOpen(bool open);
+typedef int32_t(*AIKIT_SetILogOpenPtr)(bool open);
+
+/**
+ * 本地日志最大存储个数（【1，300】）
+ * @param count
+ * @return
+ */
+AIKITAPI int32_t AIKIT_SetILogMaxCount(uint32_t count);
+typedef int32_t(*AIKIT_SetILogMaxCountPtr)(uint32_t count);
+
+/**
+ * 设置单日志文件大小（(0，10M】）
+ * @param bytes
+ * @return
+ */
+AIKITAPI int32_t AIKIT_SetILogMaxSize(long long bytes);
+typedef int32_t(*AIKIT_SetILogMaxSizePtr)(long long bytes);
+
+/**
+ * 设置SDK相关配置
+ * @param key    参数名字
+ * @param value  参数值
+ * @return 结果错误码，0=成功
+ */
+AIKITAPI int32_t AIKIT_SetConfig(const char* key, const void* value);
+typedef int32_t(*AIKIT_SetConfigPtr)(const char* key, const void* value);
+
+/**
+ * 设置SDK内存模式
+ * @param ability 能力id
+ * @param mode    模式，取值参见 AIKIT_MEMORY_MODE
+ * @return AIKITAPI 
+ */
+AIKITAPI int32_t AIKIT_SetMemoryMode(const char* ability,int32_t mode);
+typedef int32_t(*AIKIT_SetMemoryModePtr)(const char* ability,int32_t mode);
+
+/**
+ * 设置日志级别，模式，以及保存路径,旧版日志接口，不推荐使用
+ * @param  level    日志级别
+ * @param  mode     日志输出模式
+ * @param  level    输出模式为文件时的文件名称
+ * @return 错误码 0=成功，其他表示失败
+ */
+AIKITAPI int32_t AIKIT_SetLogInfo(int32_t level, int32_t mode, const char* path);
+typedef int32_t(*AIKIT_SetLogInfo_Ptr)(int32_t level, int32_t mode, const char* path);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // AIKIT_BIZ_API_C_H

aikit_tts/include/aikit_biz_builder.h

@@ -0,0 +1,299 @@
+#ifndef AIKIT_BIZ_BUILDER_H
+#define AIKIT_BIZ_BUILDER_H
+
+#include "aikit_biz_type.h"
+#include "aikit_biz_obsolete_builder.h"
+
+namespace AIKIT {
+
+/**
+ * 参数构造类
+ */
+class AIKITAPI AIKIT_ParamBuilder {
+public:
+    static AIKIT_ParamBuilder* create();
+    static void destroy(AIKIT_ParamBuilder* builder);
+    virtual ~AIKIT_ParamBuilder();
+
+    //当需要为能力会话设置header字段时，需调用此接口
+    //后续调用param设置的kv对，将全部追加到header字段
+    virtual AIKIT_ParamBuilder* header() = 0;
+    virtual AIKIT_ParamBuilder* header(const char* key, const char* data, uint32_t dataLen) = 0;
+    virtual AIKIT_ParamBuilder* header(const char* key, int value)                          = 0;
+    virtual AIKIT_ParamBuilder* header(const char* key, double value)                       = 0;
+    virtual AIKIT_ParamBuilder* header(const char* key, bool value)                         = 0;
+    //当需要设置能力会话功能参数时，需调用此接口
+    //后续调用param设置的kv对，将全部追加到能力功能参数字段
+    virtual AIKIT_ParamBuilder* service(const char* serviceID) = 0;
+    virtual AIKIT_ParamBuilder* service(const char* serviceID, AIKIT_ParamBuilder* value) = 0;
+
+    virtual AIKIT_ParamBuilder* param(const char* key, const char* data, uint32_t dataLen) = 0;
+    virtual AIKIT_ParamBuilder* param(const char* key, int value)                          = 0;
+    virtual AIKIT_ParamBuilder* param(const char* key, double value)                       = 0;
+    virtual AIKIT_ParamBuilder* param(const char* key, bool value)                         = 0;
+
+    //输出控制参数对设置
+    virtual AIKIT_ParamBuilder* param(const char* key, AIKIT_ParamBuilder* value)          = 0;
+    virtual AIKIT_BizParam* build() = 0;
+
+    virtual void clear() = 0;
+};
+
+/**
+ * @brief 数据构造类基类
+ * 
+ */
+class AIKITAPI AiData {
+public:
+    template <class T, class O>
+    class AIKITAPI AiDataHolder {
+    public:
+        virtual T* status(int status) = 0;
+        virtual T* begin()            = 0;
+        virtual T* cont()             = 0;
+        virtual T* end()              = 0;
+        virtual T* once()             = 0;
+
+        virtual T* data(const char* value,int dataLen) = 0;
+        virtual T* path(const char* path)              = 0;
+        virtual T* file(const FILE* file)              = 0;
+
+        virtual O* valid() = 0;
+    };
+
+    virtual ~AiData() = 0;
+};
+
+/**
+ * 文本数据构造类
+ * encoding 文本编码格式设置 默认值:"utf8"  常见取值:"utf8" "gbk" "gb2312"
+ * compress 文本压缩格式设置 默认值:"raw"   常见取职:"raw" "gzip"
+ * format   文本内容格式设置 默认值:"plain" 常见取值:"plain" "json" "xml"
+ */
+class AIKITAPI AiText : public AiData {
+public:
+    class AiTextHolder : public AiDataHolder<AiTextHolder,AiText> {
+    public:
+        virtual AiTextHolder* encoding(const char* encoding) = 0;
+        virtual AiTextHolder* compress(const char* compress) = 0;
+        virtual AiTextHolder* format(const char* format)     = 0;
+    };
+
+    static AiTextHolder* get(const char* key);
+    virtual ~AiText() = 0;
+public:
+    static constexpr char* const ENCODING_UTF8   = (char*)"utf8";
+    static constexpr char* const ENCODING_GBK    = (char*)"gbk";
+    static constexpr char* const ENCODING_GB2312 = (char*)"gb2312";
+    static constexpr char* const ENCODING_DEF    = ENCODING_UTF8;
+
+    static constexpr char* const COMPRESS_RAW  = (char*)"raw";
+    static constexpr char* const COMPRESS_GZIP = (char*)"gzip";
+    static constexpr char* const COMPRESS_DEF  = COMPRESS_RAW;
+
+    static constexpr char* const FORMAT_PLAIN = (char*)"plain";
+    static constexpr char* const FORMAT_JSON  = (char*)"json";
+    static constexpr char* const FORMAT_XML   = (char*)"xml";
+    static constexpr char* const FORMAT_DEF   = FORMAT_PLAIN;
+};
+
+/**
+ * 音频数据构造类
+ * encoding   音频编码格式设置 默认值:"speex-wb" 常见取值:"lame" "speex" "speex-wb" "opus" "opus-wb" "mp3" "wav" "amr"
+ * sampleRate 音频采样率设置   默认值:16000      常见取值:16000 8000
+ * channels   音频声道数设置   默认值:1          常见取值:1 2
+ * bitDepth   音频数据位深设置 默认值:16         常见取值:[16,8]
+ */
+class AIKITAPI  AiAudio : public AiData {
+public :
+    class AiAudioHolder : public AiDataHolder<AiAudioHolder,AiAudio> {
+    public:
+        virtual AiAudioHolder* encoding(const char* encoding) = 0;
+        virtual AiAudioHolder* sampleRate(int sampleRate)     = 0;
+        virtual AiAudioHolder* channels(int channels)         = 0;
+        virtual AiAudioHolder* bitDepth(int bitDepth)         = 0;
+    };
+
+    static AiAudioHolder* get(const char* key);
+    virtual ~AiAudio() = 0;
+public:
+    static constexpr char* const ENCODING_PCM      = (char*)"pcm";
+    static constexpr char* const ENCODING_RAW      = (char*)"raw";
+    static constexpr char* const ENCODING_ICO      = (char*)"ico";
+    static constexpr char* const ENCODING_SPEEX    = (char*)"speex";
+    static constexpr char* const ENCODING_SPEEX_WB = (char*)"speex-wb";
+    static constexpr char* const ENCODING_LAME     = (char*)"lame";
+    static constexpr char* const ENCODING_OPUS     = (char*)"opus";
+    static constexpr char* const ENCODING_OPUS_WB  = (char*)"opus-wb";
+    static constexpr char* const ENCODING_WAV      = (char*)"wav";
+    static constexpr char* const ENCODING_AMR      = (char*)"amr";
+    static constexpr char* const ENCODING_AMR_WB   = (char*)"amr-wb";
+    static constexpr char* const ENCODING_MP3      = (char*)"mp3";
+    static constexpr char* const ENCODING_CDA      = (char*)"cda";
+    static constexpr char* const ENCODING_WAVE     = (char*)"wave";
+    static constexpr char* const ENCODING_AIFF     = (char*)"aiff";
+    static constexpr char* const ENCODING_MPEG     = (char*)"mpeg";
+    static constexpr char* const ENCODING_MID      = (char*)"mid";
+    static constexpr char* const ENCODING_WMA      = (char*)"wma";
+    static constexpr char* const ENCODING_RA       = (char*)"ra";
+    static constexpr char* const ENCODING_RM       = (char*)"rm";
+    static constexpr char* const ENCODING_RMX      = (char*)"rmx";
+    static constexpr char* const ENCODING_VQF      = (char*)"vqf";
+    static constexpr char* const ENCODING_OGG      = (char*)"ogg";
+    static constexpr char* const ENCODING_DEF      = ENCODING_SPEEX_WB;
+
+    static const int SAMPLE_RATE_8K  = 8000;
+    static const int SAMPLE_RATE_16K = 16000;
+    static const int SAMPLE_RATE_DEF = SAMPLE_RATE_16K;
+
+    static const int CHANNELS_1   = 1;
+    static const int CHANNELS_2   = 2;
+    static const int CHANNELS_DEF = CHANNELS_1;
+
+    static const int BIT_DEPTH_8   = 8;
+    static const int BIT_DEPTH_16  = 16;
+    static const int BIT_DEPTH_DEF = BIT_DEPTH_16;
+};
+
+/**
+ * 图片数据构造类
+ * encoding 图片编码格式设置 默认值:"jpg" 常见取值:"raw" "rgb" "bgr" "yuv" "jpg" "jpeg" "png" "bmp"
+ * width     图片宽度设置 默认值:null    常见取值:根据具体图片文件各不相同
+ * height    图片高度设置 默认值:null    常见取值:根据具体图片文件各不相同
+ * dims      图片深度设置 默认值:null    常见取值:根据具体图片文件各不相同
+ */
+class AIKITAPI AiImage : public AiData {
+public:
+    class AiImageHolder : public AiDataHolder<AiImageHolder,AiImage> {
+    public:
+        virtual AiImageHolder* encoding(const char* encoding) = 0;
+        virtual AiImageHolder* width(int width)               = 0;
+        virtual AiImageHolder* height(int height)             = 0;
+        virtual AiImageHolder* dims(int dims)                 = 0;
+    };
+
+    static AiImageHolder* get(const char* key);
+    virtual ~AiImage() = 0;
+public:
+    static constexpr char* const ENCODING_RAW    = (char*)"raw";
+    static constexpr char* const ENCODING_JPG    = (char*)"jpg";
+    static constexpr char* const ENCODING_JPEG   = (char*)"jpeg";
+    static constexpr char* const ENCODING_PNG    = (char*)"png";
+    static constexpr char* const ENCODING_APNG   = (char*)"apng";
+    static constexpr char* const ENCODING_BMP    = (char*)"bmp";
+    static constexpr char* const ENCODING_WEBP   = (char*)"webp";
+    static constexpr char* const ENCODING_TIFF   = (char*)"tiff";
+    static constexpr char* const ENCODING_RGB565 = (char*)"rgb565";
+    static constexpr char* const ENCODING_RGB888 = (char*)"rgb888";
+    static constexpr char* const ENCODING_BGR565 = (char*)"bgr565";
+    static constexpr char* const ENCODING_BGR888 = (char*)"bgr888";
+    static constexpr char* const ENCODING_YUV12  = (char*)"yuv12";
+    static constexpr char* const ENCODING_YUV21  = (char*)"yuv21";
+    static constexpr char* const ENCODING_YUV420 = (char*)"yuv420";
+    static constexpr char* const ENCODING_YUV422 = (char*)"yuv422";
+    static constexpr char* const ENCODING_PSD    = (char*)"psd";
+    static constexpr char* const ENCODING_PCD    = (char*)"pcd";
+    static constexpr char* const ENCODING_DEF    = ENCODING_JPG;
+};
+
+/**
+ * 视频数据构造类
+ * encoding  视频编码设置 默认值:"h264" 常见取值:"avi" "rmvb" "flv" "h26x" "mpeg"
+ * width     视频宽度设置 默认值:null   常见取值:根据具体视频文件各不相同
+ * height    视频高度设置 默认值:null   常见取值:根据具体视频文件各不相同
+ * frameRate 视频帧率设置 默认值:null   常见取值:根据具体视频文件各不相同
+ */
+class AIKITAPI AiVideo : public AiData {
+public:
+    class AiVideoHolder : public AiDataHolder<AiVideoHolder,AiVideo> {
+    public:
+        virtual AiVideoHolder* encoding(const char* key) = 0;
+        virtual AiVideoHolder* width(int width)          = 0;
+        virtual AiVideoHolder* height(int height)        = 0;
+        virtual AiVideoHolder* frameRate(int frameRate)  = 0;
+    };
+
+    static AiVideoHolder* get(const char* key);
+    virtual ~AiVideo() = 0;
+public:
+    static constexpr char* const ENCODING_H264 = (char*)"h264";
+    static constexpr char* const ENCODING_H265 = (char*)"h265";
+    static constexpr char* const ENCODING_AVI  = (char*)"avi";
+    static constexpr char* const ENCODING_NAVI = (char*)"navi";
+    static constexpr char* const ENCODING_MP4  = (char*)"mp4";
+    static constexpr char* const ENCODING_RM   = (char*)"rm";
+    static constexpr char* const ENCODING_RMVB = (char*)"rmvb";
+    static constexpr char* const ENCODING_MKV  = (char*)"mkv";
+    static constexpr char* const ENCODING_FLV  = (char*)"flv";
+    static constexpr char* const ENCODING_F4V  = (char*)"f4v";
+    static constexpr char* const ENCODING_MPG  = (char*)"mpg";
+    static constexpr char* const ENCODING_MLV  = (char*)"mlv";
+    static constexpr char* const ENCODING_MPE  = (char*)"mpe";
+    static constexpr char* const ENCODING_MPEG = (char*)"mpeg";
+    static constexpr char* const ENCODING_DAT  = (char*)"dat";
+    static constexpr char* const ENCODING_m2v  = (char*)"m2v";
+    static constexpr char* const ENCODING_VOB  = (char*)"vob";
+    static constexpr char* const ENCODING_ASF  = (char*)"asf";
+    static constexpr char* const ENCODING_MOV  = (char*)"mov";
+    static constexpr char* const ENCODING_WMV  = (char*)"wmv";
+    static constexpr char* const ENCODING_3GP  = (char*)"3gp";
+    static constexpr char* const ENCODING_DEF  = ENCODING_H264;
+};
+
+/**
+ * 输入构造类
+ */
+class AIKITAPI AIKIT_DataBuilder : public AIKIT_DataBuilderObsolete {
+public:
+    static AIKIT_DataBuilder* create();
+    static void destroy(AIKIT_DataBuilder* builder);
+    virtual ~AIKIT_DataBuilder();
+
+    virtual AIKIT_DataBuilder* payload(AiData* data) = 0;
+    virtual AIKIT_InputData* build() = 0;
+
+    virtual void clear() = 0;
+};
+
+/**
+ * 自定义数据构造器
+ */
+class AIKITAPI AIKIT_CustomBuilder {
+public:
+    static AIKIT_CustomBuilder* create();
+    static void destroy(AIKIT_CustomBuilder* builder);
+    virtual ~AIKIT_CustomBuilder();
+
+    virtual AIKIT_CustomBuilder* text(const char* key, const char* data, uint32_t dataLen, int32_t index) = 0;
+    virtual AIKIT_CustomBuilder* textPath(const char* key, const char* path, int32_t index)               = 0;
+    virtual AIKIT_CustomBuilder* textFile(const char* key, const FILE* file, int32_t index)               = 0;
+
+    virtual AIKIT_CustomBuilder* audio(const char* key, const char* data, uint32_t dataLen, int32_t index) = 0;
+    virtual AIKIT_CustomBuilder* audioPath(const char* key, const char* path, int32_t index)               = 0;
+    virtual AIKIT_CustomBuilder* audioFile(const char* key, const FILE* file, int32_t index)               = 0;
+
+    virtual AIKIT_CustomBuilder* image(const char* key, const char* data, uint32_t dataLen, int32_t index) = 0;
+    virtual AIKIT_CustomBuilder* imagePath(const char* key, const char* path, int32_t index)               = 0;
+    virtual AIKIT_CustomBuilder* imageFile(const char* key, const FILE* file, int32_t index)               = 0;
+
+    virtual AIKIT_CustomBuilder* video(const char* key, const char* data, uint32_t dataLen, int32_t index) = 0;
+    virtual AIKIT_CustomBuilder* videoPath(const char* key, const char* path, int32_t index)               = 0;
+    virtual AIKIT_CustomBuilder* videoFile(const char* key, const FILE* file, int32_t index)               = 0;
+
+    virtual AIKIT_CustomData* build() = 0;
+    virtual void clear() = 0;
+};
+
+/**
+ * 参数和输入统一构造类
+ */
+class AIKITAPI AIKIT_Builder {
+public:
+    static AIKIT_BizParam*  build(AIKIT_ParamBuilder* param);
+    static AIKIT_InputData* build(AIKIT_DataBuilder* data);
+    static AIKIT_CustomData* build(AIKIT_CustomBuilder* custom);
+};
+
+} // namespace AIKIT
+
+#endif

aikit_tts/include/aikit_biz_config.h

@@ -0,0 +1,158 @@
+#ifndef AIKIT_BIZ_CONFIG_H
+#define AIKIT_BIZ_CONFIG_H
+#include <iostream>
+#include "aikit_biz_type.h"
+namespace AIKIT {
+
+class ConfigBuilder;
+class AppBuilder;
+class AuthBuilder;
+class LogBuilder;
+class CodecBuilder;
+
+class AIKITAPI AIKIT_Configurator {
+public:
+    static ConfigBuilder& builder();
+};
+
+class AIKITAPI ConfigBuilder {
+public:
+    /**
+     * @brief 用户app信息配置
+     */
+    AppBuilder& app();
+
+    /**
+     * @brief 授权相关配置
+     */
+    AuthBuilder& auth();
+
+    /**
+     * @brief 日志相关配置
+     */
+    LogBuilder& log();
+
+    /**
+     * @brief 音视频编解码相关配置
+     */
+    CodecBuilder&   codec();
+};
+
+class AIKITAPI AppBuilder : public ConfigBuilder {
+public:
+    /**
+     * @brief 配置应用id
+     */
+    AppBuilder& appID(const char* appID);
+
+    /**
+     * @brief 配置应用key
+     */
+    AppBuilder& apiKey(const char* apiKey);
+
+    /**
+     * @brief 配置应用secret
+     */
+    AppBuilder& apiSecret(const char* apiSecret);
+
+    /**
+     * @brief 配置sdk工作目录，需可读可写权限
+     */
+    AppBuilder& workDir(const char* workDir);
+
+     /**
+     * @brief 配置只读资源存放目录, 需可读权限
+     */
+    AppBuilder& resDir(const char* resDir);
+
+    /**
+     * @brief 配置文件路径，包括文件名
+     */
+    AppBuilder& cfgFile(const char* cfgFile);
+};
+
+class AIKITAPI AuthBuilder : public ConfigBuilder {
+public:
+    /**
+     * @brief 配置授权方式 0=设备级授权，1=应用级授权
+     */
+    AuthBuilder& authType(int authType);
+
+    /**
+     * @brief 配置离线激活方式的授权文件路径，为空时需联网进行首次在线激活
+     */
+    AuthBuilder& licenseFile(const char* licenseFile);
+
+    /**
+     * @brief 配置授权渠道Id
+     */
+    AuthBuilder& channelID(const char* channelID);
+
+    /**
+     * @brief 配置用户自定义设备标识
+     */
+    AuthBuilder& UDID(const char* UDID);
+
+    /**
+     * @brief 配置授权离线能力，如需配置多个能力，可用";"分隔开, 如"xxx;xxx"
+     */
+    AuthBuilder& ability(const char* ability);
+
+    /**
+     * @brief 配置单个在线能力id及对应请求地址，如需配置多个能力，可多次调用
+     * url格式参考：https://cn-huadong-1.xf-yun.com/v1/private/xxx:443
+     */
+    AuthBuilder& abilityURL(const char* ability, const char* url);
+
+};
+
+
+/**
+ * @brief 配置SDK日志信息
+ */
+class AIKITAPI LogBuilder : public ConfigBuilder {
+public:
+    /**
+     * @brief 配置日志等级
+     */
+    LogBuilder& logLevel(int32_t level);
+
+     /**
+     * @brief 配置日志输出模式
+     */
+    LogBuilder& logMode(int32_t mode);
+
+    /**
+     * @brief 配置日志文件保存文件
+     */
+    LogBuilder& logPath(const char* path);
+   
+};
+
+/**
+ * @brief 配置SDK音视频编解码方式
+ */
+class AIKITAPI CodecBuilder : public ConfigBuilder {
+public:
+    /**
+     * @brief 配置全局音频编码选项
+     */
+    CodecBuilder& audioEncoding(const char* encodingType);
+
+    /**
+     * @brief 配置能力级音频编码选项
+     */
+    CodecBuilder& audioEncoding(const char* abilityID, const char* encodingType);
+    /**
+     * @brief 配置全局音频解码选项
+     */
+    CodecBuilder& audioDecoding(const char* encodingType);
+
+    /**
+     * @brief 配置能力级音频解码选项
+     */
+    CodecBuilder& audioDecoding(const char* abilityID, const char* encodingType);
+};
+
+} // end of namespace AIKIT
+#endif

aikit_tts/include/aikit_biz_obsolete_builder.h

@@ -0,0 +1,35 @@
+#ifndef AIKIT_BIZ_OBSOLETE_BUILDER_H
+#define AIKIT_BIZ_OBSOLETE_BUILDER_H
+
+#include "aikit_biz_type.h"
+
+namespace AIKIT {
+
+class AIKIT_ParamBuilder;
+
+class AIKITAPI AIKIT_DataBuilderObsolete {
+public:
+    //设置SDK输入数据格式描述参数
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* desc(const char* key, AIKIT_ParamBuilder* builder) = 0;
+
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* text(const char* key, const char* data, uint32_t dataLen, uint32_t dataStatus) = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* textPath(const char* key, const char* path)               = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* textFile(const char* key, const FILE* file)               = 0;
+
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* audio(const char* key, const char* data, uint32_t dataLen, uint32_t dataStatus) = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* audioPath(const char* key, const char* path)               = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* audioFile(const char* key, const FILE* file)               = 0;
+
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* image(const char* key, const char* data, uint32_t dataLen, uint32_t dataStatus) = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* imagePath(const char* key, const char* path)               = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* imageFile(const char* key, const FILE* file)               = 0;
+
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* video(const char* key, const char* data, uint32_t dataLen, uint32_t dataStatus) = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* videoPath(const char* key, const char* path)               = 0;
+    attribute_deprecated virtual AIKIT_DataBuilderObsolete* videoFile(const char* key, const FILE* file)               = 0;
+
+};
+
+} // namespace AIKIT
+
+#endif

aikit_tts/include/aikit_biz_type.h

@@ -0,0 +1,105 @@
+#ifndef __AIKIT_BIZ_TYPE_H__
+#define __AIKIT_BIZ_TYPE_H__
+
+#include <stdio.h>
+#include "aikit_type.h"
+
+/**
+ * AIKIT API type
+ */
+#if defined(_MSC_VER)            /* Microsoft Visual C++ */
+#   if !defined(AIKITAPI)
+#       if defined(AIKIT_EXPORT)
+#           define AIKITAPI __declspec(dllexport)
+#       else
+#           define AIKITAPI __declspec(dllimport)
+#       endif
+#   endif
+#elif defined(__BORLANDC__)      /* Borland C++ */
+#   if !defined(AIKITAPI)
+#       define AIKITAPI __stdcall
+#   endif
+#elif defined(__WATCOMC__)       /* Watcom C++ */
+#   if !defined(AIKITAPI)
+#       define AIKITAPI __stdcall
+#   endif
+#else                            /* Any other including Unix */
+#   if !defined(AIKITAPI)
+#       if defined(AIKIT_EXPORT)
+#           define AIKITAPI __attribute__ ((visibility("default")))
+#       else
+#           define AIKITAPI
+#       endif
+#   endif
+#endif
+
+#if defined(_MSC_VER)
+#   if !defined(attribute_deprecated)
+#       define attribute_deprecated __declspec(deprecated)
+#   endif
+#else
+#   if !defined(attribute_deprecated)
+#       define attribute_deprecated __attribute__((deprecated))
+#   endif
+#endif
+
+#ifndef int32_t
+typedef int int32_t;
+#endif
+#ifndef uint32_t
+typedef unsigned int uint32_t;
+#endif
+
+typedef AIKIT_BaseParam       AIKIT_BizParam;
+typedef AIKIT_BaseParamPtr    AIKIT_BizParamPtr;
+typedef AIKIT_BaseData        AIKIT_BizData;
+typedef AIKIT_BaseDataPtr     AIKIT_BizDataPtr;
+
+typedef AIKIT_BizData         AIKIT_InputData;
+
+typedef struct _AIKIT_BaseDataList {
+    AIKIT_BaseData *node;     // 链表节点
+    int32_t count;          // 链表节点个数
+    int32_t totalLen;       // 链表节点所占内存空间：count*(sizeof(AIKIT_BaseData)+strlen(node->key)+node->len)
+} AIKIT_BaseDataList, *AIKIT_BaseDataListPtr;      // 配置对复用该结构定义
+
+typedef struct _AIKIT_BaseParamList {
+    AIKIT_BaseParam *node;    // 链表节点
+    int32_t count;          // 链表节点个数
+    int32_t totalLen;       // 链表节点所占内存空间：count*(sizeof(AIKIT_BaseParam)+strlen(node->key)+node->len)
+} AIKIT_BaseParamList, *AIKIT_BaseParamListPtr;      // 配置对复用该结构定义
+
+typedef AIKIT_BaseDataList    AIKIT_OutputData;
+typedef AIKIT_BaseParamList   AIKIT_OutputEvent;
+
+typedef struct {
+    void*       usrContext;
+    const char* abilityID;
+    size_t      handleID;
+} AIKIT_HANDLE;
+
+
+typedef void (*AIKIT_OnOutput)(AIKIT_HANDLE* handle, const AIKIT_OutputData* output);
+typedef void (*AIKIT_OnEvent)(AIKIT_HANDLE* handle, AIKIT_EVENT eventType, const AIKIT_OutputEvent* eventValue);
+typedef void (*AIKIT_OnError)(AIKIT_HANDLE* handle, int32_t err, const char* desc);
+
+typedef struct {
+    AIKIT_OnOutput outputCB;  //輸出回调
+    AIKIT_OnEvent  eventCB;   //事件回调
+    AIKIT_OnError  errorCB;   //错误回调
+} AIKIT_Callbacks;
+
+typedef struct {
+    int         authType;     // 授权方式，0=设备级授权，1=应用级授权
+    const char* appID;        // 应用id
+    const char* apiKey;       // 应用key
+    const char* apiSecret;    // 应用secret
+    const char* workDir;      // sdk工作目录，需可读可写权限
+    const char* resDir;       // 只读资源存放目录，需可读权限
+    const char* licenseFile;  // 离线激活方式的授权文件存放路径，为空时需联网进行首次在线激活
+    const char* batchID;      // 授权批次
+    const char* UDID;         // 用户自定义设备标识            
+    const char* cfgFile;      // 配置文件路径，包括文件名
+} AIKIT_InitParam;
+
+#endif // __AIKIT_BIZ_TYPE_H__

aikit_tts/include/aikit_common.h

@@ -0,0 +1,12 @@
+#ifndef AIKIT_COMMON_H
+#define AIKIT_COMMON_H
+
+
+typedef enum AIKIT_DATA_PTR_TYPE_E {
+    AIKIT_DATA_PTR_MEM  = 0,  // 数据内存指针
+    AIKIT_DATA_PTR_FILE = 1,  // 数据文件指针（FILE指针）
+    AIKIT_DATA_PTR_PATH = 2   // 数据文件路径指针
+} AIKIT_DATA_PTR_TYPE;
+
+
+#endif  //AIKIT_COMMON_H

aikit_tts/include/aikit_constant.h

@@ -0,0 +1,35 @@
+/*
+ * @Description: 
+ * @version: 
+ * @Author: xkzhang9
+ * @Date: 2020-10-28 10:20:45
+ * @LastEditors: rfge
+ * @LastEditTime: 2020-12-28 11:14:20
+ */
+
+#ifndef AIKIT_CONSTANT_H
+#define AIKIT_CONSTANT_H
+
+typedef enum {
+    LOG_STDOUT = 0,
+    LOG_LOGCAT,
+    LOG_FILE,
+} AIKIT_LOG_MODE;
+
+typedef enum {
+    LOG_LVL_VERBOSE,
+    LOG_LVL_DEBUG,
+    LOG_LVL_INFO,
+    LOG_LVL_WARN,
+    LOG_LVL_ERROR,
+    LOG_LVL_CRITICAL,
+    LOG_LVL_OFF = 100,
+
+} AIKIT_LOG_LEVEL;
+
+typedef enum {
+    MEMORY_FULL_MODE,
+    MEMORY_FRIENDLY_MODE
+} AIKIT_MEMORY_MODE;
+
+#endif  //AIKIT_CONSTANT_H

aikit_tts/include/aikit_err.h

@@ -0,0 +1,161 @@
+/*
+ * @Description: 
+ * @version: 
+ * @Author: rfge
+ * @Date: 2020-11-01 17:56:53
+ * @LastEditors: rfge
+ * @LastEditTime: 2020-12-14 14:26:44
+ */
+//
+// Created by xkzhang9 on 2020/6/10.
+//
+
+#ifndef AIKIT_ERR_H
+#define AIKIT_ERR_H
+
+typedef enum {
+    AIKIT_ERR_AUTH     = 18000,                // 授权部分
+    AIKIT_ERR_RES      = AIKIT_ERR_AUTH + 100,  // 资源部分
+    AIKIT_ERR_ENG      = AIKIT_ERR_AUTH + 200,  // 引擎部分
+    AIKIT_ERR_SDK      = AIKIT_ERR_AUTH + 300,  // SDK部分
+    AIKIT_ERR_SYS      = AIKIT_ERR_AUTH + 400,  // 系统部分
+    AIKIT_ERR_PARAM    = AIKIT_ERR_AUTH + 500,  // 参数部分
+    AIKIT_ERR_PROTOCOL = AIKIT_ERR_AUTH + 600,  // 协议部分
+    AIKIT_ERR_CLOUD    = AIKIT_ERR_AUTH + 700,  // 云端错误
+    AIKIT_ERR_LOCAL_NET= AIKIT_ERR_AUTH + 800,  //本地网络网络
+    AIKIT_ERR_VMS      = AIKIT_ERR_AUTH + 900,  // 虚拟人错误
+    AIKIT_ERR_SPARK    = AIKIT_ERR_AUTH + 950,  // spark大模型错误
+    AIKIT_ERR_OTHER = 0xFF00  // 其他
+} AIKIT_ERR_SECTION;
+
+typedef enum {
+    AIKIT_ERR_SUCCESS        = 0,  // 操作成功
+    AIKIT_ERR_GENERAL_FAILED = 1,  // 一般错误
+
+    AIKIT_ERR_AUTH_LICENSE_NOT_FOUND    = AIKIT_ERR_AUTH + 0,  // 18000 本地license文件不存在
+    AIKIT_ERR_AUTH_LICENSE_FILE_INVALID = AIKIT_ERR_AUTH + 1,  // 18001 授权文件内容非法
+    AIKIT_ERR_AUTH_LICENSE_PARSE_FAILED = AIKIT_ERR_AUTH + 2,  // 18002 授权文件解析失败
+    AIKIT_ERR_AUTH_PAYLOAD_DEFECT       = AIKIT_ERR_AUTH + 3,  // 18003 payload内容缺失
+    AIKIT_ERR_AUTH_SIGN_DEFECT          = AIKIT_ERR_AUTH + 4,  // 18004 signature内容缺失
+    AIKIT_ERR_AUTH_EXPIRED              = AIKIT_ERR_AUTH + 5,  // 18005 授权已过期
+    AIKIT_ERR_AUTH_TIME_ERROR           = AIKIT_ERR_AUTH + 6,  // 18006 授权时间错误，比正常时间慢30分钟以上
+    AIKIT_ERR_AUTH_APP_NOT_MATCH        = AIKIT_ERR_AUTH + 7,  // 18007 授权应用不匹配（apiKey、apiSecret）
+    AIKIT_ERR_AUTH_LICENSE_EXPIRED      = AIKIT_ERR_AUTH + 8,  // 18008 授权文件激活过期
+    AIKIT_ERR_AUTH_NULL_APP_PTR         = AIKIT_ERR_AUTH + 9,  // 18009 授权app信息指针为空
+    AIKIT_ERR_AUTH_PLATFORM_NOT_MATCH   = AIKIT_ERR_AUTH + 10, // 18010 离线授权激活文件指定平台与设备平台不匹配
+    AIKIT_ERR_AUTH_ARCH_NOT_MATCH       = AIKIT_ERR_AUTH + 11, // 18011 离线授权激活文件指定架构与设备cpu架构不匹配
+    AIKIT_ERR_AUTH_WRONG_LICENSE_NUM    = AIKIT_ERR_AUTH + 12, // 18012 离线授权激活文件中包含License个数异常
+    AIKIT_ERR_AUTH_DEVICE_NOT_FOUND     = AIKIT_ERR_AUTH + 13, // 18013 离线授权激活文件中未找到当前设备
+    AIKIT_ERR_AUTH_LEVEL_NOT_VALID      = AIKIT_ERR_AUTH + 14, // 18014 离线授权激活文件中设备指纹安全等级非法
+    AIKIT_ERR_AUTH_HARDWARE_FAILED      = AIKIT_ERR_AUTH + 15, // 18015 硬件授权验证失败
+    AIKIT_ERR_AUTH_OFFLINE_PROT_INVALID = AIKIT_ERR_AUTH + 16, // 18016 离线授权激活文件内容非法
+    AIKIT_ERR_AUTH_HEADER_INVALID       = AIKIT_ERR_AUTH + 17, // 18017 离线授权激活文件中协议头非法
+    AIKIT_ERR_AUTH_PART_COUNT_INVALID   = AIKIT_ERR_AUTH + 18, // 18018 离线授权激活文件中指纹组成项个数为0
+    AIKIT_ERR_AUTH_RESOURCE_EXPIREd     = AIKIT_ERR_AUTH + 19, // 18019 资源已过期
+
+    AIKIT_ERR_RES_VERIFY_FAILED = AIKIT_ERR_RES + 0,  // 18100 资源鉴权失败
+    AIKIT_ERR_RES_INVALID_HEADER= AIKIT_ERR_RES + 1,  // 18101 资源格式解析失败
+    AIKIT_ERR_RES_NOT_MATCH     = AIKIT_ERR_RES + 2,  // 18102 资源(与引擎)不匹配
+    AIKIT_ERR_RES_NULL_PTR      = AIKIT_ERR_RES + 3,  // 18103 资源参数不存在（指针为NULL）
+    AIKIT_ERR_RES_OPEN_FAILED   = AIKIT_ERR_RES + 4,  // 18104 资源路径打开失败
+    AIKIT_ERR_RES_LOAD_FAILED   = AIKIT_ERR_RES + 5,  // 18105 资源加载失败，workDir内未找到对应资源
+    AIKIT_ERR_RES_UNLOAD_FAILED = AIKIT_ERR_RES + 6,  // 18106 资源卸载失败, 卸载的资源未加载过
+
+    AIKIT_ERR_ENG_VERIFY_FAILED   = AIKIT_ERR_ENG + 0,  // 18200 引擎鉴权失败
+    AIKIT_ERR_ENG_LOAD_FAILED     = AIKIT_ERR_ENG + 1,  // 18201 引擎动态加载失败
+    AIKIT_ERR_ENG_NOT_INITED      = AIKIT_ERR_ENG + 2,  // 18202 引擎未初始化
+    AIKIT_ERR_ENG_API_NOT_SUPPORT = AIKIT_ERR_ENG + 3,  // 18203 引擎不支持该接口调用
+    AIKIT_ERR_ENG_NULL_CREATE_PTR = AIKIT_ERR_ENG + 4,  // 18204 引擎craete函数指针为空
+
+    AIKIT_ERR_SDK_INVALID                  = AIKIT_ERR_SDK + 0,   // 18300 sdk不可用
+    AIKIT_ERR_SDK_NOT_INITED               = AIKIT_ERR_SDK + 1,   // 18301 sdk没有初始化
+    AIKIT_ERR_SDK_INIT_FAILED              = AIKIT_ERR_SDK + 2,   // 18302 sdk初始化失败
+    AIKIT_ERR_SDK_ALREADY_INIT             = AIKIT_ERR_SDK + 3,   // 18303 sdk已经初始化
+    AIKIT_ERR_SDK_INVALID_PARAM            = AIKIT_ERR_SDK + 4,   // 18304 sdk不合法参数
+    AIKIT_ERR_SDK_NULL_SESSION_HANDLE      = AIKIT_ERR_SDK + 5,   // 18305 sdk会话handle为空
+    AIKIT_ERR_SDK_SESSION_NOT_FOUND        = AIKIT_ERR_SDK + 6,   // 18306 sdk会话未找到
+    AIKIT_ERR_SDK_SESSION_ALREADY_END      = AIKIT_ERR_SDK + 7,   // 18307 sdk会话重复终止
+    AIKIT_ERR_SDK_TIMEOUT                  = AIKIT_ERR_SDK + 8,   // 18308 超时错误
+    AIKIT_ERR_SDK_INITING                  = AIKIT_ERR_SDK + 9,   // 18309 sdk正在初始化中
+    AIKIT_ERR_SDK_SESSEION_ALREAY_START    = AIKIT_ERR_SDK + 10,  // 18310 sdk会话重复开启
+    AIKIT_ERR_SDK_CONCURRENT_OVERFLOW      = AIKIT_ERR_SDK + 11,  // 18311 sdk同一能力并发路数超出最大限制
+
+    AIKIT_ERR_SYS_WORK_DIR_ILLEGAL         = AIKIT_ERR_SYS + 0,  // 18400 工作目录无写权限
+    AIKIT_ERR_SYS_DEVICE_UNKNOWN           = AIKIT_ERR_SYS + 1,  // 18401 设备指纹获取失败，设备未知
+    AIKIT_ERR_SYS_FILE_OPEN_FAILED         = AIKIT_ERR_SYS + 2,  // 18402 文件打开失败
+    AIKIT_ERR_SYS_MEM_ALLOC_FAILED         = AIKIT_ERR_SYS + 3,  // 18403 内存分配失败
+    AIKIT_ERR_SYS_DEVICE_COMPARE_FAILED    = AIKIT_ERR_SYS + 4,  // 18404 设备指纹比较失败
+    AIKIT_ERR_SYS_WORK_DIR_NOT_EXIST       = AIKIT_ERR_SYS + 5,  // 18405 工作目录不存在
+
+    AIKIT_ERR_PARAM_NOT_FOUND                = AIKIT_ERR_PARAM + 0,   // 18500 未找到该参数key
+    AIKIT_ERR_PARAM_OVERFLOW                 = AIKIT_ERR_PARAM + 1,   // 18501 参数范围溢出，不满足约束条件
+    AIKIT_ERR_PARAM_NULL_INIT_PARAM_PTR      = AIKIT_ERR_PARAM + 2,   // 18502 sdk初始化参数为空
+    AIKIT_ERR_PARAM_NULL_APPID_PTR           = AIKIT_ERR_PARAM + 3,   // 18503 sdk初始化参数中appid为空
+    AIKIT_ERR_PARAM_NULL_APIKEY_PTR          = AIKIT_ERR_PARAM + 4,   // 18504 sdk初始化参数中apiKey为空
+    AIKIT_ERR_PARAM_NULL_APISECRET_PTR       = AIKIT_ERR_PARAM + 5,   // 18505 sdk初始化参数中apiSecret为空
+    AIKIT_ERR_PARAM_NULL_ABILITY_PTR         = AIKIT_ERR_PARAM + 6,   // 18506 ability参数为空
+    AIKIT_ERR_PARAM_NULL_INPUT_PTR           = AIKIT_ERR_PARAM + 7,   // 18507 input参数为空
+    AIKIT_ERR_PARAM_DATA_KEY_NOT_EXIST       = AIKIT_ERR_PARAM + 8,   // 18508 输入数据参数Key不存在
+    AIKIT_ERR_PARAM_REQUIRED_MISSED          = AIKIT_ERR_PARAM + 9,   // 18509 必填参数缺失
+    AIKIT_ERR_PARAM_NULL_OUTPUT_PTR          = AIKIT_ERR_PARAM + 10,  // 18510 output参数缺失
+    
+    AIKIT_ERR_CODEC_NOT_SUPPORT              = AIKIT_ERR_PARAM + 20,  // 18520 不支持的编解码类型
+    AIKIT_ERR_CODEC_NULL_PTR                 = AIKIT_ERR_PARAM + 21,  // 18521 编解码handle指针为空
+    AIKIT_ERR_CODEC_MODULE_MISSED            = AIKIT_ERR_PARAM + 22,  // 18522 编解码模块条件编译未打开
+    AIKIT_ERR_CODEC_ENCODE_FAIL              = AIKIT_ERR_PARAM + 23,  // 18523 编码错误
+    AIKIT_ERR_CODEC_DECODE_FAIL              = AIKIT_ERR_PARAM + 24,  // 18524 解码错误
+
+    AIKIT_ERR_VAD_RESPONSE_TIMEOUT           = AIKIT_ERR_PARAM + 30,  // VAD静音超时
+
+    AIKIT_ERR_PROTOCOL_TIMESTAMP_MISSING        = AIKIT_ERR_PROTOCOL + 0, // 18600 协议中时间戳字段缺失
+    AIKIT_ERR_PROTOCOL_ABILITY_NOT_FOUND        = AIKIT_ERR_PROTOCOL + 1, // 18601 协议中未找到该能力ID
+    AIKIT_ERR_PROTOCOL_RESOURCE_NOT_FOUND       = AIKIT_ERR_PROTOCOL + 2, // 18602 协议中未找到该资源ID
+    AIKIT_ERR_PROTOCOL_ENGINE_NOT_FOUND         = AIKIT_ERR_PROTOCOL + 3, // 18603 协议中未找到该引擎ID
+    AIKIT_ERR_PROTOCOL_ZERO_ENGINE_NUM          = AIKIT_ERR_PROTOCOL + 4, // 18604 协议中引擎个数为0
+    AIKIT_ERR_PROTOCOL_NOT_LOADED               = AIKIT_ERR_PROTOCOL + 5, // 18605 协议未被初始化解析
+    AIKIT_ERR_PROTOCOL_INTERFACE_TYPE_NOT_MATCH = AIKIT_ERR_PROTOCOL + 6, // 18606 协议能力接口类型不匹配
+    AIKIT_ERR_PROTOCOL_TEMP_VERIFY_FAILED       = AIKIT_ERR_PROTOCOL + 7, // 18607 预置协议解析失败
+
+    AIKIT_ERR_CLOUD_GENERAL_FAILED          = AIKIT_ERR_CLOUD + 0,  // 18700 通用网络错误
+    AIKIT_ERR_CLOUD_CONNECT_FAILED          = AIKIT_ERR_CLOUD + 1,  // 18701 网路不通
+    AIKIT_ERR_CLOUD_403                     = AIKIT_ERR_CLOUD + 2,  // 18702 网关检查不过
+    AIKIT_ERR_CLOUD_WRONG_RSP_FORMAT        = AIKIT_ERR_CLOUD + 3,  // 18703 云端响应格式不对
+    AIKIT_ERR_CLOUD_APP_NOT_FOUND           = AIKIT_ERR_CLOUD + 4,  // 18704 应用未注册
+    AIKIT_ERR_CLOUD_APP_CHECK_FAILED        = AIKIT_ERR_CLOUD + 5,  // 18705 应用apiKey&&apiSecret校验失败
+    AIKIT_ERR_CLOUD_WRONG_ARCHITECT         = AIKIT_ERR_CLOUD + 6,  // 18706 引擎不支持的平台架构
+    AIKIT_ERR_CLOUD_AUTH_EXPIRED            = AIKIT_ERR_CLOUD + 7,  // 18707 授权已过期
+    AIKIT_ERR_CLOUD_AUTH_FULL               = AIKIT_ERR_CLOUD + 8,  // 18708 授权数量已满
+    AIKIT_ERR_CLOUD_ABILITY_NOT_FOUND       = AIKIT_ERR_CLOUD + 9,  // 18709 未找到该app绑定的能力
+    AIKIT_ERR_CLOUD_RESOURCE_NOT_FOUND      = AIKIT_ERR_CLOUD + 10, // 18710 未找到该app绑定的能力资源
+    AIKIT_ERR_CLOUD_JSON_PARSE_FAILED       = AIKIT_ERR_CLOUD + 11, // 18711 JSON操作失败
+    AIKIT_ERR_CLOUD_404                     = AIKIT_ERR_CLOUD + 12, // 18712 http 404错误
+    AIKIT_ERR_CLOUD_LEVEL_NOT_MATCH         = AIKIT_ERR_CLOUD + 13, // 18713 设备指纹安全等级不匹配
+    AIKIT_ERR_CLOUD_401                     = AIKIT_ERR_CLOUD + 14, // 18714 用户没有访问权限，需要进行身份认证
+    AIKIT_ERR_CLOUD_SDK_NOT_FOUND           = AIKIT_ERR_CLOUD + 15, // 18715 未找到该SDK ID
+    AIKIT_ERR_CLOUD_ABILITYS_NOT_FOUND      = AIKIT_ERR_CLOUD + 16, // 18716 未找到该组合能力集合
+    AIKIT_ERR_CLOUD_ABILITY_NOT_ENOUGH      = AIKIT_ERR_CLOUD + 17, // 18717 SDK组合能力授权不足
+    AIKIT_ERR_CLOUD_APP_SIG_INVALID         = AIKIT_ERR_CLOUD + 18, // 18718 无效授权应用签名
+    AIKIT_ERR_CLOUD_APP_SIG_NOT_UNIQUE      = AIKIT_ERR_CLOUD + 19, // 18719 应用签名不唯一
+    AIKIT_ERR_CLOUD_SCHEMA_INVALID          = AIKIT_ERR_CLOUD + 20, // 18720 能力schema不可用
+    AIKIT_ERR_CLOUD_TEMPLATE_NOT_FOUND      = AIKIT_ERR_CLOUD + 21, // 18721 竞争授权: 未找到能力集模板
+    AIKIT_ERR_CLOUD_ABILITY_NOT_IN_TEMPLATE = AIKIT_ERR_CLOUD + 22, // 18722 竞争授权: 能力不在模板能力集模板中
+
+    AIKIT_ERR_LOCAL_NET_CONNECT_FAILED   = AIKIT_ERR_LOCAL_NET + 1, // 18801 连接建立出错
+    AIKIT_ERR_LOCAL_NET_RES_WAIT_TIMEOUT = AIKIT_ERR_LOCAL_NET + 2, // 18802 结果等待超时
+    AIKIT_ERR_LOCAL_NET_CONNECT_ERROR    = AIKIT_ERR_LOCAL_NET + 3, // 18803 连接状态异常
+
+    AIKIT_ERR_VMS_START_ERROR     = AIKIT_ERR_VMS + 1, // 18901虚拟人启动错误
+    AIKIT_ERR_VMS_STOP_ERROR      = AIKIT_ERR_VMS + 2, // 18902虚拟人结束错误
+    AIKIT_ERR_VMS_DRIVE_ERROR     = AIKIT_ERR_VMS + 3, // 18903虚拟人驱动错误
+    AIKIT_ERR_VMS_HEARTBEAT_ERROR = AIKIT_ERR_VMS + 4, // 18904虚拟人心跳报错
+    AIKIT_ERR_VMS_TIMEOUT         = AIKIT_ERR_VMS + 5, // 18905虚拟人服务超时
+    AIKIT_ERR_VMS_OTHER_ERROR     = AIKIT_ERR_VMS + 6, // 18906虚拟人通用报错
+
+    AIKIT_ERR_SPARK_REQUEST_UNORDERED = AIKIT_ERR_SPARK + 1, // 18951 同一流式大模型会话，禁止并发交互请求
+    AIKIT_ERR_SPARK_TEXT_INVALID = AIKIT_ERR_SPARK + 2       // 18952 输入文本格式或内容非法
+
+} AIKIT_ERR;
+
+typedef enum { AIKIT_ERR_TYPE_AUTH = 0, AIKIT_ERR_TYPE_HTTP = 1 } AIKIT_ERR_TYPE;
+
+#endif  //AIKIT_ERR_H

aikit_tts/include/aikit_spark_api.h

@@ -0,0 +1,90 @@
+#ifndef AIKIT_SPARK_H
+#define AIKIT_SPARK_H
+#include "aikit_biz_api.h"
+
+namespace AIKIT {
+
+
+class AIKITAPI ChatParam {
+public:
+    static ChatParam* builder();
+   
+    virtual ~ChatParam();
+
+    /**
+     * @brief 配置授权的用户id，用于关联用户交互的上下文
+     */
+    virtual ChatParam* uid(const char* uid) = 0;
+
+    /**
+     * @brief 配置chat领域信息
+     */
+    virtual ChatParam* domain(const char* domain) = 0;
+
+    /**
+     * @brief 配置内容审核策略
+     */
+    virtual ChatParam* auditing(const char* auditing) = 0;
+
+    /**
+     * @brief 配置关联会话chat id标识，需要保障⽤户下唯⼀
+     */
+    virtual ChatParam* chatID(const char* chatID) = 0;
+
+    /**
+     * @brief 配置核采样阈值，默认值0.5，向上调整可以增加结果的随机程度
+     */
+    virtual ChatParam* temperature(const float& temperature) = 0;
+
+    /**
+     * @brief 配置从k个候选中随机选择⼀个（⾮等概率），默认值4，取值范围1-99
+     */
+    virtual ChatParam* topK(const int& topK) = 0;
+
+    /**
+     * @brief 配置回答的tokens的最大长度，默认值2048
+     */
+    virtual ChatParam* maxToken(const int& maxToken) = 0;
+
+    
+    /**
+     * @brief 配置chat服务域名地址
+     */
+    virtual ChatParam* url(const char* url) = 0;
+
+     /**
+     * @brief 配置chat扩展功能参数
+     */
+    virtual ChatParam* param(const char* key, const char* value) = 0;
+    virtual ChatParam* param(const char* key, int value)                          = 0;
+    virtual ChatParam* param(const char* key, double value)                       = 0;
+    virtual ChatParam* param(const char* key, bool value)                         = 0;
+
+};
+
+using AIChat_Handle = AIKIT_HANDLE;
+
+typedef void (*onChatOutput)(AIChat_Handle* handle, const char* role, const char* content, const int& index);
+typedef void (*onChatToken)(AIChat_Handle* handle, const int& completionTokens, const int& promptTokens, const int& totalTokens);
+typedef void (*onChatError)(AIChat_Handle* handle, const int& err, const char* errDesc);
+typedef struct {
+    onChatOutput outputCB;     //输出回调
+    onChatToken  tokenCB;      //token计算信息回调
+    onChatError  errorCB;      //错误回调
+} AIKIT_ChatCBS;
+
+AIKITAPI int32_t AIKIT_ChatCallback(const AIKIT_ChatCBS& cbs);
+//异步chat
+AIKITAPI int32_t AIKIT_AsyncChat(const ChatParam* params, const char* inputText, void* usrContext);
+
+
+
+AIKITAPI int32_t AIKIT_Start(const ChatParam* params, void* usrContext, AIChat_Handle** outHandle);
+AIKITAPI int32_t AIKIT_Write(AIChat_Handle* handle, const char* inputText);
+
+//
+//AIKITAPI int32_t AIKIT_End(AIChat_Handle* handle);
+
+
+} // end of namespace AIKIT
+#endif

aikit_tts/include/aikit_type.h

@@ -0,0 +1,107 @@
+#ifndef __AIKIT_TYPE_H__
+#define __AIKIT_TYPE_H__
+
+#include <stddef.h>
+#include <stdint.h>
+#include "aikit_common.h"
+
+#if defined(_MSC_VER)            /* Microsoft Visual C++ */
+  #pragma pack(push, 8)
+#elif defined(__BORLANDC__)      /* Borland C++ */
+  #pragma option -a8
+#elif defined(__WATCOMC__)       /* Watcom C++ */
+  #pragma pack(push, 8)
+#else                            /* Any other including Unix */
+#endif
+
+typedef enum _AIKIT_VarType {
+    AIKIT_VarTypeString   =   0,      // 字符串型
+    AIKIT_VarTypeInt      =   1,      // 整型
+    AIKIT_VarTypeDouble   =   2,      // 实型
+    AIKIT_VarTypeBool     =   3,      // 布尔类型
+    AIKIT_VarTypeParamPtr =   4,      // 子参数类型
+    AIKIT_VarTypeUnknown  =   -1      //
+} AIKIT_VarType;
+
+typedef enum _AIKIT_DataStatus {
+    AIKIT_DataBegin    =  0,      // 首数据
+    AIKIT_DataContinue =  1,      // 中间数据
+    AIKIT_DataEnd      =  2,      // 尾数据
+    AIKIT_DataOnce     =  3,      // 非会话单次输入输出
+} AIKIT_DataStatus;
+
+typedef enum _AIKIT_DataType {
+    AIKIT_DataText    =   0,      // 文本数据
+    AIKIT_DataAudio   =   1,      // 音频数据
+    AIKIT_DataImage   =   2,      // 图像数据
+    AIKIT_DataVideo   =   3,      // 视频数据
+//    AIKIT_DataPer     =   4,      // 个性化数据
+} AIKIT_DataType;
+
+typedef enum {
+    AIKIT_Event_UnKnown  = 0,
+    AIKIT_Event_Start    = 1,  // 引擎计算开始事件
+    AIKIT_Event_End      = 2,  // 引擎计算结束事件
+    AIKIT_Event_Timeout  = 3,  // 引擎计算超时事件
+    AIKIT_Event_Progress = 4,  // 引擎计算进度事件
+
+    // 在线能力连接状态事件
+    AIKIT_Event_Null = 10,          // 空连接状态
+    AIKIT_Event_Init,               // 初始状态
+    AIKIT_Event_Connecting,         // 正在连接
+    AIKIT_Event_ConnTimeout,        // 连接超时
+    AIKIT_Event_Failed,             // 连接失败
+    AIKIT_Event_Connected,          // 已经连接
+    AIKIT_Event_Error,              // 收发出错，意味着断开连接
+    AIKIT_Event_Disconnected,       // 断开连接，一般指心跳超时，连接无错误
+    AIKIT_Event_Closing,            // 正在关闭连接
+    AIKIT_Event_Closed,             // 已经关闭连接
+    AIKIT_Event_Responding,         // 网络连接正在响应
+    AIKIT_Event_ResponseTimeout,    // 网络连接响应超时
+    
+    // VAD事件
+    AIKIT_Event_VadBegin = 30, // VAD开始
+    AIKIT_Event_VadEnd       // VAD结束
+} AIKIT_EVENT;
+
+typedef struct _AIKIT_BaseParam {
+    struct _AIKIT_BaseParam *next;   // 链表指针
+    const char *key;        // 数据标识
+    void *value;            // 数据实体
+    void* reserved;         // 预留字段
+    int32_t len;            // 数据长度
+    int32_t type;        // 变量类型，取值参见AIKIT_VarType
+} AIKIT_BaseParam, *AIKIT_BaseParamPtr;      // 配置对复用该结构定义
+
+typedef struct _AIKIT_BaseData {
+    struct _AIKIT_BaseData *next;    // 链表指针
+    AIKIT_BaseParam *desc;   // 数据描述，包含每个数据(audio/video/text/image)的所有特征参数数据(sample_rate,channels,data等)
+    const char *key;        // 数据标识
+    void *value;            // 数据实体
+    void* reserved;         // 预留字段
+    int32_t len;            // 数据长度
+    int32_t type;           // 数据类型，取值参见AIKIT_DataType
+    int32_t status;         // 数据状态，取值参见AIKIT_DataStatus
+    int32_t from;           // 数据来源，取值参见AIKIT_DATA_PTR_TYPE
+} AIKIT_BaseData, *AIKIT_BaseDataPtr;
+
+typedef struct _AIKIT_CustomData {
+    struct _AIKIT_CustomData*    next;       // 链表指针
+    const char*         key;        // 数据标识
+    void*               value;      // 数据内容
+    void*               reserved;   // 预留字段
+    int32_t             index;      // 数据索引,用户可自定义设置
+    int32_t             len;        // 数据长度，type非DATA_PTR_FILE时本字段有效
+    int32_t             from;       // 数据内容的类型，取值参见枚举AIKIT_DATA_PTR_TYPE
+} AIKIT_CustomData, *AIKIT_CustomDataPtr;
+
+/* Reset the structure packing alignments for different compilers. */
+#if defined(_MSC_VER)            /* Microsoft Visual C++ */
+#pragma pack(pop)
+#elif defined(__BORLANDC__)      /* Borland C++ */
+#pragma option -a.
+#elif defined(__WATCOMC__)       /* Watcom C++ */
+#pragma pack(pop)
+#endif
+
+#endif

aikit_tts/launch/tts_launch.py

@@ -0,0 +1,16 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='aikit_tts',
+            executable='aikit_tts_node',
+            name='aikit_tts_node',
+            parameters=[
+                {'app_id': '268545c7'},
+                {'api_key': '9e90faba614489a90252f823127f8348'},
+                {'api_secret': 'M2U5YjJhYjJlMDYzY2Y2ZDhjNjNiZDJl'}
+            ]
+        )
+    ])

aikit_tts/package.xml

@@ -0,0 +1,20 @@
+<?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>aikit_tts</name>
+  <version>0.0.0</version>
+  <description>ROS 2 package for AIKIT TTS</description>
+  <maintainer email="your@email.com">Your Name</maintainer>
+  <license>Apache-2.0</license>
+
+  <!-- 编译工具依赖 -->
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  
+  <!-- 核心依赖（depend 同时包含编译和运行时依赖） -->
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

aikit_tts/src/aikit_tts_node.cpp

@@ -0,0 +1,170 @@
+#include <fstream>
+#include <assert.h>
+#include <cstring>
+#include <atomic>
+#include <unistd.h>
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+
+#include "aikit_biz_api.h"
+#include "aikit_constant.h"
+#include "aikit_biz_config.h"
+
+using namespace std;
+using namespace AIKIT;
+
+// 全局变量（可改为类成员变量更规范）
+FILE *fin = nullptr;
+static const char *ABILITY = "e2e44feff";
+static std::atomic_bool ttsFinished(false);
+rclcpp::Node::SharedPtr g_node = nullptr;
+
+
+// 回调函数保持不变（输出、事件、错误处理）
+void OnOutput(AIKIT_HANDLE* handle, const AIKIT_OutputData* output){
+    RCLCPP_INFO(g_node->get_logger(), "OnOutput abilityID: %s", handle->abilityID);
+    RCLCPP_INFO(g_node->get_logger(), "OnOutput key: %s", output->node->key);
+    if((output->node->value) && (fin != nullptr))
+    {
+        fwrite(output->node->value, sizeof(char), output->node->len, fin);
+    }
+}
+
+void OnEvent(AIKIT_HANDLE* handle, AIKIT_EVENT eventType, const AIKIT_OutputEvent* eventValue){
+    RCLCPP_INFO(g_node->get_logger(), "OnEvent: %d", eventType);
+    if(eventType == AIKIT_Event_End){
+        ttsFinished = true;
+    }
+}
+
+void OnError(AIKIT_HANDLE* handle, int32_t err, const char* desc){
+    RCLCPP_ERROR(g_node->get_logger(), "OnError: %d, desc: %s", err, desc);
+}
+
+
+// 语音合成函数（改为接收字符串参数）
+void text_to_speech(const string& text) {
+    AIKIT_ParamBuilder* paramBuilder = nullptr;
+    AIKIT_DataBuilder* dataBuilder = nullptr;
+    AIKIT_HANDLE* handle = nullptr;
+    AiText* aiText_raw = nullptr;
+    int ret = 0;
+    ttsFinished = false;
+
+    // -------------------------- 关键调整：把tts_params移到goto之前 --------------------------
+    // 定义TTS参数JSON（移到前面，避免goto跳过初始化）
+    const char* tts_params = R"({
+        "vcn": "xiaoyan",
+        "language": "zh-CN",
+        "textEncoding": "UTF-8",
+        "speed": 50,
+        "pitch": 50,
+        "volume": 50
+    })";
+    // ---------------------------------------------------------------------------------------
+
+    // 打开输出文件（路径可改为ROS参数配置）
+    fin = fopen("output.pcm", "w+");
+    if (fin == nullptr){
+        RCLCPP_ERROR(g_node->get_logger(), "Failed to open output.pcm");
+        goto exit; // 此时goto不会跨越任何变量初始化
+    }
+
+    // 配置TTS参数
+    paramBuilder = AIKIT_ParamBuilder::create();
+    // 将JSON参数作为顶层"params"字段传入（SDK能直接解析）
+    paramBuilder->param("params", tts_params, strlen(tts_params));
+
+    // 启动AI服务
+    ret = AIKIT_Start(ABILITY, AIKIT_Builder::build(paramBuilder), nullptr, &handle);
+    RCLCPP_INFO(g_node->get_logger(), "AIKIT_Start ret: %d", ret);
+    if(ret != 0) goto exit;
+
+    // 发送文本数据
+    dataBuilder = AIKIT_DataBuilder::create();
+    aiText_raw = AiText::get("text")->data(text.c_str(), text.length())->valid();
+    dataBuilder->payload(aiText_raw);
+    ret = AIKIT_Write(handle, AIKIT_Builder::build(dataBuilder));
+    RCLCPP_INFO(g_node->get_logger(), "AIKIT_Write ret: %d", ret);
+    if(ret != 0) goto exit;
+
+    // 等待合成完成
+    while(ttsFinished != true && rclcpp::ok()){
+        usleep(1000);
+    }
+    AIKIT_End(handle); // 释放会话资源
+
+exit:
+    if(paramBuilder) { delete paramBuilder; paramBuilder = nullptr; }
+    if(dataBuilder) { delete dataBuilder; dataBuilder = nullptr; }
+    if(fin) { fclose(fin); fin = nullptr; }
+}
+
+
+
+// 订阅文本消息的回调函数
+void text_callback(const std_msgs::msg::String::SharedPtr msg) {
+    RCLCPP_INFO(g_node->get_logger(), "Received text: %s", msg->data.c_str());
+    text_to_speech(msg->data); // 调用TTS合成
+}
+
+
+// ROS 2节点初始化（集成AIKIT初始化）
+class AikitTtsNode : public rclcpp::Node {
+public:
+    AikitTtsNode() : Node("aikit_tts_node") {
+        // 从参数服务器获取科大讯飞密钥（建议在launch文件中配置）
+        this->declare_parameter("app_id", "268545c7");
+        this->declare_parameter("api_key", "9e90faba614489a90252f823127f8348");
+        this->declare_parameter("api_secret", "M2U5YjJhYjJlMDYzY2Y2ZDhjNjNiZDJl");
+        
+        std::string app_id, api_key, api_secret;
+        app_id = "268545c7";
+        api_key = "9e90faba614489a90252f823127f8348";
+        api_secret = "M2U5YjJhYjJlMDYzY2Y2ZDhjNjNiZDJl";
+
+        // 初始化AIKIT
+        AIKIT_Configurator::builder()
+            .app()
+                .appID(app_id.c_str())
+                .apiKey(api_key.c_str())
+                .apiSecret(api_secret.c_str())
+                .workDir("./") // 工作目录（如果资源还找不到，可改为TTS_RESOURCE_PATH）
+            .auth()
+                .authType(0)
+            .log()
+                .logLevel(LOG_LVL_INFO)
+                .logPath("./");
+
+        int ret = AIKIT_Init();
+        if(ret != 0) {
+            RCLCPP_FATAL(this->get_logger(), "AIKIT_Init failed: %d", ret);
+            rclcpp::shutdown();
+        }
+
+        // 注册回调并创建订阅者
+        AIKIT_Callbacks cbs = {OnOutput, OnEvent, OnError};
+        AIKIT_RegisterAbilityCallback(ABILITY, cbs);
+        
+        subscriber_ = this->create_subscription<std_msgs::msg::String>(
+            "tts_text", 10, text_callback);
+        RCLCPP_INFO(this->get_logger(), "Aikit TTS node initialized");
+    }
+
+    ~AikitTtsNode() {
+        AIKIT_UnInit(); // 释放AIKIT资源
+    }
+
+private:
+    rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscriber_;
+};
+
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<AikitTtsNode>();
+    g_node = node; // 供回调函数使用
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}

dist_dev/CMakeLists.txt

@@ -11,11 +11,12 @@ find_package(sensor_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED) 
 find_package(message_filters REQUIRED)
+find_package(interfaces REQUIRED)
 
-rosidl_generate_interfaces(${PROJECT_NAME}
-	"msg/DistMsg.msg"
-  DEPENDENCIES sensor_msgs
-)
+#rosidl_generate_interfaces(${PROJECT_NAME}
+#	"msg/DistMsg.msg"
+#  DEPENDENCIES sensor_msgs
+#)
 include_directories(
   include
   ${CMAKE_CURRENT_BINARY_DIR}
@@ -23,18 +24,16 @@ include_directories(
 )
 
 add_executable(dist_dev_node src/main.cpp src/dist_dev_node.cpp src/ks104.cpp)
-add_dependencies(dist_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
+#add_dependencies(dist_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
 
 target_link_libraries(dist_dev_node
-    ${PROJECT_NAME}__rosidl_typesupport_cpp
-    ${PROJECT_NAME}__rosidl_typesupport_c
     message_filters::message_filters
 )
 
 
-ament_target_dependencies(dist_dev_node rclcpp sensor_msgs)
+ament_target_dependencies(dist_dev_node rclcpp sensor_msgs interfaces)
 install(TARGETS dist_dev_node DESTINATION lib/${PROJECT_NAME})
-install(DIRECTORY msg/ DESTINATION share/${PROJECT_NAME}) 
+#install(DIRECTORY msg/ DESTINATION share/${PROJECT_NAME}) 
 install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch}) 
 ament_export_dependencies(rosidl_default_runtime)
 

dist_dev/include/dist_dev/dist_dev_node.hpp

@@ -5,7 +5,7 @@
 #include "message_filters/subscriber.h"
 #include "message_filters/time_synchronizer.h"
 #include "message_filters/sync_policies/approximate_time.hpp"
-#include "dist_dev/msg/dist_msg.hpp"
+#include "interfaces/msg/dist_msg.hpp"
 using namespace std;
 namespace dist_dev{
     
@@ -16,8 +16,8 @@ namespace dist_dev{
 	    void pub_msg(const sensor_msgs::msg::PointCloud2& pc,const DistCfg& cfg);
         void pub_msg(const DistCfg& cfg);
         private:
-            rclcpp::Publisher<dist_dev::msg::DistMsg>::SharedPtr dist_pub;
-            std::shared_ptr<dist_dev::msg::DistMsg> dist_msg; 
+            rclcpp::Publisher<interfaces::msg::DistMsg>::SharedPtr dist_pub;
+            std::shared_ptr<interfaces::msg::DistMsg> dist_msg; 
             
     };
 }

dist_dev/launch/dist_dev.launch.py

@@ -6,10 +6,13 @@ from launch.conditions import IfCondition
 
 def generate_launch_description():
     return LaunchDescription([
-        
+        ExecuteProcess(
+            cmd=["ros2","launch","livox_ros_driver2","msg_MID360_launch.py"],
+            output="screen"
+        ), 
         # 等待一段时间确保相机启动完成
         ExecuteProcess(
-            cmd=['sleep', '3'],
+            cmd=['sleep', '2'],
             output='screen'
         ),
         Node(

dist_dev/msg/DistMsg.msg

@@ -1,5 +0,0 @@
-string source
-string type
-string position
-int32[] ranges
-sensor_msgs/PointCloud2 points

dist_dev/src/dist_dev_node.cpp

@@ -1,15 +1,15 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/point_cloud2.hpp>
-#include "dist_dev/msg/dist_msg.hpp"// 自定义消息头文件
+#include "interfaces/msg/dist_msg.hpp"// 自定义消息头文件
 #include "dist_dev/dist_cfg.hpp"
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.hpp>
 #include <message_filters/synchronizer.h>
 #include"dist_dev/dist_dev_node.hpp"
 namespace dist_dev{
-DistSub::DistSub() : Node("DistSub") {
-    dist_msg= std::make_shared<dist_dev::msg::DistMsg>();
-    dist_pub = this->create_publisher<dist_dev::msg::DistMsg>("/dist_msg", 10);
+DistSub::DistSub() : Node("dist_dev_node") {
+    dist_msg= std::make_shared<interfaces::msg::DistMsg>();
+    dist_pub = this->create_publisher<interfaces::msg::DistMsg>("/dist_msg", 10);
 }
 static long long pub_cnt=0;
 void DistSub::pub_msg(const sensor_msgs::msg::PointCloud2& pc,const DistCfg& cfg){
@@ -29,7 +29,7 @@ void DistSub::pub_msg(const DistCfg& cfg){
     dist_msg->points= sensor_msgs::msg::PointCloud2();
     dist_pub->publish(*dist_msg);
     pub_cnt+=1;
-    std::cout<<"pub msg["<<pub_cnt<<"]:"<<std::endl;
+    std::cout<<"pub msg["<<pub_cnt<<"]:"<<dist_arr[0]<<","<<dist_arr[1]<<std::endl;
 }
 
 }

dist_dev/src/ks104.cpp

@@ -7,9 +7,10 @@ static int uart_send(const char *port, int baud, const u8 *tx_data, int tx_len,u
     // 1. 打开串口（非阻塞模式）
     int fd = open(port, O_RDWR | O_NOCTTY | O_NONBLOCK);
     if (fd < 0) {
-        perror("open port failed");
+        printf("open com:%s error\n",port);
         return -1;
     }
+   /// printf("now config:%s\n",port);
     // 2. 配置串口参数（8N1）
     struct termios options;
     tcgetattr(fd, &options);
@@ -54,7 +55,7 @@ static int uart_send(const char *port, int baud, const u8 *tx_data, int tx_len,u
     return total_read;
 }
 static int ks104_send(const u8 *tx,u8* rx){
-    int ret=uart_send("/dev/ttyUSB0",115200,tx,3,rx,128,100);
+    int ret=uart_send("/dev/ttyS0",115200,tx,3,rx,128,100);
     return ret;
 }
 int ks104_dist(u8 addr){

dist_dev/src/main.cpp

@@ -1,5 +1,5 @@
 #include <rclcpp/rclcpp.hpp>
-#include "dist_dev/msg/dist_msg.hpp"
+#include "interfaces/msg/dist_msg.hpp"
 #include "dist_dev/dist_cfg.hpp"
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.hpp>
@@ -22,7 +22,7 @@ int main(int argc, char* argv[]) {
         return -1;
     }
     //超声测距
-    cur_node->dist_arr.resize(4);
+    cur_node->dist_arr.resize(2);
     rclcpp::TimerBase::SharedPtr timer_=cur_node->create_wall_timer(std::chrono::milliseconds(100), [=]() { 
 
         //ls.header.frame_id="ultralsonic";
@@ -30,8 +30,8 @@ int main(int argc, char* argv[]) {
         #if 1
         cur_node->dist_arr[0]=ks104_dist(0xd0); 
         cur_node->dist_arr[1]=ks104_dist(0xd2); 
-        cur_node->dist_arr[2]=ks104_dist(0xd4); 
-        cur_node->dist_arr[3]=ks104_dist(0xd6); 
+       // cur_node->dist_arr[2]=ks104_dist(0xd4); 
+       // cur_node->dist_arr[3]=ks104_dist(0xd6); 
         cur_node->pub_msg(cfg0);
         #endif
     });

drv_test/CMakeLists.txt

@@ -0,0 +1,34 @@
+cmake_minimum_required(VERSION 3.8)
+project(drv_test)
+
+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(rclcpp REQUIRED)
+find_package(sensor_msgs REQUIRED)
+find_package(motor_dev REQUIRED)
+find_package(interfaces REQUIRED)
+find_package(std_msgs REQUIRED)
+
+add_executable(main src/main.cpp)
+add_executable(xmain src/xmain.cpp)
+ament_target_dependencies(main rclcpp motor_dev interfaces sensor_msgs)
+ament_target_dependencies(xmain rclcpp motor_dev interfaces sensor_msgs)
+install(TARGETS main DESTINATION lib/${PROJECT_NAME})
+install(TARGETS xmain DESTINATION lib/${PROJECT_NAME})
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # comment the line when a copyright and license is added to all source files
+  set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # comment the line when this package is in a git repo and when
+  # a copyright and license is added to all source files
+  set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

drv_test/package.xml

@@ -0,0 +1,23 @@
+<?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>drv_test</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="NuoDaJia02@hivecore.cn">demo</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>std_msg</depend>
+  <depend>rclcpp</depend>
+  <depend>sensor_msgs</depend>
+  <depend>motor_dev</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

drv_test/src/main.cpp

@@ -0,0 +1,161 @@
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "interfaces/msg/motor_cmd.hpp"
+#include "interfaces/srv/motor_info.hpp"
+#include "interfaces/srv/motor_param.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/image.hpp"
+using namespace std::chrono_literals;
+
+static auto g_program_start_time = std::chrono::steady_clock::now();
+
+double log_printf(const char* format, ...) {
+    auto now = std::chrono::steady_clock::now();
+    auto elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(now - g_program_start_time);
+    double seconds = elapsed.count();
+    double minute=seconds/60;
+    char buffer[1024];
+    va_list args;
+    va_start(args, format);
+    int n = vsnprintf(buffer, sizeof(buffer), format, args);
+    va_end(args);
+
+    if (n < 0) {
+        std::fprintf(stderr, "[%.3f] (log error)\n", seconds);
+        return 0;
+    }
+
+    std::fprintf(stdout, "[%.3f/%.1f] %s", seconds,minute, buffer);
+    std::fflush(stdout);
+    return seconds;
+}
+using MotorCmd = interfaces::msg::MotorCmd;
+using MotorInfo=interfaces::srv::MotorInfo;
+using MotorParam=interfaces::srv::MotorParam;
+class MotorCmdPublisher : public rclcpp::Node
+{
+public:
+  MotorCmdPublisher()
+  : Node("motor_test_node")
+  {
+    publisher_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
+    //publisher_ = this->create_publisher<std_msgs::msg::String>("/led_cmd", 10);
+     cam_sub=this->create_subscription<sensor_msgs::msg::Image>("/camera/camera/color/image_raw",10,[=](sensor_msgs::msg::Image::SharedPtr img){
+		     static double last_stamp;
+		   double cur_stamp= log_printf("sub img:%dx%d ",img->width,img->height);
+		   double diff=cur_stamp-last_stamp;
+		   last_stamp=cur_stamp;
+		   log_printf("hz:%.1f\n",1.0/diff);
+		     
+		     });
+    //client = create_client<MotorInfo>("/motor_info");
+    ///client = create_client<MotorParam>("/motor_param");
+    ////timer_ = this->create_wall_timer(5000ms, std::bind(&MotorCmdPublisher::timer_callback, this));
+  }
+ 
+private:
+  int angle1=9999;
+  int angle2=6666;
+  int xcnt=0;
+  //rclcpp::Client<interfaces::srv::MotorInfo>::SharedPtr client;
+  rclcpp::Client<interfaces::srv::MotorParam>::SharedPtr client;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam_sub;
+  void timer_callback()
+  {
+
+
+auto request = std::make_shared<MotorParam::Request>();
+request->motor_id=1;
+request->max_speed=20;
+request->add_acc=4;
+request->dec_acc=5;
+/*
+auto future = client->async_send_request(request);
+
+if (future.wait_for(5s) != std::future_status::ready) {
+  RCLCPP_ERROR(this->get_logger(), "服务调用超时");
+  return;
+}
+auto response=future.get();
+printf("ret :%d\n",response->ret);
+*/
+
+using ServiceResponseFuture = rclcpp::Client<interfaces::srv::MotorParam>::SharedFuture;
+    auto response_received_callback = [this](ServiceResponseFuture future) {
+        auto response = future.get();
+        RCLCPP_INFO(this->get_logger(), "Service response: ret = %d", response->ret);
+    };
+
+    auto future = client->async_send_request(request, response_received_callback);
+
+
+
+
+#if 0
+auto request = std::make_shared<MotorInfo::Request>();
+request->type="bm";
+request->motor_ids.clear();
+request->motor_ids.push_back(static_cast<uint8_t>(1));
+request->motor_ids.push_back(static_cast<uint8_t>(2));
+
+auto future = client->async_send_request(request);
+
+if (future.wait_for(10s) != std::future_status::ready) {
+  RCLCPP_ERROR(this->get_logger(), "服务调用超时");
+  return;
+}
+
+auto response = future.get();
+printf("ret angle:%.1f,%.1f\n",response->motor_angles[0],response->motor_angles[1]);
+#endif
+#if 0
+    auto message = MotorCmd();
+    
+    message.target = "rs485";
+    message.type = "bm";
+    message.position = "";
+    
+    message.motor_id.push_back(1);
+    message.motor_id.push_back(2);
+    idx+=1;
+    //angle1=angle1+20;
+    //angle2=angle2-20;
+    /*
+    if(idx%2==0){
+    }else{
+	angle1=-300;
+	angle2=300;
+    }
+    */
+    message.motor_angle.push_back((float)angle1);
+    message.motor_angle.push_back((float)angle2);
+    publisher_->publish(message);
+    RCLCPP_INFO(this->get_logger(), "Published MotorCmd message:%d,%d",angle1,angle2);
+#endif 
+#if 0  
+    auto message=std_msgs::msg::String();
+    xcnt+=1;
+    if(xcnt%2==0)
+    	message.data="green,10";
+    else
+	message.data="blue,10";
+    publisher_->publish(message);
+
+    RCLCPP_INFO(this->get_logger(), "Published LedCmd message:%s",message.data.c_str());
+#endif
+    //timer_->cancel();
+  }
+  int idx=0; 
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<MotorCmd>::SharedPtr publisher_;
+  //rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MotorCmdPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}

drv_test/src/xmain.cpp

@@ -0,0 +1,187 @@
+#include <chrono>
+#include <memory>
+#include "rclcpp/rclcpp.hpp"
+#include "interfaces/msg/motor_cmd.hpp"
+#include "interfaces/srv/motor_info.hpp"
+#include "interfaces/srv/motor_param.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/image.hpp"
+using namespace std::chrono_literals;
+
+static auto g_program_start_time = std::chrono::steady_clock::now();
+
+double log_printf(const char* format, ...) {
+    auto now = std::chrono::steady_clock::now();
+    auto elapsed = std::chrono::duration_cast<std::chrono::duration<double>>(now - g_program_start_time);
+    double seconds = elapsed.count();
+    double minute=seconds/60;
+    char buffer[1024];
+    va_list args;
+    va_start(args, format);
+    int n = vsnprintf(buffer, sizeof(buffer), format, args);
+    va_end(args);
+
+    if (n < 0) {
+        std::fprintf(stderr, "[%.3f] (log error)\n", seconds);
+        return 0;
+    }
+
+    std::fprintf(stdout, "[%.3f/%.1f] %s", seconds,minute, buffer);
+    std::fflush(stdout);
+    return seconds;
+}
+using MotorCmd = interfaces::msg::MotorCmd;
+using MotorInfo=interfaces::srv::MotorInfo;
+using MotorParam=interfaces::srv::MotorParam;
+class MotorCmdPublisher : public rclcpp::Node
+{
+public:
+  MotorCmdPublisher()
+  : Node("motor_test_node")
+  {
+    publisher_ = this->create_publisher<MotorCmd>("/motor_cmd", 10);
+    img_pub = this->create_publisher<sensor_msgs::msg::Image>("/color/image_raw",10);
+    //publisher_ = this->create_publisher<std_msgs::msg::String>("/led_cmd", 10);
+     cam_sub=this->create_subscription<sensor_msgs::msg::Image>("/camera/color/image_raw",10,[=](sensor_msgs::msg::Image::SharedPtr img){
+		     static double last_stamp;
+		   double cur_stamp= log_printf("sub img:%dx%d ",img->width,img->height);
+		   double diff=cur_stamp-last_stamp;
+		   last_stamp=cur_stamp;
+		   log_printf("hz:%.1f\n",1.0/diff);
+		     
+		     });
+    //client = create_client<MotorInfo>("/motor_info");
+    ///client = create_client<MotorParam>("/motor_param");
+    timer_ = this->create_wall_timer(33ms, std::bind(&MotorCmdPublisher::timer_callback, this));
+  }
+ 
+private:
+  int angle1=9999;
+  int angle2=6666;
+  int xcnt=0;
+  //rclcpp::Client<interfaces::srv::MotorInfo>::SharedPtr client;
+  rclcpp::Client<interfaces::srv::MotorParam>::SharedPtr client;
+  rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr cam_sub;
+  rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr img_pub;
+
+  void timer_callback()
+  {
+        auto msg = std::make_unique<sensor_msgs::msg::Image>();
+        msg->header.stamp = this->now();
+        msg->header.frame_id = "camera_frame";
+        msg->height = 720;
+        msg->width = 1280;
+        msg->encoding = "rgb8";  // 3通道RGB
+        msg->is_bigendian = false;
+        msg->step = msg->width * 3;  // 每行字节数 = 宽度 × 通道数
+
+        // 2. 填充随机数据 (1280x720x3 = 2,764,800字节)
+        msg->data.resize(msg->height * msg->step);
+        for (auto& pixel : msg->data) {
+            pixel = 23;//dist_(rng_);
+        }
+
+        // 3. 发布消息
+        img_pub->publish(std::move(msg));
+        static double last_stamp;
+        double cur_stamp=log_printf("pub msg:");
+        double diff=cur_stamp-last_stamp;
+        log_printf("hz:%.1f\n",1/diff);
+        last_stamp=cur_stamp;
+        return;
+
+
+// auto request = std::make_shared<MotorParam::Request>();
+// request->motor_id=1;
+// request->max_speed=20;
+// request->add_acc=4;
+// request->dec_acc=5;
+/*
+auto future = client->async_send_request(request);
+
+if (future.wait_for(5s) != std::future_status::ready) {
+  RCLCPP_ERROR(this->get_logger(), "服务调用超时");
+  return;
+}
+auto response=future.get();
+printf("ret :%d\n",response->ret);
+*/
+
+// using ServiceResponseFuture = rclcpp::Client<interfaces::srv::MotorParam>::SharedFuture;
+//     auto response_received_callback = [this](ServiceResponseFuture future) {
+//         auto response = future.get();
+//         RCLCPP_INFO(this->get_logger(), "Service response: ret = %d", response->ret);
+//     };
+
+//     auto future = client->async_send_request(request, response_received_callback);
+
+
+
+
+#if 0
+auto request = std::make_shared<MotorInfo::Request>();
+request->type="bm";
+request->motor_ids.clear();
+request->motor_ids.push_back(static_cast<uint8_t>(1));
+request->motor_ids.push_back(static_cast<uint8_t>(2));
+
+auto future = client->async_send_request(request);
+
+if (future.wait_for(10s) != std::future_status::ready) {
+  RCLCPP_ERROR(this->get_logger(), "服务调用超时");
+  return;
+}
+
+auto response = future.get();
+printf("ret angle:%.1f,%.1f\n",response->motor_angles[0],response->motor_angles[1]);
+#endif
+#if 0
+    auto message = MotorCmd();
+    
+    message.target = "rs485";
+    message.type = "bm";
+    message.position = "";
+    
+    message.motor_id.push_back(1);
+    message.motor_id.push_back(2);
+    idx+=1;
+    //angle1=angle1+20;
+    //angle2=angle2-20;
+    /*
+    if(idx%2==0){
+    }else{
+	angle1=-300;
+	angle2=300;
+    }
+    */
+    message.motor_angle.push_back((float)angle1);
+    message.motor_angle.push_back((float)angle2);
+    publisher_->publish(message);
+    RCLCPP_INFO(this->get_logger(), "Published MotorCmd message:%d,%d",angle1,angle2);
+#endif 
+#if 0  
+    auto message=std_msgs::msg::String();
+    xcnt+=1;
+    if(xcnt%2==0)
+    	message.data="green,10";
+    else
+	message.data="blue,10";
+    publisher_->publish(message);
+
+    RCLCPP_INFO(this->get_logger(), "Published LedCmd message:%s",message.data.c_str());
+#endif
+    //timer_->cancel();
+  }
+  int idx=0; 
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Publisher<MotorCmd>::SharedPtr publisher_;
+  //rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<MotorCmdPublisher>());
+  rclcpp::shutdown();
+  return 0;
+}

ecrt_dev_dual/CMakeLists.txt

@@ -0,0 +1,106 @@
+cmake_minimum_required(VERSION 3.8)
+project(ecrt_driver)
+
+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(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(rclcpp_action REQUIRED)
+find_package(trajectory_msgs REQUIRED)
+find_package(control_msgs REQUIRED)
+find_package(controller_manager_msgs REQUIRED)
+find_library(ECRT_LIB
+  NAMES ethercat ecrt
+  PATHS /opt/etherlab/lib /usr/local/lib /usr/lib
+)
+if(NOT ECRT_LIB)
+  message(FATAL_ERROR "ecrt not found; set CMAKE_PREFIX_PATH or system ld path.")
+endif()
+add_library(${PROJECT_NAME} SHARED src/ethercat_driver.cpp)
+target_link_libraries(${PROJECT_NAME} ${ECRT_LIB} pthread)
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  rclcpp_action
+)
+
+
+# 构建可执行文件
+#add_executable(test_motor src/test_motor.cpp)
+#ament_target_dependencies(test_motor 
+#  rclcpp 
+#  rclcpp_action
+#  geometry_msgs 
+#  sensor_msgs
+#  trajectory_msgs
+#  control_msgs
+#  controller_manager_msgs
+#)
+#install(TARGETS test_motor DESTINATION lib/${PROJECT_NAME})
+
+add_executable(dual_motor src/dual_motor.cpp)
+ament_target_dependencies(dual_motor 
+  rclcpp 
+  rclcpp_action
+  geometry_msgs 
+  sensor_msgs
+  trajectory_msgs
+  control_msgs
+  controller_manager_msgs
+)
+install(TARGETS dual_motor DESTINATION lib/${PROJECT_NAME})
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(${PROJECT_NAME} PRIVATE "ETHERCAT_DRIVER_BUILDING_LIBRARY")
+# prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+pluginlib_export_plugin_description_file(hardware_interface ethercat_driver_plugin.xml)
+
+# INSTALL
+install(
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+install(
+  DIRECTORY config description launch
+  DESTINATION share/${PROJECT_NAME}
+)
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  ethercat_interface
+)
+ament_package()

ecrt_dev_dual/config/dual_arm_controllers.yaml

@@ -0,0 +1,72 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 250
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+    left_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    right_arm_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+    left_arm_gpio_controller:
+      type: gpio_controllers/GpioCommandController
+    right_arm_gpio_controller:
+      type: gpio_controllers/GpioCommandController
+left_arm_controller:
+  ros__parameters:
+    joints: [left_arm_joint_1, left_arm_joint_2,left_arm_joint_3,left_arm_joint_4,left_arm_joint_5,left_arm_joint_6]
+    command_interfaces: [position]
+    state_interfaces: [position, velocity]
+
+right_arm_controller:
+  ros__parameters:
+    joints: [right_arm_joint_1,right_arm_joint_2,right_arm_joint_3,right_arm_joint_4,right_arm_joint_5,right_arm_joint_6]
+    command_interfaces: [position]
+    state_interfaces: [position, velocity]
+
+left_arm_gpio_controller:
+  ros__parameters:
+    gpios:
+      - left_arm_joint_1
+      - left_arm_joint_2
+      - left_arm_joint_3
+      - left_arm_joint_4
+      - left_arm_joint_5
+      - left_arm_joint_6
+    command_interfaces:
+      left_arm_joint_1:  { interfaces: [fault, enable] }
+      left_arm_joint_2:  { interfaces: [fault, enable] }
+      left_arm_joint_3:  { interfaces: [fault, enable] }
+      left_arm_joint_4:  { interfaces: [fault, enable] }
+      left_arm_joint_5:  { interfaces: [fault, enable] }
+      left_arm_joint_6:  { interfaces: [fault, enable] }
+    state_interfaces:
+      left_arm_joint_1:  { interfaces: [fault, enable] }
+      left_arm_joint_2:  { interfaces: [fault, enable] }
+      left_arm_joint_3:  { interfaces: [fault, enable] }
+      left_arm_joint_4:  { interfaces: [fault, enable] }
+      left_arm_joint_5:  { interfaces: [fault, enable] }
+      left_arm_joint_6:  { interfaces: [fault, enable] }
+
+right_arm_gpio_controller:
+  ros__parameters:
+    gpios:
+      - right_arm_joint_1
+      - right_arm_joint_2
+      - right_arm_joint_3
+      - right_arm_joint_4
+      - right_arm_joint_5
+      - right_arm_joint_6
+    command_interfaces:
+      right_arm_joint_1:  { interfaces: [fault, enable] }
+      right_arm_joint_2:  { interfaces: [fault, enable] }
+      right_arm_joint_3:  { interfaces: [fault, enable] }
+      right_arm_joint_4:  { interfaces: [fault, enable] }
+      right_arm_joint_5:  { interfaces: [fault, enable] }
+      right_arm_joint_6:  { interfaces: [fault, enable] }
+    state_interfaces:
+      right_arm_joint_1:  { interfaces: [fault, enable] }
+      right_arm_joint_2:  { interfaces: [fault, enable] }
+      right_arm_joint_3:  { interfaces: [fault, enable] }
+      right_arm_joint_4:  { interfaces: [fault, enable] }
+      right_arm_joint_5:  { interfaces: [fault, enable] }
+      right_arm_joint_6:  { interfaces: [fault, enable] }

ecrt_dev_dual/config/zeroErr_config.yaml

@@ -0,0 +1,34 @@
+# Configuration file for Maxon EPOS3 drive
+vendor_id: 0x5a65726f
+product_id: 0x00029252
+assign-activate: 0x0300   #DC synch register
+auto_fault_reset: true
+auto_enable_set: true
+auto_state_transitions: true
+sdo:  # sdo data to be transferred at drive startup
+   - {index: 0x6060, sub_index: 0, type: uint16, value: 8} # Set 0x1C33:01h = 0x02  DC设置
+   #- {index: 0x6040, sub_index: 0, type: uint16, value: 0x80} # 清除错误
+   #- {index: 0x6040, sub_index: 0, type: uint16, value: 0}
+
+rpdo: #PxPDO = receive PDO Mapping
+  - index: 0x1600
+    channels:
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} #target position	
+      # - {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0}	#target velocity
+      # - {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0}	#target torque
+      # - {index: 0x6072, sub_index: 0, type: uint16, default: 500}	# Max torque
+      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}	# Control word
+      - {index: 0x6060, sub_index: 0, type: uint8, default: 8}	# Control operation
+      - {index: 0xf0ff, sub_index: 0, type: uint8}	# Dummy byte
+    
+tpdo:  #TxPDO = transmit PDO Mapping
+  - index: 0x1a00
+    channels:
+      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 1.0, offset: 0} #Position actual value	
+      # - {index: 0x60F4, sub_index: 0, type: int32}	#Position Following error actual value
+      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 1.0 }	# Velocity actual value
+      # - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 1.0 }	# Torque actual value
+      - {index: 0x6041, sub_index: 0, type: uint16}	# State word
+      # - {index: 0x603f, sub_index: 0, type: uint16}	# Error code
+      - {index: 0x6061, sub_index: 0, type: uint8, state_interface: mode_of_operation}	        # Mode of operation display
+      - {index: 0xf0ff, sub_index: 0, type: uint8}	        # dummy byte

ecrt_dev_dual/description/motor_drive.config.xacro

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="motor_drive">
+
+  <link name="world"/>
+
+  <xacro:include filename="motor_drive.ros2_control.xacro" />
+
+  <xacro:motor_drive/>
+</robot>

ecrt_dev_dual/description/motor_drive.ros2_control.xacro

@@ -0,0 +1,64 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <!-- 公共参数定义（一次修改，全局生效） -->
+  <xacro:property name="master_id" value="0" />
+  <xacro:property name="control_frequency" value="10" />
+  <xacro:property name="ec_plugin" value="ethercat_generic_plugins/EcCiA402Drive" />
+  <xacro:property name="alias" value="0" />
+  <xacro:property name="mode_of_operation" value="8" />
+  <xacro:property name="slave_config_path" value="config/zeroErr_config.yaml" />
+
+  <!-- 子宏：封装单个关节的重复配置 -->
+  <!-- 参数：joint_name（关节名称）、ec_position（EtherCAT位置索引） -->
+  <xacro:macro name="single_joint_config" params="joint_name ec_position">
+    <joint name="${joint_name}">
+      <!-- 状态接口（位置/速度/力矩） -->
+      <state_interface name="position"/>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+      <state_interface name="fault"/>
+      <state_interface name="enable"/>
+
+      <!-- 命令接口（位置控制/故障重置/使能） -->
+      
+      <command_interface name="fault"/>
+      <command_interface name="enable"/>
+      <command_interface name="position"/>
+      <!-- EtherCAT模块配置（复用公共参数） -->
+      <ec_module name="zeroErr">
+        <plugin>${ec_plugin}</plugin>
+        <param name="alias">${alias}</param>
+        <param name="position">${ec_position}</param>
+        <param name="mode_of_operation">${mode_of_operation}</param>
+        <param name="slave_config">${slave_config_path}</param>
+      </ec_module>
+    </joint>
+  </xacro:macro>
+
+  <!-- 主宏：电机驱动系统配置 -->
+  <xacro:macro name="motor_drive">
+    <ros2_control name="motor_drive" type="system">
+      <hardware>
+        <plugin>ethercat_driver/EthercatDriver</plugin>
+        <param name="master_id">${master_id}</param>
+        <param name="control_frequency">${control_frequency}</param>
+      </hardware>
+
+      <!-- 调用子宏生成多个关节（仅需指定名称和位置索引） -->
+      <xacro:single_joint_config joint_name="left_arm_joint_1" ec_position="1" />
+      <xacro:single_joint_config joint_name="left_arm_joint_2" ec_position="2" />
+      <xacro:single_joint_config joint_name="left_arm_joint_3" ec_position="3" />
+      <xacro:single_joint_config joint_name="left_arm_joint_4" ec_position="4" />
+      <xacro:single_joint_config joint_name="left_arm_joint_5" ec_position="5" />
+      <xacro:single_joint_config joint_name="left_arm_joint_6" ec_position="6" />
+      <xacro:single_joint_config joint_name="right_arm_joint_1" ec_position="7" />
+      <xacro:single_joint_config joint_name="right_arm_joint_2" ec_position="8" />
+      <xacro:single_joint_config joint_name="right_arm_joint_3" ec_position="9" />
+      <xacro:single_joint_config joint_name="right_arm_joint_4" ec_position="10" />
+      <xacro:single_joint_config joint_name="right_arm_joint_5" ec_position="11" />
+      <xacro:single_joint_config joint_name="right_arm_joint_6" ec_position="12" />
+    </ros2_control>
+  </xacro:macro>
+
+</robot>

ecrt_dev_dual/ethercat_driver_plugin.xml

@@ -0,0 +1,9 @@
+<library path="ecrt_driver">
+  <class name="ethercat_driver/EthercatDriver"
+         type="ethercat_driver::EthercatDriver"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+        EtherCAT Driver for ros2_control.
+    </description>
+  </class>
+</library>

ecrt_dev_dual/include/ethercat_driver/ethercat_driver.hpp

@@ -0,0 +1,194 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// 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 ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
+#define ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
+#include<thread>
+#include <unordered_map>
+#include <memory>
+#include <string>
+#include <vector>
+#include <array>
+#include <atomic>
+#include <pluginlib/class_loader.hpp>
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "ethercat_driver/visibility_control.h"
+#include <pthread.h>
+#include <sched.h>
+#include <sys/mman.h>
+#include <atomic>
+//#include "ethercat_interface/ec_slave.hpp"
+//#include "ethercat_interface/ec_master.hpp"
+#include "zer_config.hpp"
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+#define CYCLIC_POSITION            8            //CSP 周期同步位置模式
+#define CYCLIC_VELOCITY            9            //CSP 周期同步速度模式
+#define CYCLIC_TORQUE              10           //CSP 周期同步扭矩模式
+#define PVT_MODE                   5            //PVT模式
+namespace ethercat_driver
+{
+
+class EthercatDriver : public hardware_interface::SystemInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(EthercatDriver)
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
+  void readData();
+  void writeData();
+  void check_master_state();
+  void check_domain1_state();
+  bool check_slave_config_states();
+  void set_motor_enable(int id,bool enable){
+    if(id>0&&id<NUM_SLAVES+1){
+        motor_enable_arr[id-1].store(enable);
+    }
+  };
+  bool get_motor_enable(int id){
+    if(id>0&&id<NUM_SLAVES+1){
+        return motor_enable_arr[id-1].load();
+    }
+    return false;
+  }
+  struct timespec timespec_add(struct timespec time1, struct timespec time2);
+private:
+  std::chrono::high_resolution_clock::time_point rec_time;
+  std::array<std::atomic<bool>, NUM_SLAVES> motor_enable_arr;
+  std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
+    std::string & urdf, std::string component_name, std::string component_type);
+
+  ///std::vector<std::shared_ptr<ethercat_interface::EcSlave>> ec_modules_;
+  std::vector<std::unordered_map<std::string, std::string>> ec_module_parameters_;
+
+  std::vector<std::vector<double>> hw_joint_commands_;
+  std::vector<double> hw_cmd_position_;
+  std::vector<std::vector<double>> hw_sensor_commands_;
+  std::vector<std::vector<double>> hw_gpio_commands_;
+  std::vector<std::vector<double>> hw_joint_states_;
+  std::vector<std::vector<double>> hw_sensor_states_;
+  std::vector<std::vector<double>> hw_gpio_states_;
+
+  
+  //pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{"ethercat_interface", "ethercat_interface::EcSlave"};
+    struct timespec wakeupTime,time;
+  int control_frequency_;
+  ///ethercat_interface::EcMaster master_;
+  ec_master_t *master = NULL;
+  ec_domain_t *domain1 = NULL;
+  ec_slave_config_t *sc[NUM_SLAVES] = {0}; 
+  uint8_t *domain1_pd = NULL;
+  ec_domain_state_t domain1_state = {};
+  ec_master_state_t master_state = {};
+  ec_slave_config_state_t sc_state[NUM_SLAVES] = {};
+  bool all_motor_op=false;
+
+  std::mutex ec_mutex_;
+  int32_t getValidCount_ze(uint8_t id){
+    int32_t val=dst_rads[id]*rad_to_count+pos_offsets[id];
+    if(id<NUM_SLAVES){
+      int32_t min=pos_offsets[id]-262000;//262144
+      int32_t max=pos_offsets[id]+262000;
+      if(val<min){
+        return min;
+      }else if(val>max){
+        return max;
+      }
+    }
+    return val;
+  };
+  double getValidAngle(uint8_t id){
+    double rad=hw_joint_commands_[id][2];
+    if(id<NUM_SLAVES){
+      if(id==2||id==8)
+        rad=-rad;
+      if(rad>M_PI){
+        return M_PI-0.001;
+      }else if(rad<-M_PI){
+        return -M_PI+0.001;
+      }
+    }
+    return rad;
+  }
+  std::atomic<bool> activated_{false};
+  #define FREQUENCY 250
+  #define CSP_MAX_VEL_COUNTS_PER_S    65536
+   #define CSP_POS_DEADBAND           10
+  #define CSP_DEADBAND1           100//100           //CSP允许的误差(计数),原来是10
+  #define CSP_DEADBAND2           1000 //300
+  #define CSP_SPEED1 20//30
+  #define CSP_SPEED2 100
+  #define CLOCK_TO_USE CLOCK_MONOTONIC
+  #define NSEC_PER_SEC (1000000000L)
+  #define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)    //本次设置周期PERIOD_NS为1ms
+  #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + (B).tv_nsec - (A).tv_nsec)
+  #define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
+  const struct timespec cycletime = {0, PERIOD_NS};
+  uint16_t clear_cmd[NUM_SLAVES]; 
+  uint16_t command[NUM_SLAVES];             //状态字掩码
+  uint16_t status[NUM_SLAVES];              //状态字
+  uint16_t last_status[NUM_SLAVES];         //上个循环的状态字
+  int8_t  last_mode_cmd[NUM_SLAVES] = {CYCLIC_VELOCITY};
+  uint8_t reach_target[NUM_SLAVES];
+  uint8_t op_states[NUM_SLAVES]={0};
+  double dst_rads[NUM_SLAVES]={0};
+  int32_t step_pos[NUM_SLAVES]={0};
+  int32_t cur_pos[NUM_SLAVES]={0};
+  double enable_arr[NUM_SLAVES]={0};
+  int8_t mode_cmd=8;
+  int inited = 0;                           //初始化
+  unsigned int counter = 0;
+  unsigned int sync_ref_counter = 0;
+  bool has_clear_all=false;
+  enum ErrState{
+    ERR_NONE,
+    ERR_ACK,
+    ERR_ACK_WAIT,
+    ERR_CLEAR,
+    ERR_CLEAR_WAIT,
+    ERR_FIN,
+    ERR_FIN_WAIT
+  };
+  ErrState errStaArr[NUM_SLAVES]={ERR_NONE};
+};
+}  // namespace ethercat_driver
+
+#endif  // ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_

ecrt_dev_dual/include/ethercat_driver/visibility_control.h

@@ -0,0 +1,49 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// 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 ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_
+#define ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define ETHERCAT_DRIVER_EXPORT __attribute__ ((dllexport))
+    #define ETHERCAT_DRIVER_IMPORT __attribute__ ((dllimport))
+  #else
+    #define ETHERCAT_DRIVER_EXPORT __declspec(dllexport)
+    #define ETHERCAT_DRIVER_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef ETHERCAT_DRIVER_BUILDING_LIBRARY
+    #define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_EXPORT
+  #else
+    #define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_IMPORT
+  #endif
+  #define ETHERCAT_DRIVER_PUBLIC_TYPE ETHERCAT_DRIVER_PUBLIC
+  #define ETHERCAT_DRIVER_LOCAL
+#else
+  #define ETHERCAT_DRIVER_EXPORT __attribute__ ((visibility("default")))
+  #define ETHERCAT_DRIVER_IMPORT
+  #if __GNUC__ >= 4
+    #define ETHERCAT_DRIVER_PUBLIC __attribute__ ((visibility("default")))
+    #define ETHERCAT_DRIVER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define ETHERCAT_DRIVER_PUBLIC
+    #define ETHERCAT_DRIVER_LOCAL
+  #endif
+  #define ETHERCAT_DRIVER_PUBLIC_TYPE
+#endif
+
+#endif  // ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_

ecrt_dev_dual/include/ethercat_driver/zer_config.hpp

@@ -0,0 +1,116 @@
+#include "ecrt.h"
+#include <math.h>
+#define NUM_SLAVES 12
+#define ZER_VID_PID          0x5a65726f,0x00029252
+#define YY_VID_PID         0x00001097,0x00002406
+typedef struct {
+    unsigned int ctrl_word;                   // 0x6040:00
+    unsigned int target_position;             // 0x607A:00
+    unsigned int target_velocity;             // 0x60FF:00 
+    unsigned int target_torque;               // 0x6071:00 
+    unsigned int max_torque;                  // 0x6072:00 
+    unsigned int operation_mode;              // 0x6060:00
+    unsigned int reserved1;                   // 0xf0ff:00 
+    unsigned int status_word;                 // 0x6041:00
+    unsigned int position_actual_value;       // 0x6064:00
+    unsigned int velocity_actual_value;       // 0x606C:00 
+    unsigned int torque_actual_value;         // 0x6077:00 
+    unsigned int error_code;                  // 0x603F:00
+    unsigned int modes_of_operation_display;  // 0x6061:00
+    unsigned int reserved2;                   // 0xf0ff:00 
+} zer_offset_t;
+
+static zer_offset_t zer_offsets[NUM_SLAVES];
+static double rated_currents[NUM_SLAVES]={12.5,12.5,6.3,5.4,5.4,5.4,12.5,12.5,6.3,5.4,5.4,5.4};
+static double pos_offsets[NUM_SLAVES]={248214.0,358098.0,251704.0,240977.0,280113.0,54646.0, 211980.0,262157.0,281128.0,270017.0,268839.0,275363.0};
+//static double pos_offsets[NUM_SLAVES]={-13930.0, 95954.0, -10440.0, -21167.0, 17969.0, -207498.0, -50164.0, 13.0, 18984.0, 7873.0, 1553.0, 13219.0};
+constexpr double rad_to_count= 524288.0/(2*M_PI);
+constexpr double count_to_rad=2*M_PI/524288.0;
+constexpr double speed_to_count=524288.0/(2*M_PI);//262144.0/(2*M_PI);
+constexpr double count_to_speed=2*M_PI/524288.0;//2*M_PI/262144.0;
+#define PDO_ENTRY(alias,position,index) \
+    {alias,position, ZER_VID_PID, 0x6040, 0, &zer_offsets[index].ctrl_word,NULL}, \
+    {alias,position, ZER_VID_PID, 0x607A, 0, &zer_offsets[index].target_position,NULL},\
+    {alias,position, ZER_VID_PID, 0x60FF, 0, &zer_offsets[index].target_velocity,NULL},\
+    {alias,position, ZER_VID_PID, 0x6071, 0, &zer_offsets[index].target_torque,NULL},\
+    {alias,position, ZER_VID_PID, 0x6072, 0, &zer_offsets[index].max_torque,NULL},\
+    {alias,position, ZER_VID_PID, 0x6060, 0, &zer_offsets[index].operation_mode,NULL},\
+    {alias,position, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved1,NULL},\
+    {alias,position, ZER_VID_PID, 0x6041, 0, &zer_offsets[index].status_word,NULL},\
+    {alias,position, ZER_VID_PID, 0x6064, 0, &zer_offsets[index].position_actual_value,NULL},\
+    {alias,position, ZER_VID_PID, 0x606C, 0, &zer_offsets[index].velocity_actual_value,NULL},\
+    {alias,position, ZER_VID_PID, 0x6077, 0, &zer_offsets[index].torque_actual_value,NULL},\
+    {alias,position, ZER_VID_PID, 0x603F, 0, &zer_offsets[index].error_code,NULL},\
+    {alias,position, ZER_VID_PID, 0x6061, 0, &zer_offsets[index].modes_of_operation_display,NULL},\
+    {alias,position, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[index].reserved2,NULL},
+// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
+#if 1
+const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
+    PDO_ENTRY(0,1,0)
+    PDO_ENTRY(0,2,1)
+    PDO_ENTRY(0,3,2)
+    PDO_ENTRY(0,4,3)
+    PDO_ENTRY(0,5,4)
+    PDO_ENTRY(0,6,5)
+
+    PDO_ENTRY(0,7,6)
+    PDO_ENTRY(0,8,7)
+    PDO_ENTRY(0,9,8)
+    PDO_ENTRY(0,10,9)
+    PDO_ENTRY(0,11,10)
+    PDO_ENTRY(0,12,11)
+    {} // 结束标记
+};
+#else
+const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
+    YY_PDO_ENTRY(0,1,0)
+    PDO_ENTRY(0,2,1)
+    PDO_ENTRY(0,3,2)
+    PDO_ENTRY(0,4,3)
+    PDO_ENTRY(0,5,4)
+    PDO_ENTRY(0,6,5)
+    PDO_ENTRY(0,7,6)
+
+    PDO_ENTRY(0,8,7)
+    PDO_ENTRY(0,9,8)
+    PDO_ENTRY(0,10,9)
+    PDO_ENTRY(0,11,10)
+    PDO_ENTRY(0,12,11)
+    PDO_ENTRY(0,13,12)
+    {} // 结束标记
+};
+#endif
+
+static ec_pdo_entry_info_t zer_device_pdo_entries[] = {
+    /*RxPdo 0x1600*/
+    {0x6040, 0x00, 16}, /* control word */
+    {0x607a, 0x00, 32}, /* target position */
+    {0x60ff, 0x00, 32}, /* target velocity */
+    {0x6071, 0x00, 16}, /* Target Torque */
+    {0x6072, 0x00, 16}, /* Max Torque */
+    {0x6060, 0x00, 8}, /* modes of operation */
+    {0xf0ff, 0x00, 8},
+    /*TxPdo 0x1A00*/
+    {0x6041, 0x00, 16}, /* status word */
+    {0x6064, 0x00, 32}, /* position actual value */
+    {0x606c, 0x00, 32}, /* velocity actual value */
+    {0x6077, 0x00, 16}, /* torque actual value */
+    {0x603f, 0x00, 16}, /* error code */
+    {0x6061, 0x00, 8}, /* modes of operation display */
+    {0xf0ff, 0x00, 8},
+};
+
+static ec_pdo_info_t zer_device_pdos[] = {
+    //RxPdo
+    {0x1600, 7, zer_device_pdo_entries + 0 },
+    //TxPdo
+    {0x1A00, 7, zer_device_pdo_entries + 7 }
+};
+
+static ec_sync_info_t zer_device_syncs[] = {
+    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
+    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
+    {2, EC_DIR_OUTPUT, 1, zer_device_pdos + 0, EC_WD_ENABLE},
+    {3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE},
+    {0xff}
+};

ecrt_dev_dual/launch/dual_arm.launch.py

@@ -0,0 +1,55 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution
+from launch_ros.substitutions import FindPackageShare
+
+def load_robot_desc():
+    return {
+        "robot_description": Command([
+            FindExecutable(name="xacro"), " ",
+            PathJoinSubstitution([
+                FindPackageShare("ecrt_driver"),
+                "description", "motor_drive.config.xacro"
+            ])
+        ])
+    }
+
+def generate_launch_description():
+    ld = LaunchDescription()
+    ret=load_robot_desc()
+    print(ret["robot_description"])
+    ld.add_action(Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[load_robot_desc(),
+                    PathJoinSubstitution([
+                        FindPackageShare("ecrt_driver"),
+                        "config", "dual_arm_controllers.yaml"
+                    ]),{"lock_memory": True,
+                            "thread_priority": 99,
+                            "cpu_affinity": 6}
+                    ],
+        output="both"))
+    ld.add_action(Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[load_robot_desc()]))
+    #状态发布
+    jsb = Node(package="controller_manager", executable="spawner",
+               arguments=["joint_state_broadcaster"])
+    ld.add_action(jsb)
+    #左右关节
+    left  = Node(package="controller_manager", executable="spawner",
+                 arguments=["left_arm_controller",'--inactive'])
+    right = Node(package="controller_manager", executable="spawner",
+                 arguments=["right_arm_controller",'--inactive'])
+    #左右gpio
+    l_gpio  = Node(package="controller_manager", executable="spawner",
+                 arguments=["left_arm_gpio_controller"])
+    r_gpio  = Node(package="controller_manager", executable="spawner",
+                 arguments=["right_arm_gpio_controller"])
+    ld.add_action(left)
+    ld.add_action(right)
+    ld.add_action(l_gpio)
+    ld.add_action(r_gpio)
+    return ld

ecrt_dev_dual/package.xml

@@ -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>ecrt_driver</name>
+  <version>1.2.0</version>
+  <description>EtherCAT Driver for `ros2_control`</description>
+  <maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+<depend>rclcpp_action</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>ethercat_interface</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

ecrt_dev_dual/src/dual_motor.cpp

@@ -0,0 +1,484 @@
+#include "rclcpp/rclcpp.hpp"
+#include <filesystem>
+#include <fstream>  // 添加这行来支持文件流操作
+#include <time.h>
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include "std_msgs/msg/string.hpp"
+#include "trajectory_msgs/msg/joint_trajectory.hpp"
+#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "control_msgs/msg/dynamic_interface_group_values.hpp"
+#include "control_msgs/action/follow_joint_trajectory.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+#include <controller_manager_msgs/srv/switch_controller.hpp>
+
+namespace fs = std::filesystem;
+constexpr double rad_to_count= 524288.0/(2*M_PI);
+constexpr double count_to_rad=2*M_PI/524288.0;
+ class TestMotor : public rclcpp::Node 
+    {
+        public:
+            TestMotor();
+            ~TestMotor();
+            
+	        int motor_dst(std::string name,double dst);//add by hehe
+            //void ctrl_motor(int id,double pos,int reset,int enable);
+            void left_clear(double clear);
+            void left_enable(double enable);
+            void right_clear(double clear);
+            void right_enable(double enable);
+            void motor_action(int id,double angle);
+            void ControlLoop();
+            bool activateController(const std::string& controller_name);
+            void setJointValueTarget(double angle);
+            void setJointValueTarget(const std::vector<double> angles);
+            void pubTraj(const std::vector<double> angles1,const std::vector<double> angles2);
+            void pubTraj(const double angle);
+            bool motor_drv_on=false;
+            bool all_motor_op=false;
+            bool is_reach=false;
+            std::unordered_map<std::string,double> curMap_;
+	        std::unordered_map<std::string,double> dstMap_;
+            void left_pubTraj(const std::vector<double> angles);
+            void right_pubTraj(const std::vector<double> angles);
+        private:
+            //rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr cmdPub_;
+            rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr left_ctl_;
+            rclcpp::Publisher<trajectory_msgs::msg::JointTrajectory>::SharedPtr right_ctl_;
+            ///rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr gpioPub_;
+            rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr left_gpio_;
+            rclcpp::Publisher<control_msgs::msg::DynamicInterfaceGroupValues>::SharedPtr right_gpio_;
+
+            rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr jointStatesSub_;
+            //rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr left_state_;
+            //rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr right_state_;
+
+            rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SharedPtr client_;
+            rclcpp::Client<controller_manager_msgs::srv::SwitchController>::SharedPtr switch_client;
+             // 文件流相关
+            std::ofstream data_file_;  // 用于写入数据的文件流
+            std::string data_file_path_;  // 数据文件路径
+
+            rclcpp::TimerBase::SharedPtr controlTimer_;
+            rclcpp::Time lastTime_;  // 移至类成员
+            struct termios original_termios_;
+            
+	    //add by hehe
+	    sensor_msgs::msg::JointState curJointSta_;
+        int loop_cnt=0;
+	    //add by hehe end
+            // 执行当前状态对应的动作
+            void JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg);
+    };
+
+// 机器人控制器构造函数
+TestMotor::TestMotor() : Node("test_motor_node")
+{
+    std::cout << "TestMotor Node is created!" << std::endl;
+     // 创建发布者
+    left_ctl_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/left_arm_controller/joint_trajectory", 10);
+    right_ctl_ = this->create_publisher<trajectory_msgs::msg::JointTrajectory>("/right_arm_controller/joint_trajectory", 10);
+    //action
+    client_=rclcpp_action::create_client<control_msgs::action::FollowJointTrajectory>(this,"/trajectory_controller/follow_joint_trajectory");
+    switch_client = create_client<controller_manager_msgs::srv::SwitchController>("/controller_manager/switch_controller");
+    // 创建发布者
+    //gpioPub_ = this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/gpio_command_controller/commands", 10);
+    left_gpio_=this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("/left_arm_gpio_controller/commands",10);
+    right_gpio_=this->create_publisher<control_msgs::msg::DynamicInterfaceGroupValues>("right_arm_gpio_controller/commands",10);
+    // 订阅仿真状态
+    jointStatesSub_ = this->create_subscription<sensor_msgs::msg::JointState>("/joint_states", 10,std::bind(&TestMotor::JointStatesCallback, this, std::placeholders::_1));
+    
+    lastTime_ = this->now();  // 初始化时间
+    // 创建定时器，每10ms执行一次控制逻辑（频率100Hz）
+    controlTimer_ = this->create_wall_timer(std::chrono::milliseconds(5000),std::bind(&TestMotor::ControlLoop, this));  // 绑定到新的控制函数);
+    curMap_.clear();
+    dstMap_.clear();
+    //posMsg_.interface_groups.resize(12);
+    //posMsg_.interface_values.resize(12);
+    ///std::cout << "start enblex ..." << std::endl;
+
+    std::cout << "TestMotor Node is created finished!" << std::endl;
+}
+
+TestMotor::~TestMotor() 
+{
+    std::cout << "Robot controller stopped." << std::endl;
+}
+///std::vector<double> angles={-40,  -40, -40, 0, -40, 0,40, 40, 40, 0, 40, 0};
+void TestMotor::JointStatesCallback(const sensor_msgs::msg::JointState::SharedPtr msg) 
+{
+    if (!msg) {  // 检查消息是否有效
+        std::cout << "get null joint states!" << std::endl;
+        return;
+    }
+    if(msg->name.size()>0)
+        motor_drv_on=true;
+    //printf("motor_drv_on:%d\n",motor_drv_on);
+    all_motor_op=true;
+    for(int i=0;i<msg->name.size();i++)
+    {
+        curMap_[msg->name[i]] = msg->position[i];
+        ///std::cout<<"cur:"<<msg->name[i]<<":"<<curMap_[msg->name[i]]<<std::endl;
+        if(curMap_[msg->name[i]]==9999.0)
+            all_motor_op=false;
+    }
+    if(all_motor_op){
+        if(is_reach){
+            ////pubTraj(angles);
+        }
+    }
+}
+bool TestMotor::activateController(const std::string& controller_name) {
+        ///std::cout<<"激活控制器"<<controller_name<<std::endl;
+        auto request = std::make_shared<controller_manager_msgs::srv::SwitchController::Request>();
+        request->activate_controllers = {controller_name};
+        request->deactivate_controllers = {};
+        request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
+        request->activate_asap = true;
+        request->timeout = rclcpp::Duration::from_seconds(5.0);
+        
+        auto future = switch_client->async_send_request(request);
+        auto result = future.get();
+        return result->ok;
+}
+void TestMotor::left_pubTraj(const std::vector<double> angles){
+    trajectory_msgs::msg::JointTrajectory traj_msg;
+    ///traj_msg.header.stamp = this->now();
+    traj_msg.joint_names = {"left_arm_joint_1", "left_arm_joint_2", "left_arm_joint_3", "left_arm_joint_4", "left_arm_joint_5", "left_arm_joint_6"};
+    trajectory_msgs::msg::JointTrajectoryPoint point;
+    for(int i=0;i<6;i++){
+        std::string jname = "left_arm_joint_" + std::to_string(i+1);
+        dstMap_[jname]=angles.at(i)*M_PI/180.0;
+        double rad=curMap_[jname];
+        rad=dstMap_[jname];
+        point.positions.push_back(rad);
+        printf("D %s:  %.2f-->%.2f\n",jname.c_str(),curMap_[jname],rad);
+    }
+    traj_msg.points.push_back(point);
+    left_ctl_->publish(traj_msg);
+}
+void TestMotor::right_pubTraj(const std::vector<double> angles){
+    trajectory_msgs::msg::JointTrajectory traj_msg;
+    ///traj_msg.header.stamp = this->now();
+    traj_msg.joint_names = {"right_arm_joint_1", "right_arm_joint_2", "right_arm_joint_3", "right_arm_joint_4", "right_arm_joint_5", "right_arm_joint_6"};
+    trajectory_msgs::msg::JointTrajectoryPoint point;
+    for(int i=0;i<6;i++){
+        std::string jname = "right_arm_joint_" + std::to_string(i+1);
+        dstMap_[jname]=angles.at(i)*M_PI/180.0;
+        double rad=curMap_[jname];
+        rad=dstMap_[jname];
+        point.positions.push_back(rad);
+        printf("D %s:  %.2f-->%.2f\n",jname.c_str(),curMap_[jname],rad);
+    }
+    traj_msg.points.push_back(point);
+    right_ctl_->publish(traj_msg);
+}
+// 状态机主循环
+void TestMotor::ControlLoop() {
+    //Gpio_publish_joint_trajectory();
+   ////all_motor();
+}
+void TestMotor::left_clear(double clear)
+{
+    control_msgs::msg::DynamicInterfaceGroupValues gpioMsg_;
+    //std::cout<<"fault:";
+    for(int i=0;i<6;i++){
+        std::string tempInterfaceGroup = "left_arm_joint_" + std::to_string(i+1);
+        gpioMsg_.interface_groups.push_back(tempInterfaceGroup);
+        control_msgs::msg::InterfaceValue tempValue;
+        //reset
+        tempValue.interface_names = {"fault"};
+        tempValue.values = {clear};
+        std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
+        gpioMsg_.interface_values.push_back(tempValue);
+    }
+    std::cout<<std::endl;
+    left_gpio_->publish(gpioMsg_); 
+    usleep(50000);
+}
+void TestMotor::left_enable(double enable)
+{
+    control_msgs::msg::DynamicInterfaceGroupValues gpioMsg_;
+    ////std::cout<<"enable:";
+    for(int i=0;i<6;i++){
+        std::string tempInterfaceGroup = "left_arm_joint_" + std::to_string(i+1);
+        gpioMsg_.interface_groups.push_back(tempInterfaceGroup);
+        control_msgs::msg::InterfaceValue tempValue;
+        tempValue.interface_names = {"enable"};
+        tempValue.values = {enable};
+        gpioMsg_.interface_values.push_back(tempValue);
+    }
+    left_gpio_->publish(gpioMsg_); 
+    usleep(1000000);
+}
+
+void TestMotor::right_clear(double clear)
+{
+    control_msgs::msg::DynamicInterfaceGroupValues gpioMsg_;
+    //std::cout<<"fault:";
+    for(int i=0;i<6;i++){
+        std::string tempInterfaceGroup = "right_arm_joint_" + std::to_string(i+1);
+        gpioMsg_.interface_groups.push_back(tempInterfaceGroup);
+        control_msgs::msg::InterfaceValue tempValue;
+        //reset
+        tempValue.interface_names = {"fault"};
+        tempValue.values = {clear};
+        std::cout<<tempInterfaceGroup<<":"<<tempValue.values[0]<<",";
+        gpioMsg_.interface_values.push_back(tempValue);
+    }
+    std::cout<<std::endl;
+    right_gpio_->publish(gpioMsg_); 
+    usleep(50000);
+}
+void TestMotor::right_enable(double enable)
+{
+    control_msgs::msg::DynamicInterfaceGroupValues gpioMsg_;
+    ////std::cout<<"enable:";
+    for(int i=0;i<6;i++){
+        std::string tempInterfaceGroup = "right_arm_joint_" + std::to_string(i+1);
+        gpioMsg_.interface_groups.push_back(tempInterfaceGroup);
+        control_msgs::msg::InterfaceValue tempValue;
+        tempValue.interface_names = {"enable"};
+        tempValue.values = {enable};
+        gpioMsg_.interface_values.push_back(tempValue);
+    }
+    right_gpio_->publish(gpioMsg_); 
+    usleep(1000000);
+}
+void TestMotor::setJointValueTarget(std::vector<double> angles){
+    return;
+     if(client_->wait_for_action_server(std::chrono::seconds(5))){
+        auto goal=control_msgs::action::FollowJointTrajectory::Goal();
+        goal.trajectory.joint_names={"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7","joint_8","joint_9","joint_10","joint_11","joint_12"};
+        trajectory_msgs::msg::JointTrajectoryPoint point;
+        // for(int i=0;i<12;i++){
+        //     std::string joint="joint_"+std::to_string(i+1);
+        //     double rad=angles.at(i)*M_PI/180.0;
+        //     point.positions.push_back(rad);
+        //     printf("D %s:  %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
+        // }
+        double delta=0.01;
+        for(int i=0;i<12;i++){
+            std::string jname = "joint_" + std::to_string(i+1);
+            dstMap_[jname]=angles.at(i)*M_PI/180.0;
+            double rad=0.0;
+            double diff=dstMap_[jname]-curMap_[jname];
+            if(diff<-0.03)
+                rad= curMap_[jname]-delta;
+            else if(diff>0.03)
+                rad= curMap_[jname]+delta;
+            point.positions.push_back(rad);
+            printf("D %s:  %.2f/%.2f\n",jname.c_str(),curMap_[jname],point.positions[i]);
+        }
+
+
+
+        goal.trajectory.points.push_back(point);
+        auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
+        send_goal_option.goal_response_callback=[this](auto res){
+            auto goal_handle=res.get();
+            if(goal_handle){
+                printf("goal has be accept!!!\n");
+            }
+        };
+        send_goal_option.feedback_callback=[this](auto,auto feedback){
+            for(int i=0;i<12;i++){
+                printf("%s,%.3f/%.3f\n",feedback->joint_names[i].c_str(),feedback->desired.positions[i],feedback->actual.positions[i]);
+            }
+        };
+        send_goal_option.result_callback=[this](auto ret){
+            if(ret.code==rclcpp_action::ResultCode::SUCCEEDED)
+                printf("action ret succeed\n");
+            else if(ret.code==rclcpp_action::ResultCode::ABORTED)
+                printf("action ret aborted\n");
+        };
+        //client_->async_send_goal(goal,send_goal_option);
+        client_->async_send_goal(goal,send_goal_option);
+    }else{
+        printf("wait action server error\n");
+    }
+}
+void TestMotor::setJointValueTarget(double angle){
+    return;
+     if(client_->wait_for_action_server(std::chrono::seconds(5))){
+        auto goal=control_msgs::action::FollowJointTrajectory::Goal();
+        goal.trajectory.joint_names={"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7","joint_8","joint_9","joint_10","joint_11","joint_12"};
+        trajectory_msgs::msg::JointTrajectoryPoint point;
+        for(int i=0;i<12;i++){
+            std::string joint="joint_"+std::to_string(i+1);
+            point.positions.push_back(angle);
+            printf("S %s:  %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
+        }
+        goal.trajectory.points.push_back(point);
+        auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
+        send_goal_option.goal_response_callback=[this](auto res){
+            auto goal_handle=res.get();
+            if(goal_handle){
+                printf("goal has be accept!!!\n");
+            }
+        };
+        send_goal_option.feedback_callback=[this](auto,auto feedback){
+            for(int i=0;i<12;i++){
+                printf("%s,%.3f/%.3f\n",feedback->joint_names[i].c_str(),feedback->desired.positions[i],feedback->actual.positions[i]);
+            }
+        };
+        send_goal_option.result_callback=[this](auto ret){
+            if(ret.code==rclcpp_action::ResultCode::SUCCEEDED)
+                printf("action ret succeed\n");
+            else if(ret.code==rclcpp_action::ResultCode::ABORTED)
+                printf("action ret aborted\n");
+        };
+        client_->async_send_goal(goal,send_goal_option);
+    }else{
+        printf("wait action server error\n");
+    }
+}
+void TestMotor::motor_action(int id,double delta)
+{
+    return;
+    //std::cout<<"motor_action:"<<id<<","<<delta<<std::endl;
+    if(client_->wait_for_action_server(std::chrono::seconds(5))){
+        auto goal=control_msgs::action::FollowJointTrajectory::Goal();
+        goal.trajectory.joint_names={"joint_1","joint_2","joint_3","joint_4","joint_5","joint_6","joint_7","joint_8","joint_9","joint_10","joint_11","joint_12"};
+        trajectory_msgs::msg::JointTrajectoryPoint point;
+        for(int i=0;i<12;i++){
+            std::string joint="joint_"+std::to_string(i+1);
+            //if(id==(i+1))
+            if(i<12)
+                point.positions.push_back(curMap_[joint]+delta);
+            else
+                point.positions.push_back(curMap_[joint]);
+            printf("%s:  %.2f/%.2f\n",joint.c_str(),curMap_[joint],point.positions[i]);
+        }
+        goal.trajectory.points.push_back(point);
+        auto send_goal_option=rclcpp_action::Client<control_msgs::action::FollowJointTrajectory>::SendGoalOptions();
+        send_goal_option.goal_response_callback=[this](auto res){
+            auto goal_handle=res.get();
+            if(goal_handle){
+                printf("goal has be accept!!!\n");
+            }
+        };
+        send_goal_option.feedback_callback=[this](auto,auto feedback){
+            for(int i=0;i<12;i++){
+                printf("%s,%.3f/%.3f\n",feedback->joint_names[i].c_str(),feedback->desired.positions[i],feedback->actual.positions[i]);
+            }
+        };
+        send_goal_option.result_callback=[this](auto ret){
+            if(ret.code==rclcpp_action::ResultCode::SUCCEEDED)
+                printf("action ret succeed\n");
+            else if(ret.code==rclcpp_action::ResultCode::ABORTED)
+                printf("action ret aborted\n");
+        };
+        client_->async_send_goal(goal,send_goal_option);
+    }else{
+        printf("wait action server error\n");
+    }
+}
+
+int main(int argc,char* argv[]){
+    rclcpp::init(argc,argv);
+    auto node=std::make_shared<TestMotor>();
+    auto spin_thread=std::thread([=](){
+        rclcpp::spin(node);
+    });
+    std::cout<<"等待电机驱动启动...\n";
+    while(rclcpp::ok()&&!node->motor_drv_on){
+        sleep(1);
+    }
+    std::cout<<"电机驱动启动完毕!\n等待电机进入OP...\n";
+    //std::cout<<""<<std::endl;
+    while(rclcpp::ok()&&!node->all_motor_op){
+        sleep(1);
+    }
+    std::cout<<"电机进入OP!\n切换控制器...\n";
+    ///bool ret=node->activateController("trajectory_controller");
+    //if(ret)
+    {
+    
+        std::vector<double> angles1={0, 0, 0, 0, 0, 0};
+        std::vector<double> angles2={90,0, 40, 0, 0, 0};
+#if 0
+        node->is_reach=true;
+        static int loop_cnt=0;
+        while(rclcpp::ok()){
+            std::cout<<"开始执行角度1.05...\n";
+            loop_cnt+=1;
+            if(loop_cnt%2==0){
+                angles1={-40, -40, -40,-40,-40,-40};
+                angles2={-40,-40,-40, -40,-40,-40};
+            }else{
+                angles1={40, 40, 40,40,40,40};
+                angles2={40,40,40, 40,40,40};
+            }
+            angles1={10, 20, 30,40,50,60};
+            angles2={40,40,40, 40,40,40};
+            std::cout<<"开始动作...\n";
+            ///node->left_pubTraj(angles1);
+            sleep(2);
+            node->right_pubTraj(angles1);
+            sleep(10);
+            std::cout<<"开始执行复位...\n";
+            angles1={0, 0, 0, 0, 0, 0};
+            angles2={0, 0, 0,0, 0, 0};
+            ///node->left_pubTraj(angles1);
+            sleep(2);
+            ////node->right_pubTraj(angles2);
+            sleep(10);
+        }
+#endif
+#if 1
+        sleep(1);
+        node->activateController("left_arm_controller");
+        std::cout<<"左臂测试...\n";
+        ///node->left_clear(1);
+        node->left_enable(1);
+        angles1={-40, -40, -40,0,-40,0};
+        node->left_pubTraj(angles1);
+        sleep(4);
+        std::cout<<"左臂回位...\n";
+        angles1={0, 0, 0, 0, 0, 0};
+        node->left_pubTraj(angles1);
+        sleep(4);
+        node->left_enable(0);
+        std::cout<<"左臂测试完毕!\n";
+#endif
+        sleep(2);
+        node->activateController("right_arm_controller");
+        std::cout<<"左臂测试...\n";
+        ///node->left_clear(1);
+        angles1={-40, -40, -40,0,-40,0};
+        node->right_pubTraj(angles1);
+        node->right_enable(1);
+        sleep(4);
+        std::cout<<"左臂回位...\n";
+        angles1={0, 0, 0, 0, 0, 0};
+        node->right_pubTraj(angles1);
+        sleep(4);
+        node->right_enable(0);
+        std::cout<<"左臂测试完毕!\n";
+        sleep(2);
+        std::cout<<"双臂同时测试...\n";
+        node->left_enable(1);
+        node->right_enable(1);
+        angles1={40, 40, 40,0,40,0};
+        node->left_pubTraj(angles1);
+        node->right_pubTraj(angles1);
+        sleep(4);
+        std::cout<<"双臂回位...\n";
+        angles1={0, 0, 0, 0, 0, 0};
+        node->left_pubTraj(angles1);
+        node->right_pubTraj(angles1);
+        sleep(4);
+        node->left_enable(0);
+        node->right_enable(0);
+        std::cout<<"双臂测试完毕!\n";
+    }
+    ///usleep(10000000);
+    ///node->all_motor();
+    rclcpp::shutdown();
+    spin_thread.join();
+    return 0;
+}

ecrt_dev_dual/src/ethercat_driver.cpp

@@ -0,0 +1,796 @@
+#include <sys/mman.h>      // 用于 mlockall
+#include <pthread.h>       // 用于线程优先级设置
+#include <sched.h>         // 用于调度策略
+#include <cstring>         // 用于 strerror
+#include <unistd.h>        // 用于 getpid
+#include <sys/resource.h> 
+#include "ethercat_driver/ethercat_driver.hpp"
+#include <tinyxml2.h>
+#include <string>
+#include <regex>
+#include"ecrt.h"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+#include <chrono>
+#include<sstream>
+static std::stringstream logStream; 
+namespace ethercat_driver
+{
+CallbackReturn EthercatDriver::on_init(const hardware_interface::HardwareInfo & info)
+{
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
+  }
+
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  int cpu_core=7;
+  CPU_SET(cpu_core, &cpuset);
+  if (pthread_setaffinity_np(pthread_self(), sizeof(cpu_set_t), &cpuset) != 0) {
+        RCLCPP_ERROR(rclcpp::get_logger("hehe"), "Failed to set CPU affinity to core %d!", cpu_core);
+        return CallbackReturn::ERROR;
+  }
+
+  struct sched_param param;
+  param.sched_priority = 90;
+
+  if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &param) != 0) {
+      RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"),
+                  "Failed to set real-time priority: %s", strerror(errno));
+      return CallbackReturn::ERROR;
+  }
+
+  if (mlockall(MCL_CURRENT | MCL_FUTURE) != 0) {
+      RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"),
+                  "Failed to lock memory: %s", strerror(errno));
+  }
+  const std::lock_guard<std::mutex> lock(ec_mutex_);
+  activated_.store(false,std::memory_order_relaxed);
+  for(int i=0;i<NUM_SLAVES;i++){
+    set_motor_enable(i+1,false);
+  }
+  hw_joint_states_.resize(info_.joints.size());
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    hw_joint_states_[j].resize(
+      info_.joints[j].state_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_sensor_states_.resize(info_.sensors.size());
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    hw_sensor_states_[s].resize(
+      info_.sensors[s].state_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_gpio_states_.resize(info_.gpios.size());
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    hw_gpio_states_[g].resize(
+      info_.gpios[g].state_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_cmd_position_.resize(info_.joints.size());
+#if 1
+  hw_joint_commands_.resize(info_.joints.size());
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    hw_joint_commands_[j].resize(
+      info_.joints[j].command_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+#endif
+  hw_sensor_commands_.resize(info_.sensors.size());
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    hw_sensor_commands_[s].resize(
+      info_.sensors[s].command_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_gpio_commands_.resize(info_.gpios.size());
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    hw_gpio_commands_[g].resize(
+      info_.gpios[g].command_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints.size:%ld,%ld,%ld",info_.joints.size(),info_.joints[0].state_interfaces.size(),info_.joints[1].state_interfaces.size());
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints");
+    // check all joints for EC modules and load into ec_modules_
+    auto module_params = getEcModuleParam(info_.original_xml, info_.joints[j].name, "joint");
+    ec_module_parameters_.insert(
+      ec_module_parameters_.end(), module_params.begin(), module_params.end());
+    for (auto i = 0ul; i < module_params.size(); i++) {
+      for (auto k = 0ul; k < info_.joints[j].state_interfaces.size(); k++) {
+        module_params[i]["state_interface/" +
+          info_.joints[j].state_interfaces[k].name] = std::to_string(k);
+      }
+      for (auto k = 0ul; k < info_.joints[j].command_interfaces.size(); k++) {
+        module_params[i]["command_interface/" +
+          info_.joints[j].command_interfaces[k].name] = std::to_string(k);
+      }  
+    }
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios.size:%ld",info_.gpios.size());
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios");
+    // check all gpios for EC modules and load into ec_modules_
+    auto module_params = getEcModuleParam(info_.original_xml, info_.gpios[g].name, "gpio");
+    ec_module_parameters_.insert(
+      ec_module_parameters_.end(), module_params.begin(), module_params.end());
+    for (auto i = 0ul; i < module_params.size(); i++) {
+      for (auto k = 0ul; k < info_.gpios[g].state_interfaces.size(); k++) {
+        module_params[i]["state_interface/" +
+          info_.gpios[g].state_interfaces[k].name] = std::to_string(k);
+      }
+      for (auto k = 0ul; k < info_.gpios[g].command_interfaces.size(); k++) {
+        module_params[i]["command_interface/" +
+          info_.gpios[g].command_interfaces[k].name] = std::to_string(k);
+      }
+    }
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors.size:%ld",info_.joints.size());
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors");
+    // check all sensors for EC modules and load into ec_modules_
+    auto module_params = getEcModuleParam(info_.original_xml, info_.sensors[s].name, "sensor");
+    ec_module_parameters_.insert(
+      ec_module_parameters_.end(), module_params.begin(), module_params.end());
+    for (auto i = 0ul; i < module_params.size(); i++) {
+      for (auto k = 0ul; k < info_.sensors[s].state_interfaces.size(); k++) {
+        module_params[i]["state_interface/" +
+          info_.sensors[s].state_interfaces[k].name] = std::to_string(k);
+      }
+      for (auto k = 0ul; k < info_.sensors[s].command_interfaces.size(); k++) {
+        module_params[i]["command_interface/" +
+          info_.sensors[s].command_interfaces[k].name] = std::to_string(k);
+      }
+      
+    }
+  }
+  //
+  master = ecrt_request_master(0);
+  if (!master) { 
+      perror("ecrt_request_master"); 
+      //g_started.store(false); return false; 
+  }
+  domain1 = ecrt_master_create_domain(master);
+  if (!domain1){ 
+      perror("ecrt_master_create_domain"); 
+      //g_started.store(false); return false; 
+  }
+  const uint32_t shift_step = PERIOD_NS / 12;
+  for (int i = 0; i < NUM_SLAVES; i++) {
+      std::cout << "Configuring slave " << i+1 << "..." << std::endl;
+      {
+          sc[i] = ecrt_master_slave_config(master, 0, i+1, ZER_VID_PID);
+          if (!sc[i]) 
+          { 
+              std::cout << "Failed slave cfg at position " << i << std::endl;
+          }
+          if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs)) 
+          {
+              std::cout << "Failed PDO config " << i << std::endl;
+          }
+      }
+      ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 20000, 0, 0);
+      //ecrt_slave_config_dc(sc[i], 0x0700, PERIOD_NS, 5000, PERIOD_NS/2, 0);
+  }
+  ///ecrt_master_set_send_interval(master, PERIOD_NS);
+  ecrt_domain_reg_pdo_entry_list(domain1,zer_domain1_regs);
+  if (ecrt_master_activate(master)) { 
+      perror("ecrt_master_activate");
+      return CallbackReturn::ERROR;
+  }
+  domain1_pd = ecrt_domain_data(domain1);
+  if (!domain1_pd) 
+  { 
+      fprintf(stderr,"domain1_pd null\n"); 
+      return CallbackReturn::ERROR;
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Got %li modules", info_.joints.size());
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn EthercatDriver::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  RCLCPP_INFO(rclcpp::get_logger("hehe"),"on configure......");
+  
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface>
+EthercatDriver::export_state_interfaces()
+{
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_state_interfaces:%ld,%ld,%ld",info_.joints.size(),info_.sensors.size(),info_.gpios.size());
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  // export joint state interface
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) {
+      ////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"export:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].state_interfaces[i].name.c_str());
+      state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+          info_.joints[j].name,
+          info_.joints[j].state_interfaces[i].name,
+          &hw_joint_states_[j][i]));
+    }
+  }
+  // export sensor state interface
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    for (uint i = 0; i < info_.sensors[s].state_interfaces.size(); i++) {
+      state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+          info_.sensors[s].name,
+          info_.sensors[s].state_interfaces[i].name,
+          &hw_sensor_states_[s][i]));
+    }
+  }
+  // export gpio state interface
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    for (uint i = 0; i < info_.gpios[g].state_interfaces.size(); i++) {
+      state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+          info_.gpios[g].name,
+          info_.gpios[g].state_interfaces[i].name,
+          &hw_gpio_states_[g][i]));
+    }
+  }
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface>
+EthercatDriver::export_command_interfaces()
+{
+RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "#######export_command_interfaces:%ld",info_.joints.size());
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+  // export joint command interface
+  ///std::vector<double> test;
+  #if 0
+  for (uint i = 0; i < info_.joints.size(); i++) {
+      command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+          info_.joints[i].name,
+          hardware_interface::HW_IF_POSITION,
+          &hw_cmd_position_[i]));
+  }
+  #endif
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    ////RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"size:%d\n",info_.joints[j].command_interfaces.size());
+    for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) {
+      ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"joints:%s,%s",info_.joints[j].name.c_str(),info_.joints[j].command_interfaces[i].name.c_str());
+      command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+          info_.joints[j].name,
+          info_.joints[j].command_interfaces[i].name,
+          &hw_joint_commands_[j][i]));
+    }
+  }
+  // export sensor command interface
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    for (uint i = 0; i < info_.sensors[s].command_interfaces.size(); i++) {
+      command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+          info_.sensors[s].name,
+          info_.sensors[s].command_interfaces[i].name,
+          &hw_sensor_commands_[s][i]));
+    }
+  }
+  // export gpio command interface
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    for (uint i = 0; i < info_.gpios[g].command_interfaces.size(); i++) {
+      ///printf("gpios:%s,%s\n",info_.gpios[g].name,info_.gpios[g].command_interfaces[i].name);
+      command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+          info_.gpios[g].name,
+          info_.gpios[g].command_interfaces[i].name,
+          &hw_gpio_commands_[g][i]));
+    }
+  }
+  return command_interfaces;
+}
+
+CallbackReturn EthercatDriver::on_activate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  const std::lock_guard<std::mutex> lock(ec_mutex_);
+  if (activated_.load(std::memory_order_relaxed)) {
+    RCLCPP_FATAL(rclcpp::get_logger("EthercatDriver"), "Double on_activate()");
+    return CallbackReturn::ERROR;
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "on activate ...please wait...");
+  if (info_.hardware_parameters.find("control_frequency") == info_.hardware_parameters.end()) {
+    control_frequency_ = 100;
+  } else {
+    control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]);
+  }
+  control_frequency_=FREQUENCY;
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"control_frequency_:%d\n",control_frequency_);
+  if (control_frequency_ < 0) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!");
+    return CallbackReturn::ERROR;
+  }
+  clock_gettime(CLOCK_TO_USE, &wakeupTime); 
+  for(int i=0;i<NUM_SLAVES;i++){
+        RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"slave:%d,ctl offset:%d,sta offset:%d\n",i,zer_offsets[i].ctrl_word,zer_offsets[i].status_word);
+    }
+  
+
+  for(int i=0;i<info_.joints.size();i++){
+      for(int j=0;j<info_.joints[i].command_interfaces.size();j++)
+          hw_joint_commands_[i][j]=0.0;
+  }
+  for(int i=0;i<NUM_SLAVES;i++)
+      RCLCPP_INFO(rclcpp::get_logger("hehe"),"hw_joint_commands_[i][2]:%.1f",hw_joint_commands_[i][2]);
+  activated_.store(true,std::memory_order_relaxed);
+  all_motor_op=false;
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn EthercatDriver::on_deactivate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  const std::lock_guard<std::mutex> lock(ec_mutex_);
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "SSSSSSSSSSSSSSSSSSSSSSSS...");
+  for(int i=0;i<NUM_SLAVES;i++){
+      ///RCLCPP_INFO(rclcpp::get_logger("hehe"),"close state:%.1f",hw_joint_states_[i][0]);
+      EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
+      // CSV 安全：速度清零
+      EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
+      // CSP 安全：目标位置=当前位置 -> 立即“冻结”
+      int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
+      EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
+      // 可选：扭矩模式也清零（最好维持急停状态，待修改）
+      EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
+      EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
+  }
+  activated_.store(false,std::memory_order_relaxed);
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "System successfully stopped!");
+  return CallbackReturn::SUCCESS;
+}
+
+
+
+hardware_interface::return_type EthercatDriver::read(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & /*period*/)
+{
+  // try to lock so we can avoid blocking the read/write loop on the lock.
+  ////const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
+  //printf("read data...\n");
+  //if (lock.owns_lock() && activated_) {
+  if (activated_.load(std::memory_order_relaxed)){
+    auto start=std::chrono::high_resolution_clock::now();
+    readData();
+    auto end=std::chrono::high_resolution_clock::now();
+    auto diff=end-start;
+    if(diff.count()>120000)
+      RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"rdiff:%ld",diff.count());
+  }
+  return hardware_interface::return_type::OK;
+}
+void EthercatDriver::check_domain1_state(void)
+{
+    ec_domain_state_t ds;
+
+    ecrt_domain_state(domain1, &ds);
+
+    if (ds.working_counter != domain1_state.working_counter)
+        //printf("Domain1: WC %u.\n", ds.working_counter); //当数字变化，表示丢包
+    if (ds.wc_state != domain1_state.wc_state)
+        //printf("Domain1: State %u.\n", ds.wc_state);
+
+    domain1_state = ds;
+}
+void EthercatDriver::check_master_state(void)
+{
+    ec_master_state_t ms;
+
+    ecrt_master_state(master, &ms);
+
+    if (ms.slaves_responding != master_state.slaves_responding)
+        //printf("%u slave(s).\n", ms.slaves_responding);
+    if (ms.al_states != master_state.al_states)
+        //printf("AL states: 0x%02X.\n", ms.al_states);
+    if (ms.link_up != master_state.link_up)
+        //printf("Link is %s.\n", ms.link_up ? "up" : "down");
+
+    master_state = ms;
+}
+bool EthercatDriver::check_slave_config_states(void)
+{
+    ec_slave_config_state_t s;
+    bool all_op=true;
+    for (int i = 0; i < NUM_SLAVES; ++i) {
+        errStaArr[i]=ERR_NONE;
+        if (!sc[i]) continue;
+        ecrt_slave_config_state(sc[i], &s);
+        if ((s.al_state != sc_state[i].al_state)||(s.online != sc_state[i].online)||(s.operational != sc_state[i].operational)){
+            RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"[S%02d] %s,State 0x%02X,OP %s", i, s.online ? "online" : "offline",s.al_state,s.operational ? "OK" : "ERR");
+        }
+        sc_state[i] = s;
+        if(!s.operational){
+          op_states[i]=0;
+          all_op=false;
+        }else{
+          op_states[i]=1;
+        }
+    }
+    return all_op;
+}
+struct timespec EthercatDriver::timespec_add(struct timespec time1, struct timespec time2)
+{
+    struct timespec result;
+
+    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
+        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
+        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
+    } else {
+        result.tv_sec = time1.tv_sec + time2.tv_sec;
+        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
+    }
+
+    return result;
+}
+void EthercatDriver::readData(){
+  static int print_cnt=0;
+  print_cnt+=1;
+  
+  #if 1
+    wakeupTime = timespec_add(wakeupTime, cycletime);
+    ////clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL); 
+    ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));  
+  #endif
+    ecrt_master_receive(master); 
+    ecrt_domain_process(domain1); 
+    check_domain1_state(); 
+    if (counter) {
+        counter--;
+    } else { // do this at 1 Hz
+      counter = FREQUENCY;
+      check_master_state();
+      if(all_motor_op){
+          if(!has_clear_all){
+              has_clear_all=true;
+              for (int i = 0; i < NUM_SLAVES; ++i) {
+                reach_target[i]=1;
+                uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
+                if(err>0){
+                    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],first code 0x%04x",i,err);
+                    clear_cmd[i]=1;//
+                    has_clear_all=false;
+                }else{
+                  clear_cmd[i]=0;
+                }
+              }
+          }
+      }else{
+        all_motor_op=check_slave_config_states();        //检查从站状态
+        ////return;
+      }
+    }
+    if (!inited) {
+            for (int i = 0; i < NUM_SLAVES; ++i) {
+                command[i] = 0x004F;
+                status[i] = 0x000F;
+                last_status[i] = status[i];  
+            }
+            inited = 1;
+    }
+
+  for (int i = 0; i < NUM_SLAVES; ++i) 
+    {
+        uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
+        int32_t  pos = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
+        int32_t  vel = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
+        int16_t  tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
+        int8_t   md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
+        uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
+        cur_pos[i]=pos;
+        double up_pos=cur_pos[i]-pos_offsets[i];
+        //double angle=std::round(up_pos*count_to_rad * 1000.0) / 1000.0;
+        if(i==2||i==8)
+          up_pos=-up_pos;
+        if(op_states[i]==1){
+            hw_joint_states_[i][0]=up_pos*count_to_rad;//angle;
+            hw_joint_states_[i][1]=vel*count_to_speed;//std::round(vel*count_to_speed*1000.0)/1000.0;
+            double amp=tv*rated_currents[i];
+            double effort=0.014*0.6*amp;
+            hw_joint_states_[i][2]=effort;//std::round(amp*10.0)/10.0;
+        }else{
+            hw_joint_states_[i][0]=9999;
+            hw_joint_states_[i][1]=9999;
+            hw_joint_states_[i][2]=9999;
+        }
+        
+        hw_joint_states_[i][3]=err;
+        hw_joint_states_[i][4]=enable_arr[i];
+        //hw_joint_states_[i][2]=err;
+
+        status[i] = sw;                                 //侦测子站状态字变化
+        if (status[i] != last_status[i]) {
+            last_status[i] = status[i];
+        }
+    }
+}
+void EthercatDriver::writeData(){
+  static int print_cnt=0;
+  print_cnt+=1;
+  #if 0
+  if(print_cnt%1000==0){
+    logStream.str("");
+    for(int i=0;i<info_.joints.size();i++){
+      logStream<<info_.joints[i].name.c_str()<<":";
+      for(int j=0;j<info_.joints[i].command_interfaces.size();j++)
+        //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"%.2f,",hw_joint_commands_[i][j]);
+        logStream<<hw_joint_commands_[i][j]<<",";
+      logStream<<"\n";
+      //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"[%02d]%s: %s",i,info_.joints[i].name.c_str(),logStream.str().c_str());
+    }
+    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"%s",logStream.str().c_str());
+  }
+  #endif
+  for (int i = 0; i < NUM_SLAVES; ++i) 
+    {
+        dst_rads[i]=getValidAngle(i);
+#if 1
+        if ((status[i]&0x006f)==0x0008){
+          uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
+          if(err>0){
+              double fault=hw_joint_commands_[i][0];
+              if(print_cnt%1000==1){
+                ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],EE:0x%04x",i,err);
+              }
+              if(fault==1.0||clear_cmd[i]==1){
+                if(print_cnt%1000==0)
+                  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],%.1f/%d,fault code 0x%04x",i,fault,clear_cmd[i],err);
+                  if(errStaArr[i]==ERR_NONE){
+                      errStaArr[i]=ERR_ACK;
+                  }
+              }
+          }
+        }
+        //clear fault
+        if(errStaArr[i]==ERR_ACK){
+            EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0000);
+            errStaArr[i]=ERR_ACK_WAIT;
+            ///RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_ACK %d",i,err,(sw&0x006f)==0x0008);
+        }else if(errStaArr[i]==ERR_ACK_WAIT){
+           //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_ACK_WAIT %d",i,err,(sw&0x006f)==0x0008);
+            if ((status[i]&0x006f)==0x0008){
+                
+            }
+            errStaArr[i]=ERR_CLEAR;
+        }else if(errStaArr[i]==ERR_CLEAR){
+            EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080);
+            errStaArr[i]=ERR_CLEAR_WAIT;
+            //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],%d,ERR_CLEAR",i,clear_cmd[i]);
+        }else if(errStaArr[i]==ERR_CLEAR_WAIT){
+            //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_CLEAR_WAIT %d",i,err,(sw&0x006f)==0x0008);
+            errStaArr[i]=ERR_FIN;
+        }else if(errStaArr[i]==ERR_FIN){
+            EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
+            //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_FIN %d",i,err,(sw&0x006f)==0x0008);
+            errStaArr[i]=ERR_FIN_WAIT;
+        }else if(errStaArr[i]==ERR_FIN_WAIT){
+            //RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "[%d],0x%04x,ERR_FIN_WAIT %d",i,err,(sw&0x006f)==0x0008);
+            if((status[i]&0x006f)==0x0008){
+                errStaArr[i]=ERR_ACK;
+            }else{
+                errStaArr[i]=ERR_NONE;
+                ///continue;
+            }
+        }
+ #endif
+        if(errStaArr[i]!=ERR_NONE||op_states[i]!=1)
+          continue;
+        enable_arr[i]=hw_joint_commands_[i][1];
+        //if(print_cnt%500==0)
+        //  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"),"enable:%d,%.1f,%.2f",i,enable,hw_joint_commands_[i][2]);
+        if(enable_arr[i]!=1.0){
+          EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
+          EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
+          hw_joint_commands_[i][2]=0;
+          continue;
+        }
+      if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
+            EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);   // Ready to switch on
+            command[i] = 0x006F;
+            step_pos[i]=cur_pos[i];
+        } else if ((status[i] & command[i]) == 0x0021) {
+            EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);   // Switched on
+            command[i] = 0x006F;
+            int32_t  pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
+            EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
+            step_pos[i]=cur_pos[i];
+        } else if ((status[i] & command[i]) == 0x0023) {
+            EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f);   // Operation enabled
+            command[i] = 0x006F;
+            step_pos[i]=cur_pos[i];
+        } else if ((status[i] & command[i]) == 0x0027) {      
+            if(true)
+            {
+                //EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
+                switch (mode_cmd) {
+                    case CYCLIC_VELOCITY: // 9
+                        ////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
+                        break;
+                    case CYCLIC_POSITION: { // 8
+                        // ---- 绝对式目标生成，不再用 pv 做增量基准 ----
+                        // 每轴记住“上次下发的命令位置”
+                        static int32_t csp_cmd_pos[NUM_SLAVES];         // 上次下发到 0x607A 的值
+                        static uint8_t csp_inited[NUM_SLAVES] = {0};    // 是否已初始化
+                        // 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
+                        static int64_t vmax_acc[NUM_SLAVES] = {0};      // 单位：counts/s 累加器
+
+                        // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
+                        if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
+                            csp_cmd_pos[i] = cur_pos[i];       // 先对齐
+                            vmax_acc[i]     = 0;
+                            csp_inited[i]   = 1;
+                        }
+                        //double down_pos=hw_joint_commands_[i][2]*rad_to_count;
+                        //if(i==2||i==8){
+                        //    down_pos=-down_pos;
+                        //}
+                        //double target_pos=down_pos+pos_offsets[i]+262144.0;
+                        double target_pos=getValidCount_ze(i);
+                        if(print_cnt%1000==1){
+                          ////RCLCPP_INFO(rclcpp::get_logger("hehe"),"[%d] rad:%.1f,target_pos:%.1f,cur pos:%d",i,dst_rads[i],target_pos,cur_pos[i]);
+                        }
+                        EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, target_pos);//
+                        #if 0
+                          int32_t max_step=CSP_MAX_VEL_COUNTS_PER_S/FREQUENCY;
+                          const int32_t goal=target_pos;
+                          const int32_t err_cmd = goal -csp_cmd_pos[i];
+                          if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
+                              csp_cmd_pos[i] = goal;
+                          } else {
+                              // 限速迈步：命令轨迹只按时间往 goal 逼近
+                              if (err_cmd > 0)      csp_cmd_pos[i] += (err_cmd >  max_step) ?  max_step : err_cmd;
+                              else                  csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
+                          }
+                          ////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
+                        #else
+                          const int32_t err_cmd=target_pos-cur_pos[i];
+                          if (abs(err_cmd)<CSP_DEADBAND1) {
+                              step_pos[i] = target_pos;
+                              reach_target[i]=1;
+                          }else if(abs(err_cmd)<CSP_DEADBAND2){
+                              if(err_cmd>0)   step_pos[i]+=(err_cmd>CSP_SPEED1) ? CSP_SPEED1:err_cmd;
+                              else            step_pos[i]+=(err_cmd<-CSP_SPEED1) ? -CSP_SPEED1:err_cmd;
+                              // reach_target[i]=0;
+                              //if(err_cmd>0) step_pos[i]+=CSP_SPEED1;
+                              //else        step_pos[i]-=CSP_SPEED1;
+                          } else {
+                              if(err_cmd>0)   step_pos[i]+=(err_cmd>CSP_SPEED2) ? CSP_SPEED2:err_cmd;
+                              else            step_pos[i]+=(err_cmd<-CSP_SPEED2) ? -CSP_SPEED2:err_cmd;
+                              //if(err_cmd>0) step_pos[i]+=CSP_SPEED2;
+                              //else        step_pos[i]-=CSP_SPEED2;
+                          }
+                          ////EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, step_pos[i]);
+                        #endif 
+                        // 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
+                        // const int32_t follow_err = csp_cmd_pos[i] - pv;
+                        // const int32_t FE_LIMIT =  (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
+                        // if (std::abs(follow_err) > FE_LIMIT) {
+                        //     // 例如：冻结或减半 max_step，给驱动时间追上
+                        // }
+                        
+                        break;
+                    }
+                    case CYCLIC_TORQUE:   // 10
+                        ///////EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
+                        break;
+                    default:
+                        // 未知模式：默认速度模式安全置 0
+                        //EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
+                        break;
+                        
+                }
+            } else {
+                // run_enable=false：清零目标
+                EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
+                // CSV 安全：速度清零
+                EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
+                // CSP 安全：目标位置=当前位置 -> 立即“冻结”
+                
+                int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
+                EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
+                // 可选：扭矩模式也清零（最好维持急停状态，待修改）
+                EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
+                EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
+            }
+        }
+    }
+    //Final
+    if (sync_ref_counter) {
+        sync_ref_counter--;
+    } else {
+        sync_ref_counter = 1; // sync every 2 cycle
+        clock_gettime(CLOCK_TO_USE, &time);
+        ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟
+        ecrt_master_sync_slave_clocks(master);
+    }
+
+    ecrt_domain_queue(domain1);     //排队域数据
+    ecrt_master_send(master);   //发送ETHERCAT帧 
+}
+hardware_interface::return_type EthercatDriver::write(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & /*period*/)
+{
+  // try to lock so we can avoid blocking the read/write loop on the lock.
+ /// const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
+  if (activated_.load(std::memory_order_relaxed)){
+    auto start=std::chrono::high_resolution_clock::now();
+    writeData();
+    auto end=std::chrono::high_resolution_clock::now();
+    auto diff=end-start;
+    if(diff.count()>100000)
+      RCLCPP_INFO(rclcpp::get_logger("Ethercat"),"wdiff:%ld",diff.count());
+  }
+  return hardware_interface::return_type::OK;
+}
+
+std::vector<std::unordered_map<std::string, std::string>> EthercatDriver::getEcModuleParam(
+  std::string & urdf,
+  std::string component_name,
+  std::string component_type)
+{
+  // Check if everything OK with URDF string
+  if (urdf.empty()) {
+    throw std::runtime_error("empty URDF passed to robot");
+  }
+  tinyxml2::XMLDocument doc;
+  if (!doc.Parse(urdf.c_str()) && doc.Error()) {
+    throw std::runtime_error("invalid URDF passed in to robot parser");
+  }
+  if (doc.Error()) {
+    throw std::runtime_error("invalid URDF passed in to robot parser");
+  }
+
+  tinyxml2::XMLElement * robot_it = doc.RootElement();
+  if (std::string("robot").compare(robot_it->Name())) {
+    throw std::runtime_error("the robot tag is not root element in URDF");
+  }
+
+  const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement("ros2_control");
+  if (!ros2_control_it) {
+    throw std::runtime_error("no ros2_control tag");
+  }
+
+  std::vector<std::unordered_map<std::string, std::string>> module_params;
+  std::unordered_map<std::string, std::string> module_param;
+
+  while (ros2_control_it) {
+    const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(component_type.c_str());
+    while (ros2_control_child_it) {
+      if (!component_name.compare(ros2_control_child_it->Attribute("name"))) {
+        const auto * ec_module_it = ros2_control_child_it->FirstChildElement("ec_module");
+        while (ec_module_it) {
+          module_param.clear();
+          module_param["name"] = ec_module_it->Attribute("name");
+          const auto * plugin_it = ec_module_it->FirstChildElement("plugin");
+          if (NULL != plugin_it) {
+            module_param["plugin"] = plugin_it->GetText();
+          }
+          const auto * param_it = ec_module_it->FirstChildElement("param");
+          while (param_it) {
+            module_param[param_it->Attribute("name")] = param_it->GetText();
+            param_it = param_it->NextSiblingElement("param");
+          }
+          module_params.push_back(module_param);
+          ec_module_it = ec_module_it->NextSiblingElement("ec_module");
+        }
+      }
+      ros2_control_child_it = ros2_control_child_it->NextSiblingElement(component_type.c_str());
+    }
+    ros2_control_it = ros2_control_it->NextSiblingElement("ros2_control");
+  }
+
+  return module_params;
+}
+
+}  // namespace ethercat_driver
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  ethercat_driver::EthercatDriver, hardware_interface::SystemInterface)

ethercat_dev/readme.txt

@@ -0,0 +1,28 @@
+#！！！程序需要开发板安装RT Linux系统！！！
+
+#新建mt_ws文件夹，将src文件放在mt_ws下
+
+#加载环境
+sudo ethercatctl start
+source /opt/ros/humble/setup.bash
+
+#构建（在工作空间下）
+colcon build --symlink-install
+
+给可执行文件setcap
+cmake --build build/ethercat_control --target setcap_ethercat_node
+
+# 加载工作空间环境
+source ~/mt_ws/install/setup.bash
+#运行主程序
+ros2 run ethercat_control ethercat_node
+
+#新开终端运行测试指令
+ros2 topic pub --rate 10 /ethercat/set std_msgs/msg/String "data: '0 pos 0; 1 pos 0; 2 pos 0;3 pos 0'"
+第一个参数为电机Ethercat上的id
+第二个参数为模式 分为pos vel tor，对应位置、速度、扭矩模式
+第三个参数为位置/速度/扭矩设定值
+
+举例：	0 pos 0 为将0号电机运行至0位
+	1 pos 65535 为将1号电机运行至180度位置
+

ethercat_dev/src/ethercat_control/CMakeLists.txt

@@ -0,0 +1,70 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_control)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sensor_msgs REQUIRED)
+
+# IGH ecrt（如需要，设置自定义路径）
+find_library(ECRT_LIB
+  NAMES ethercat ecrt
+  PATHS /opt/etherlab/lib /usr/local/lib /usr/lib
+)
+if(NOT ECRT_LIB)
+  message(FATAL_ERROR "ecrt not found; set CMAKE_PREFIX_PATH or system ld path.")
+endif()
+
+include_directories(include)
+
+# 核心对象文件
+add_library(ethercat_core_obj OBJECT
+  src/motor_control.cpp
+  src/ethercat_core.cpp
+)
+
+# 静态/共享库（任选其一）
+add_library(ethercat_core STATIC $<TARGET_OBJECTS:ethercat_core_obj>)
+target_link_libraries(ethercat_core ${ECRT_LIB} pthread)
+
+# 节点
+add_executable(ethercat_node src/ethercat_node.cpp)
+target_link_libraries(ethercat_node ethercat_core)
+ament_target_dependencies(ethercat_node rclcpp std_msgs sensor_msgs rcl_interfaces)
+
+# add_executable(ethercat_csp_test src/ethercat_csp_test.cpp)
+# ament_target_dependencies(ethercat_csp_test rclcpp std_msgs)
+# install(TARGETS ethercat_csp_test DESTINATION lib/${PROJECT_NAME})
+
+
+install(TARGETS ethercat_node
+  DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY include/
+  DESTINATION include)
+
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}/
+)
+# 测试程序
+# add_executable(ethercat_topic_test src/ethercat_topic_test.cpp)
+# ament_target_dependencies(ethercat_topic_test rclcpp std_msgs)
+
+# install(TARGETS ethercat_topic_test
+#   DESTINATION lib/${PROJECT_NAME})
+
+
+
+# 创建一个手动触发的目标：为真实二进制设置 cap_sys_nice
+add_custom_target(setcap_ethercat_node
+  COMMAND ${CMAKE_COMMAND} -E echo "Setting cap_sys_nice on:"
+  COMMAND ${CMAKE_COMMAND} -E echo "$<TARGET_FILE:ethercat_node>"
+  COMMAND sudo setcap cap_sys_nice+ep $<TARGET_FILE:ethercat_node>
+  COMMAND getcap $<TARGET_FILE:ethercat_node>
+  DEPENDS ethercat_node
+  VERBATIM
+)
+
+ament_package()

ethercat_dev/src/ethercat_control/include/ethercat_control/cmd_interface.hpp

@@ -0,0 +1,5 @@
+#pragma once
+#include <thread>
+
+void start_rt_cyclic_thread();     // 启动实时线程跑 cyclic_task()
+void cli_command_loop();           // 在主线程调用：阻塞读命令

ethercat_dev/src/ethercat_control/include/ethercat_control/ds_config.hpp

@@ -0,0 +1,65 @@
+#pragma once
+#include "ecrt.h"
+#include "ethercat_control/motor_control.hpp" // 电机控制库 
+
+#define ZER_Device0        0,0    /*EtherCAT address on the bus 别名，位置*/
+  
+#define VID_PID          0x00202288,0x00000384   /*Vendor ID, product code	厂商ID和产品代码*/
+
+typedef struct {
+    unsigned int ctrl_word;                   // 0x6040:00  1
+    unsigned int target_position;             // 0x607A:00  1
+    unsigned int target_velocity;             // 0x60FF:00  1
+    unsigned int operation_mode;              // 0x6060:00  1
+    unsigned int status_word;                 // 0x6041:00  1
+    unsigned int position_actual_value;       // 0x6064:00  1
+    unsigned int velocity_actual_value;       // 0x606C:00  1
+    unsigned int error_code;                  // 0x603F:00  1
+    unsigned int modes_of_operation_display;  // 0x6061:00  1
+} zer_offset_t;
+
+static zer_offset_t zer_offsets[NUM_SLAVES];
+
+// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
+// 下列结构由IGH主站 cstruct 自动生成
+const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
+    {ZER_Device0, VID_PID, 0x6040, 0, &zer_offsets[0].ctrl_word},                   // 控制字
+    {ZER_Device0, VID_PID, 0x607A, 0, &zer_offsets[0].target_position},             // 目标位置
+    {ZER_Device0, VID_PID, 0x60FF, 0, &zer_offsets[0].target_velocity},             // 目标速度
+    {ZER_Device0, VID_PID, 0x6060, 0, &zer_offsets[0].operation_mode},              // 运行模式
+    {ZER_Device0, VID_PID, 0x6041, 0, &zer_offsets[0].status_word},                 // 状态字
+    {ZER_Device0, VID_PID, 0x6064, 0, &zer_offsets[0].position_actual_value},       // 实际位置
+    {ZER_Device0, VID_PID, 0x606C, 0, &zer_offsets[0].velocity_actual_value},       // 实际速度
+    {ZER_Device0, VID_PID, 0x603F, 0, &zer_offsets[0].error_code},                  // 错误代码
+    {ZER_Device0, VID_PID, 0x6061, 0, &zer_offsets[0].modes_of_operation_display},  // 模式显示
+    {} // 结束标记
+};
+
+static ec_pdo_entry_info_t zer_device_pdo_entries[] = {
+    /*RxPdo 0x1600*/
+    {0x6040, 0x00, 16}, /* control word */
+    {0x607a, 0x00, 32}, /* target position */
+    {0x60ff, 0x00, 32}, /* target velocity */
+    {0x6060, 0x00, 8}, /* modes of operation */
+    /*TxPdo 0x1A00*/
+    {0x6041, 0x00, 16}, /* status word */
+    {0x6064, 0x00, 32}, /* position actual value */
+    {0x606c, 0x00, 32}, /* velocity actual value */
+    {0x603f, 0x00, 16}, /* error code */
+    {0x6061, 0x00, 8}, /* modes of operation display */
+};
+
+static ec_pdo_info_t zer_device_pdos[] = {
+    //RxPdo
+    {0x1600, 4, zer_device_pdo_entries + 0 },
+    //TxPdo
+    {0x1A00, 5, zer_device_pdo_entries + 4 }
+};
+
+static ec_sync_info_t zer_device_syncs[] = {
+    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
+    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
+    {2, EC_DIR_OUTPUT, 1, zer_device_pdos + 0, EC_WD_ENABLE},
+    {3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE},
+    {0xff}
+};

ethercat_dev/src/ethercat_control/include/ethercat_control/et_config.hpp

@@ -0,0 +1,80 @@
+#pragma once
+#include "ecrt.h"
+#include "ethercat_control/motor_control.hpp" // 电机控制库 
+
+#define ET_Device0        0,0    /*EtherCAT address on the bus 别名，位置*/
+  
+#define ET_VID_PID          0x00000009,0x26483062   /*Vendor ID, product code	厂商ID和产品代码*/
+
+typedef struct {
+    unsigned int ctrl_word;                   // 0x6040:00
+    unsigned int target_position;             // 0x607A:00
+    unsigned int target_velocity;             // 0x60FF:00 
+    unsigned int target_torque;               // 0x6071:00 
+    unsigned int max_torque;                  // 0x6072:00 
+    unsigned int operation_mode;              // 0x6060:00
+    unsigned int reserved1;                   // 0x5FFE:00 
+    unsigned int status_word;                 // 0x6041:00
+    unsigned int position_actual_value;       // 0x6064:00
+    unsigned int velocity_actual_value;       // 0x606C:00 
+    unsigned int torque_actual_value;         // 0x6077:00 
+    unsigned int error_code;                  // 0x603F:00
+    // unsigned int modes_of_operation_display;  // 0x6061:00
+    // unsigned int reserved2;                   // 0x5FFE:00 
+} et_offset_t;
+
+static et_offset_t et_offsets[NUM_SLAVES];
+
+// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
+// 下列结构由IGH主站 cstruct 自动生成
+const static ec_pdo_entry_reg_t et_domain1_regs[] = {
+    {ET_Device0, ET_VID_PID, 0x6040, 0, &et_offsets[0].ctrl_word},                   // 控制字
+    {ET_Device0, ET_VID_PID, 0x607A, 0, &et_offsets[0].target_position},             // 目标位置
+    {ET_Device0, ET_VID_PID, 0x60FF, 0, &et_offsets[0].target_velocity},             // 目标速度
+    {ET_Device0, ET_VID_PID, 0x6071, 0, &et_offsets[0].target_torque},               // 目标扭矩
+    {ET_Device0, ET_VID_PID, 0x6072, 0, &et_offsets[0].max_torque},                  // 最大扭矩
+    {ET_Device0, ET_VID_PID, 0x6060, 0, &et_offsets[0].operation_mode},              // 运行模式
+    {ET_Device0, ET_VID_PID, 0x0000, 0, &et_offsets[0].reserved1},                   // 保留字段1
+    {ET_Device0, ET_VID_PID, 0x6041, 0, &et_offsets[0].status_word},                 // 状态字
+    {ET_Device0, ET_VID_PID, 0x6064, 0, &et_offsets[0].position_actual_value},       // 实际位置
+    {ET_Device0, ET_VID_PID, 0x606C, 0, &et_offsets[0].velocity_actual_value},       // 实际速度
+    {ET_Device0, ET_VID_PID, 0x6077, 0, &et_offsets[0].torque_actual_value},         // 实际扭矩）
+    {ET_Device0, ET_VID_PID, 0x603F, 0, &et_offsets[0].error_code},                  // 错误代码
+    // {ET_Device0, ET_VID_PID, 0x6061, 0, &et_offsets[0].modes_of_operation_display},  // 模式显示
+    // {ET_Device0, ET_VID_PID, 0x5FF2, 0, &et_offsets[0].reserved2},                   // 保留字段2
+    {} // 结束标记
+};
+
+static ec_pdo_entry_info_t et_device_pdo_entries[] = {
+    /*RxPdo 0x1600*/
+    {0x6040, 0x00, 16}, /* control word */
+    {0x607a, 0x00, 32}, /* target position */
+    {0x60ff, 0x00, 32}, /* target velocity */
+    {0x6071, 0x00, 16}, /* Target Torque */
+    {0x6072, 0x00, 16}, /* Max Torque */
+    {0x6060, 0x00, 8}, /* modes of operation */
+    {0x0000, 0x00, 8},
+    /*TxPdo 0x1A00*/
+    {0x6041, 0x00, 16}, /* status word */
+    {0x6064, 0x00, 32}, /* position actual value */
+    {0x606c, 0x00, 32}, /* velocity actual value */
+    {0x6077, 0x00, 16}, /* torque actual value */
+    {0x603f, 0x00, 16}, /* error code */
+    // {0x6061, 0x00, 8}, /* modes of operation display */
+    // {0x5ff2, 0x00, 8},
+};
+
+static ec_pdo_info_t et_device_pdos[] = {
+    //RxPdo
+    {0x1600, 7, et_device_pdo_entries + 0 },
+    //TxPdo
+    {0x1A00, 5, et_device_pdo_entries + 7 }
+};
+
+static ec_sync_info_t et_device_syncs[] = {
+    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
+    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
+    {2, EC_DIR_OUTPUT, 1, et_device_pdos + 0, EC_WD_ENABLE},
+    {3, EC_DIR_INPUT, 1, et_device_pdos + 1, EC_WD_DISABLE},
+    {0xff}
+};

ethercat_dev/src/ethercat_control/include/ethercat_control/ethercat_core.hpp

@@ -0,0 +1,14 @@
+#pragma once
+#include <stdbool.h>
+
+namespace ethercat_core {
+  // 初始化 EtherCAT（请求 master、配置 PDO、激活、启动实时循环）。
+  // 返回 true 表示已启动/已在运行；false 表示失败或重复启动。
+  bool start();
+
+  // 请求实时循环退出（由 Ctrl+C 回调调用）
+  void request_exit();
+
+  // （后续添加）停机：拉低 run_enable。实时线程不强退，保持可再次启用。
+  void stop_soft();
+}

ethercat_dev/src/ethercat_control/include/ethercat_control/motor_control.hpp

@@ -0,0 +1,81 @@
+#pragma once
+#include <array>
+#include <atomic>
+#include <cstdint>
+#include <algorithm>
+
+constexpr int NUM_SLAVES = 1; //定义电机数量
+
+//运行模式
+enum class OpMode : int8_t {
+  CSP = 8,    // Cyclic Synchronous Position
+  CSV = 9,    // Cyclic Synchronous Velocity
+  CST = 10,   // Cyclic Synchronous Torque
+  PVT = 5     // PVT模式
+};
+
+// 周期线程与上层通讯的共享结构
+// 电机命令
+struct MotorCommand {
+  std::array<std::atomic<int8_t>,  NUM_SLAVES> mode;             // 运行模式
+  std::array<std::atomic<int32_t>, NUM_SLAVES> target_vel;       // 目标速度	
+  std::array<std::atomic<int32_t>, NUM_SLAVES> target_pos;       // 目标位置
+  std::array<std::atomic<int16_t>, NUM_SLAVES> target_torque;    // 目标扭矩
+  std::atomic<double>  pvt_kp[NUM_SLAVES];                       // PVT kp增益
+  std::atomic<double>  pvt_kd[NUM_SLAVES];                       // PVT kd增益
+
+  // 运行控制意图（周期线程据此决定是否写 0x001F）
+  std::array<std::atomic<bool>,    NUM_SLAVES> run_enable;
+
+  // 故障复位/急停意图（待后续添加）
+  std::array<std::atomic<bool>,    NUM_SLAVES> fault_reset;
+  std::array<std::atomic<bool>,    NUM_SLAVES> quick_stop;
+};
+// 电机状态
+struct MotorState {
+  std::array<std::atomic<uint16_t>,NUM_SLAVES> status_word;      // 从站状态字
+  std::array<std::atomic<int32_t>, NUM_SLAVES> pos_act;          // 实际位置
+  std::array<std::atomic<int32_t>, NUM_SLAVES> vel_act;          // 实际速度
+  std::array<std::atomic<int16_t>, NUM_SLAVES> torque_act;       // 实际扭矩
+  std::array<std::atomic<int32_t>, NUM_SLAVES> pos_cmd;          // 每个周期CSP的目标位置（因速度限制，由target_pos拆分而来）
+};
+
+// 全局共享实例（由 .cpp 定义）
+extern MotorCommand g_cmd;
+extern MotorState   g_state;
+
+// ---------- 控制接口（供上层/ROS2 调用） ----------
+
+// 初始化缺省值（上电后调用一次）
+void mc_init_defaults(OpMode default_mode = OpMode::CSV);
+
+// 设定单轴模式/目标值/运行意图
+void mc_set_mode(int idx, OpMode mode);
+void mc_set_target_velocity(int idx, int32_t vel);
+void mc_set_target_position(int idx, int32_t pos);
+void mc_set_target_torque(int idx, int16_t tq);
+void mc_enable_run(int idx, bool enable);
+
+// 批量设置电机指令
+void mc_set_all_mode(OpMode mode);
+void mc_set_all_target_velocity(int32_t vel);
+void mc_enable_all(bool enable);
+
+// 设置PVT参数
+void mc_set_all_pvt_gains(double kp, double kd);
+void mc_set_pvt_gains(int axis, double kp, double kd);
+
+// 故障/急停意图（待后续添加）
+void mc_fault_reset(int idx);
+void mc_quick_stop(int idx);
+
+// 状态读取（从共享状态快照中取，非直接读 PDO）
+void mc_get_state(int idx, uint16_t &status, int32_t &pos, int32_t &vel, int16_t &torque);
+
+// 检查电机轴号是否合法
+inline bool mc_valid_index(int idx) { return idx >= 0 && idx < NUM_SLAVES; }
+
+// void mc_get_pos_cmd_and_act(int idx, int32_t &pos_cmd, int32_t &pos_act);
+// void mc_print_pos_cmd_and_act(int idx);              // 打印单轴
+// void mc_print_all_pos_cmd_and_act();                 // 打印全部
+

ethercat_dev/src/ethercat_control/include/ethercat_control/mt_config.hpp

@@ -0,0 +1,80 @@
+#pragma once
+#include "ecrt.h"
+#include "ethercat_control/motor_control.hpp" // 电机控制库 
+
+#define MT_Device0        0,0    /*EtherCAT address on the bus 别名，位置*/
+  
+#define MT_VID_PID          0x00202008,0x00000000   /*Vendor ID, product code	厂商ID和产品代码*/
+
+typedef struct {
+    unsigned int ctrl_word;                   // 0x6040:00
+    unsigned int target_position;             // 0x607A:00
+    unsigned int target_velocity;             // 0x60FF:00 
+    unsigned int target_torque;               // 0x6071:00 
+    unsigned int max_torque;                  // 0x6072:00 
+    unsigned int operation_mode;              // 0x6060:00
+    unsigned int reserved1;                   // 0x5FFE:00 
+    unsigned int status_word;                 // 0x6041:00
+    unsigned int position_actual_value;       // 0x6064:00
+    unsigned int velocity_actual_value;       // 0x606C:00 
+    unsigned int torque_actual_value;         // 0x6077:00 
+    unsigned int error_code;                  // 0x603F:00
+    unsigned int modes_of_operation_display;  // 0x6061:00
+    unsigned int reserved2;                   // 0x5FFE:00 
+} mt_offset_t;
+
+static mt_offset_t mt_offsets[NUM_SLAVES];
+
+// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
+// 下列结构由IGH主站 cstruct 自动生成
+const static ec_pdo_entry_reg_t mt_domain1_regs[] = {
+    {MT_Device0, MT_VID_PID, 0x6040, 0, &mt_offsets[0].ctrl_word},                   // 控制字
+    {MT_Device0, MT_VID_PID, 0x607A, 0, &mt_offsets[0].target_position},             // 目标位置
+    {MT_Device0, MT_VID_PID, 0x60FF, 0, &mt_offsets[0].target_velocity},             // 目标速度
+    {MT_Device0, MT_VID_PID, 0x6071, 0, &mt_offsets[0].target_torque},               // 目标扭矩
+    {MT_Device0, MT_VID_PID, 0x6072, 0, &mt_offsets[0].max_torque},                  // 最大扭矩
+    {MT_Device0, MT_VID_PID, 0x6060, 0, &mt_offsets[0].operation_mode},              // 运行模式
+    {MT_Device0, MT_VID_PID, 0x5FFE, 0, &mt_offsets[0].reserved1},                   // 保留字段1
+    {MT_Device0, MT_VID_PID, 0x6041, 0, &mt_offsets[0].status_word},                 // 状态字
+    {MT_Device0, MT_VID_PID, 0x6064, 0, &mt_offsets[0].position_actual_value},       // 实际位置
+    {MT_Device0, MT_VID_PID, 0x606C, 0, &mt_offsets[0].velocity_actual_value},       // 实际速度
+    {MT_Device0, MT_VID_PID, 0x6077, 0, &mt_offsets[0].torque_actual_value},         // 实际扭矩）
+    {MT_Device0, MT_VID_PID, 0x603F, 0, &mt_offsets[0].error_code},                  // 错误代码
+    {MT_Device0, MT_VID_PID, 0x6061, 0, &mt_offsets[0].modes_of_operation_display},  // 模式显示
+    {MT_Device0, MT_VID_PID, 0x5FFE, 0, &mt_offsets[0].reserved2},                   // 保留字段2
+    {} // 结束标记
+};
+
+static ec_pdo_entry_info_t mt_device_pdo_entries[] = {
+    /*RxPdo 0x1600*/
+    {0x6040, 0x00, 16}, /* control word */
+    {0x607a, 0x00, 32}, /* target position */
+    {0x60ff, 0x00, 32}, /* target velocity */
+    {0x6071, 0x00, 16}, /* Target Torque */
+    {0x6072, 0x00, 16}, /* Max Torque */
+    {0x6060, 0x00, 8}, /* modes of operation */
+    {0x5ffe, 0x00, 8},
+    /*TxPdo 0x1A00*/
+    {0x6041, 0x00, 16}, /* status word */
+    {0x6064, 0x00, 32}, /* position actual value */
+    {0x606c, 0x00, 32}, /* velocity actual value */
+    {0x6077, 0x00, 16}, /* torque actual value */
+    {0x603f, 0x00, 16}, /* error code */
+    {0x6061, 0x00, 8}, /* modes of operation display */
+    {0x5ffe, 0x00, 8},
+};
+
+static ec_pdo_info_t mt_device_pdos[] = {
+    //RxPdo
+    {0x1600, 7, mt_device_pdo_entries + 0 },
+    //TxPdo
+    {0x1A00, 7, mt_device_pdo_entries + 7 }
+};
+
+static ec_sync_info_t mt_device_syncs[] = {
+    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
+    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
+    {2, EC_DIR_OUTPUT, 1, mt_device_pdos + 0, EC_WD_ENABLE},
+    {3, EC_DIR_INPUT, 1, mt_device_pdos + 1, EC_WD_DISABLE},
+    {0xff}
+};

ethercat_dev/src/ethercat_control/include/ethercat_control/zer_config.hpp

@@ -0,0 +1,80 @@
+#pragma once
+#include "ecrt.h"
+#include "ethercat_control/motor_control.hpp" // 电机控制库 
+
+#define ZER_Device0        0,0    /*EtherCAT address on the bus 别名，位置*/
+  
+#define ZER_VID_PID          0x5a65726f,0x00029252   /*Vendor ID, product code	厂商ID和产品代码*/
+
+typedef struct {
+    unsigned int ctrl_word;                   // 0x6040:00
+    unsigned int target_position;             // 0x607A:00
+    unsigned int target_velocity;             // 0x60FF:00 
+    unsigned int target_torque;               // 0x6071:00 
+    unsigned int max_torque;                  // 0x6072:00 
+    unsigned int operation_mode;              // 0x6060:00
+    unsigned int reserved1;                   // 0xf0ff:00 
+    unsigned int status_word;                 // 0x6041:00
+    unsigned int position_actual_value;       // 0x6064:00
+    unsigned int velocity_actual_value;       // 0x606C:00 
+    unsigned int torque_actual_value;         // 0x6077:00 
+    unsigned int error_code;                  // 0x603F:00
+    unsigned int modes_of_operation_display;  // 0x6061:00
+    unsigned int reserved2;                   // 0xf0ff:00 
+} zer_offset_t;
+
+static zer_offset_t zer_offsets[NUM_SLAVES];
+
+// ------------------- PDO 定义（CSV/CSP/CST），对应 0x1600/0x1A00 -------------------
+// 下列结构由IGH主站 cstruct 自动生成
+const static ec_pdo_entry_reg_t zer_domain1_regs[] = {
+    {ZER_Device0, ZER_VID_PID, 0x6040, 0, &zer_offsets[0].ctrl_word},                   // 控制字
+    {ZER_Device0, ZER_VID_PID, 0x607A, 0, &zer_offsets[0].target_position},             // 目标位置
+    {ZER_Device0, ZER_VID_PID, 0x60FF, 0, &zer_offsets[0].target_velocity},             // 目标速度
+    {ZER_Device0, ZER_VID_PID, 0x6071, 0, &zer_offsets[0].target_torque},               // 目标扭矩
+    {ZER_Device0, ZER_VID_PID, 0x6072, 0, &zer_offsets[0].max_torque},                  // 最大扭矩
+    {ZER_Device0, ZER_VID_PID, 0x6060, 0, &zer_offsets[0].operation_mode},              // 运行模式
+    {ZER_Device0, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[0].reserved1},                   // 保留字段1
+    {ZER_Device0, ZER_VID_PID, 0x6041, 0, &zer_offsets[0].status_word},                 // 状态字
+    {ZER_Device0, ZER_VID_PID, 0x6064, 0, &zer_offsets[0].position_actual_value},       // 实际位置
+    {ZER_Device0, ZER_VID_PID, 0x606C, 0, &zer_offsets[0].velocity_actual_value},       // 实际速度
+    {ZER_Device0, ZER_VID_PID, 0x6077, 0, &zer_offsets[0].torque_actual_value},         // 实际扭矩）
+    {ZER_Device0, ZER_VID_PID, 0x603F, 0, &zer_offsets[0].error_code},                  // 错误代码
+    {ZER_Device0, ZER_VID_PID, 0x6061, 0, &zer_offsets[0].modes_of_operation_display},  // 模式显示
+    {ZER_Device0, ZER_VID_PID, 0xf0ff, 0, &zer_offsets[0].reserved2},                   // 保留字段2
+    {} // 结束标记
+};
+
+static ec_pdo_entry_info_t zer_device_pdo_entries[] = {
+    /*RxPdo 0x1600*/
+    {0x6040, 0x00, 16}, /* control word */
+    {0x607a, 0x00, 32}, /* target position */
+    {0x60ff, 0x00, 32}, /* target velocity */
+    {0x6071, 0x00, 16}, /* Target Torque */
+    {0x6072, 0x00, 16}, /* Max Torque */
+    {0x6060, 0x00, 8}, /* modes of operation */
+    {0xf0ff, 0x00, 8},
+    /*TxPdo 0x1A00*/
+    {0x6041, 0x00, 16}, /* status word */
+    {0x6064, 0x00, 32}, /* position actual value */
+    {0x606c, 0x00, 32}, /* velocity actual value */
+    {0x6077, 0x00, 16}, /* torque actual value */
+    {0x603f, 0x00, 16}, /* error code */
+    {0x6061, 0x00, 8}, /* modes of operation display */
+    {0xf0ff, 0x00, 8},
+};
+
+static ec_pdo_info_t zer_device_pdos[] = {
+    //RxPdo
+    {0x1600, 7, zer_device_pdo_entries + 0 },
+    //TxPdo
+    {0x1A00, 7, zer_device_pdo_entries + 7 }
+};
+
+static ec_sync_info_t zer_device_syncs[] = {
+    {0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE},
+    {1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE},
+    {2, EC_DIR_OUTPUT, 1, zer_device_pdos + 0, EC_WD_ENABLE},
+    {3, EC_DIR_INPUT, 1, zer_device_pdos + 1, EC_WD_DISABLE},
+    {0xff}
+};

ethercat_dev/src/ethercat_control/launch/ethercat_node.launch.py

@@ -0,0 +1,24 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import TimerAction
+
+def generate_launch_description():
+
+    # realtime_env = {
+    #     "RCUTILS_SCHEDULER_POLICY" : "SCHED_FIFO",
+    #     "RCUTILS_THREAD_PRIORITY" : "90",
+    #     "RCUTILS_CONSOLE_OUTPUT_FORMAT" : "{time}:[{name}]:{message}"
+    # }
+    # 创建节点启动描述
+    ethercat_node = Node(
+        package='ethercat_control',  # 功能包名称
+        executable='ethercat_node',  # 可执行文件名称
+        name='ethercat_node',  # 节点名称（可自定义）
+        output='screen',  # 输出到屏幕
+        parameters=[{"lock_memory": True, "thread_priority":90}]  # 启用模拟时间
+    )
+    
+    # 组装launch描述
+    return LaunchDescription([
+        ethercat_node
+    ])

ethercat_dev/src/ethercat_control/package.xml

@@ -0,0 +1,23 @@
+<?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>ethercat_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="you@example.com">orangepi</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>rcl_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>

ethercat_dev/src/ethercat_control/src/cmd_interface.cpp

@@ -0,0 +1,265 @@
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <thread>
+#include <pthread.h>
+#include <sched.h>
+#include <optional>          // ✅ 新增
+#include "motor_control.hpp"
+
+// 由你现有源码提供：
+void cyclic_task();        // 实时循环（已接入 g_cmd / g_state）
+void ethercat_after_init();// 可选：若你想在这里放 mc_init_defaults 等
+
+// ---------- 模式解析（安全版）：失败返回 nullopt，不做任何电机操作 ----------
+static std::optional<OpMode> parse_mode(const std::string &s){
+  if (s=="csv" || s=="CSV" || s=="vel" || s=="VEL" || s=="speed" || s=="SPEED")
+    return OpMode::CSV; // 9
+  if (s=="csp" || s=="CSP" || s=="pos" || s=="POS" || s=="position" || s=="POSITION")
+    return OpMode::CSP; // 8
+  if (s=="cst" || s=="CST" || s=="tor" || s=="TOR" || s=="torque" || s=="TORQUE")
+    return OpMode::CST; // 10
+  return std::nullopt;  // ❗未知输入 -> 调用者处理，绝不默认 CSV
+}
+
+void start_rt_cyclic_thread(){
+  std::thread([]{
+    // 把实时优先级设置到线程上（不占用主线程）
+    sched_param param{};
+    param.sched_priority = sched_get_priority_max(SCHED_FIFO);
+    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
+    cyclic_task(); // 阻塞
+  }).detach();
+}
+
+static void print_help(){
+  std::cout <<
+R"(命令格式：
+  help                      显示帮助
+  quit / exit               退出程序
+  enable on|off [i|-1]      使能/禁止运行；省略或 -1 表示所有电机
+  state [i]                 打印第 i 台状态（省略打印 0..19 摘要）
+  set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>   同时设置多个轴
+  （别名：csv|vel|speed，csp|pos|position，cst|tor|torque）
+)"
+  << std::endl;
+}
+
+// static void print_help(){
+//   std::cout <<
+// R"(命令格式：
+//   help                      显示帮助
+//   quit / exit               退出程序
+//   enable on|off [i|-1]      使能/禁止运行；省略或 -1 表示所有电机
+//   mode i csv|csp|cst        设置第 i 台的模式（速度/位置/扭矩）
+//   vel  i value              设置第 i 台目标速度（int32）
+//   pos  i value              设置第 i 台目标位置（int32）
+//   tor  i value              设置第 i 台目标扭矩（int16）
+//   all vel value             所有电机目标速度
+//   state [i]                 打印第 i 台状态（省略打印 0..19 摘要）
+//   set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>   同时设置多个轴
+//   （别名：csv|vel|speed，csp|pos|position，cst|tor|torque）
+// )"
+//   << std::endl;
+// }
+void cli_command_loop(){
+  print_help();
+  std::string line;
+  while (std::cout << "> " && std::getline(std::cin, line)){
+    std::istringstream iss(line);
+    std::string cmd; iss >> cmd;
+    if (cmd.empty()) continue;
+
+    if (cmd=="help") { print_help(); continue; }
+    if (cmd=="quit" || cmd=="exit") { break; }
+
+    if (cmd=="enable"){
+      std::string onoff; int idx = -1;
+      iss >> onoff >> idx;
+      bool en = (onoff=="on" || onoff=="1" || onoff=="true");
+      if (!iss || idx==-1){
+        mc_enable_all(en);
+        std::cout << "[ALL] enable=" << en << "\n";
+      }else{
+        if (mc_valid_index(idx)){ mc_enable_run(idx,en); std::cout << "[S"<<idx<<"] enable="<<en<<"\n";}
+        else std::cout << "无效索引\n";
+      }
+      continue;
+    }
+
+    if (cmd=="mode"){
+      int idx; std::string m; iss >> idx >> m;
+      if (iss && mc_valid_index(idx)){
+        auto pm = parse_mode(m);
+        if (!pm){
+          std::cout << "无效模式: " << m << "（允许: csv/csp/cst 或 vel/pos/tor）\n";
+          continue; // ❗不做任何电机操作
+        }
+        mc_set_mode(idx, *pm);
+        std::cout << "[S"<<idx<<"] mode="<<m<<"\n";
+      }else {
+        std::cout << "用法: mode i csv|csp|cst\n";
+      }
+      continue;
+    }
+
+    if (cmd=="vel"){
+      int idx; long v; iss >> idx >> v;
+      if (iss && mc_valid_index(idx)){
+        mc_set_target_velocity(idx, (int32_t)v);
+        std::cout << "[S"<<idx<<"] vel="<<v<<"\n";
+      }else std::cout << "用法: vel i value\n";
+      continue;
+    }
+
+    if (cmd=="pos"){
+      int idx; long p; iss >> idx >> p;
+      if (iss && mc_valid_index(idx)){
+        mc_set_target_position(idx, (int32_t)p);
+        std::cout << "[S"<<idx<<"] pos="<<p<<"\n";
+      }else std::cout << "用法: pos i value\n";
+      continue;
+    }
+
+    if (cmd=="tor"){
+      int idx; long t; iss >> idx >> t;
+      if (iss && mc_valid_index(idx)){
+        mc_set_target_torque(idx, (int16_t)t);
+        std::cout << "[S"<<idx<<"] tor="<<t<<"\n";
+      }else std::cout << "用法: tor i value\n";
+      continue;
+    }
+
+    if (cmd=="all"){
+      std::string what; long v;
+      iss >> what >> v;
+      if (what=="vel" && iss){
+        mc_set_all_target_velocity((int32_t)v);
+        std::cout << "[ALL] vel="<<v<<"\n";
+      }else{
+        std::cout << "用法: all vel value\n";
+      }
+      continue;
+    }
+
+    if (cmd == "set") { // 批量控制函数（先校验，后执行）
+      // 把这一行剩余内容整体取出来，然后按 ';' 切成多段
+      std::string rest;
+      std::getline(iss, rest);
+      if (rest.empty()) {
+          std::cout << "用法: set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>\n";
+          continue;
+      }
+
+      // 去掉可能的开头空格/结尾空格
+      auto ltrim = [](std::string& s){
+          size_t p = s.find_first_not_of(" \t");
+          if (p != std::string::npos) s.erase(0, p);
+      };
+      auto rtrim = [](std::string& s){
+          size_t p = s.find_last_not_of(" \t");
+          if (p != std::string::npos) s.erase(p+1);
+      };
+
+      // 按 ';' 分割子命令
+      std::vector<std::string> chunks;
+      {
+          std::stringstream ss(rest);
+          std::string piece;
+          while (std::getline(ss, piece, ';')) {
+              ltrim(piece); rtrim(piece);
+              if (!piece.empty()) chunks.push_back(piece);
+          }
+      }
+      if (chunks.empty()) {
+          std::cout << "用法: set <i> csp pos <val>; <j> csv vel <val>; <k> cst tor <val>\n";
+          continue;
+      }
+
+      // 逐个子命令解析与执行：⚠️ 先校验，再动作；出错则跳过该子命令
+      for (auto& one : chunks) {
+          std::istringstream si(one);
+          int idx = -1;
+          std::string mode_s, field;
+          long long val = 0;
+
+          si >> idx >> mode_s >> field >> val;
+          if (!si || !mc_valid_index(idx)) {
+              std::cout << "[set] 子命令格式错误或轴号越界: \"" << one << "\"\n";
+              continue; // ❗不做任何电机操作
+          }
+
+          // 解析模式
+          auto pm = parse_mode(mode_s);
+          if (!pm) {
+              std::cout << "[set] 未知模式: " << mode_s << " in \"" << one << "\"\n";
+              continue; // ❗不做任何电机操作
+          }
+          OpMode mode = *pm;
+
+          // 校验字段是否与模式匹配
+          bool ok_field = false;
+          if (mode == OpMode::CSP) {
+              ok_field = (field=="pos" || field=="position" || field=="POS" || field=="POSITION");
+              if (!ok_field) {
+                  std::cout << "[set] CSP 需要字段: pos <val> ；Got \"" << field << "\"\n";
+                  continue; // ❗不做任何电机操作
+              }
+          } else if (mode == OpMode::CSV) {
+              ok_field = (field=="vel" || field=="velocity" || field=="speed" ||
+                          field=="VEL" || field=="VELOCITY" || field=="SPEED");
+              if (!ok_field) {
+                  std::cout << "[set] CSV 需要字段: vel <val> ；Got \"" << field << "\"\n";
+                  continue; // ❗不做任何电机操作
+              }
+          } else if (mode == OpMode::CST) {
+              ok_field = (field=="tor" || field=="torque" || field=="TOR" || field=="TORQUE");
+              if (!ok_field) {
+                  std::cout << "[set] CST 需要字段: tor <val> ；Got \"" << field << "\"\n";
+                  continue; // ❗不做任何电机操作
+              }
+          }
+
+          // —— 所有校验通过，才真正执行 —— //
+          mc_set_mode(idx, mode);
+          if (mode == OpMode::CSP) {
+              mc_set_target_position(idx, static_cast<int32_t>(val));
+              std::cout << "[S" << idx << "] mode=CSP pos=" << val << "\n";
+          } else if (mode == OpMode::CSV) {
+              mc_set_target_velocity(idx, static_cast<int32_t>(val));
+              std::cout << "[S" << idx << "] mode=CSV vel=" << val << "\n";
+          } else if (mode == OpMode::CST) {
+              mc_set_target_torque(idx, static_cast<int16_t>(val));
+              std::cout << "[S" << idx << "] mode=CST tor=" << val << "\n";
+          }
+          // 是否自动 enable 不在此处改变，保持你当前逻辑；如需可追加：
+          // mc_enable_run(idx, true);
+      }
+      continue;
+    }
+
+    if (cmd=="state"){
+      int idx=-1; iss >> idx;
+      if (!iss){ // 打印摘要
+        for (int i=0;i<NUM_SLAVES;++i){
+          uint16_t sw; int32_t pos,vel; int16_t tq;
+          mc_get_state(i, sw, pos, vel, tq);
+          std::cout << "[S"<<i<<"] SW=0x"<<std::hex<<sw<<std::dec
+                    <<" pos="<<pos<<" vel="<<vel<<" tq="<<tq<<"\n";
+        }
+      }else if (mc_valid_index(idx)){
+        uint16_t sw; int32_t pos,vel; int16_t tq;
+        mc_get_state(idx, sw, pos, vel, tq);
+        std::cout << "[S"<<idx<<"] SW=0x"<<std::hex<<sw<<std::dec
+                  <<" pos="<<pos<<" vel="<<vel<<" tq="<<tq<<"\n";
+      }else{
+        std::cout << "索引无效\n";
+      }
+      continue;
+    }
+
+    std::cout << "未知命令，输入 help 查看帮助\n";
+  }
+  std::cout << "退出命令行控制。\n";
+}

ethercat_dev/src/ethercat_control/src/ethercat_core.cpp

@@ -0,0 +1,828 @@
+/*
+ * 程序功能为通过EtherCAT周期控制伺服电机
+ *
+ * 功能总览：
+ * 1) 初始化 EtherCAT Master / Domain / 从站配置（PDO/同步管理/DC 时钟）；
+ * 2) 注册 PDO 映射，建立 process image 的偏移（offsets[]）；
+ * 3) 启动 1kHz 实时循环（cyclic_task）：
+ *    - 同步 DC 时钟，收发域数据；
+ *    - 跟踪各轴 DS402 状态机（Ready->Switched on->Operation enabled）；
+ *    - 根据上层 g_cmd（CSV/CSP/CST/PVT）写入目标值到 PDO；
+
+*/
+#include <errno.h>
+#include <signal.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/resource.h>
+#include <sys/time.h>
+#include <sys/types.h>
+#include <unistd.h>
+#include <time.h>
+#include <sys/mman.h>
+#include <malloc.h>
+#include <sched.h> /* sched_setscheduler() */
+#include <thread>
+#include <atomic>
+#include <pthread.h>
+#include <cmath>   // for std::llround
+#include <iostream>
+
+
+
+#include "ethercat_control/mt_config.hpp"
+#include "ethercat_control/zer_config.hpp"
+#include "ethercat_control/et_config.hpp"
+// #include "ethercat_control/ds_config.hpp"
+
+
+//#include "cmd_interface.hpp" //命令行控制库
+
+
+// sudo /opt/etherlab/bin/ethercat
+// sudo /etc/init.d/ethercat start
+// sudo dmesg | grep EtherCAT
+// sudo /opt/etherlab/bin/ethercat cstruct -a 0
+
+// ------------------- 应用参数 -------------------
+#define FREQUENCY 125                          //控制周期频率1000Hz
+#define CLOCK_TO_USE CLOCK_MONOTONIC
+
+//#define MEASURE_TIMING
+
+//#define NUM_SLAVES                 3          //从端数量在motor_control.hpp中定义
+//#define TARGET_VALUE             10000        //目标位置、速度或者扭矩
+
+#define CYCLIC_POSITION            8            //CSP 周期同步位置模式
+#define CYCLIC_VELOCITY            9            //CSP 周期同步速度模式
+#define CYCLIC_TORQUE              10           //CSP 周期同步扭矩模式
+#define PVT_MODE                   5            //PVT模式
+
+#define CSP_MAX_VEL_COUNTS_PER_S    65536 //65536        //CSP最大速度（计数/s）
+//#define CSP_MAX_VEL_COUNTS_PER_S   1             //低速测试CSP最大速度（计数）
+
+#define CSP_POS_DEADBAND           10           //CSP允许的误差(计数)
+
+#define NSEC_PER_SEC (1000000000L)
+#define PERIOD_NS (NSEC_PER_SEC / FREQUENCY)    //本次设置周期PERIOD_NS为1ms
+
+#define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + \
+        (B).tv_nsec - (A).tv_nsec)
+
+#define TIMESPEC2NS(T) ((uint64_t) (T).tv_sec * NSEC_PER_SEC + (T).tv_nsec)
+
+static std::atomic<bool> g_started{false}; //g_started 是一个进程内的“已启动”标志
+static std::atomic<bool> g_run{true}; //g_run是运行标志，为1时电机可运行
+
+
+// ------------------- EtherCAT 句柄 -------------------
+static ec_master_t *master = NULL;
+static ec_master_state_t master_state = {};
+
+static ec_domain_t *domain1 = NULL;
+static ec_domain_state_t domain1_state = {};
+
+static ec_slave_config_t *sc[NUM_SLAVES] = {0}; 
+static ec_slave_config_state_t sc_state[NUM_SLAVES] = {};
+
+static uint8_t *domain1_pd = NULL; // process data
+
+static unsigned int counter = 0;
+static unsigned int sync_ref_counter = 0;
+const struct timespec cycletime = {0, PERIOD_NS};
+
+// ------------------- 工具函数 -------------------
+// 相加时间并处理纳秒进位
+struct timespec timespec_add(struct timespec time1, struct timespec time2)
+{
+    struct timespec result;
+
+    if ((time1.tv_nsec + time2.tv_nsec) >= NSEC_PER_SEC) {
+        result.tv_sec = time1.tv_sec + time2.tv_sec + 1;
+        result.tv_nsec = time1.tv_nsec + time2.tv_nsec - NSEC_PER_SEC;
+    } else {
+        result.tv_sec = time1.tv_sec + time2.tv_sec;
+        result.tv_nsec = time1.tv_nsec + time2.tv_nsec;
+    }
+
+    return result;
+}
+
+//查看domain1工作状态
+void check_domain1_state(void)
+{
+    ec_domain_state_t ds;
+
+    ecrt_domain_state(domain1, &ds);
+
+    if (ds.working_counter != domain1_state.working_counter)
+        //printf("Domain1: WC %u.\n", ds.working_counter); //当数字变化，表示丢包
+    if (ds.wc_state != domain1_state.wc_state)
+        //printf("Domain1: State %u.\n", ds.wc_state);
+
+    domain1_state = ds;
+}
+
+//查看master工作状态
+void check_master_state(void)
+{
+    ec_master_state_t ms;
+
+    ecrt_master_state(master, &ms);
+
+    if (ms.slaves_responding != master_state.slaves_responding)
+        //printf("%u slave(s).\n", ms.slaves_responding);
+    if (ms.al_states != master_state.al_states)
+        //printf("AL states: 0x%02X.\n", ms.al_states);
+    if (ms.link_up != master_state.link_up)
+        //printf("Link is %s.\n", ms.link_up ? "up" : "down");
+
+    master_state = ms;
+}
+
+//查询并更新每个从站的在线/AL/operational 状态
+void check_slave_config_states(void)
+{
+    ec_slave_config_state_t s;
+    for (int i = 0; i < NUM_SLAVES; ++i) {
+        if (!sc[i]) continue;
+        ecrt_slave_config_state(sc[i], &s);
+        if (s.al_state != sc_state[i].al_state)
+            //printf("[S%02d] State 0x%02X.\n", i, s.al_state);
+        if (s.online != sc_state[i].online)
+            //printf("[S%02d] %s.\n", i, s.online ? "online" : "offline");
+        if (s.operational != sc_state[i].operational)
+            //printf("[S%02d] %soperational.\n", i, s.operational ? "" : "Not ");
+        sc_state[i] = s;
+    }
+}
+
+// ------------------- 实时循环 -------------------
+/**
+ *  1kHz 实时控制主循环
+ *
+ * 核心流程（每周期）：
+ *  1) 绝对时钟休眠到下个周期；写入应用时间到主站；
+ *  2) 接收帧 / 处理域数据；
+ *  3) 遍历每个从站：
+ *     - 读取 TxPDO：状态字/实际位置/速度/扭矩/模式显示；
+ *     - 把读到的实际值写回共享状态 g_state（给上位机/ROS2）；
+ *     - 读取上层命令 g_cmd：期望模式、run_enable、pos/vel/tor、PVT kp/kd；
+ *     - 模式切换：降级到perop→写 0x6060→初始化必要变量→进入下周期再跑；
+ *     - 跟踪 DS402 状态机，达到 Operation enabled 后根据模式写入：
+ *         CSP：写 0x607A；
+ *         CSV：写 0x60FF；
+ *         CST：写 0x6071；
+ *         PVT：写 0x607A/0x60FF/0x6071 及 KP/KD(0x2000/0x2001)；
+ *     - run_enable=false：安全处理（速度清零、CSP 对齐当前位置、扭矩清零）。
+ *  4) 同步 DC 参考时钟与从站时钟；队列域数据并发送帧。
+ *
+ */
+
+int lastStatus = 0;
+int motorCount1 = 0;
+int motorCount2 = 0;
+
+void cyclic_task()
+{
+    struct timespec wakeupTime, time;
+#ifdef MEASURE_TIMING
+    struct timespec startTime, endTime, lastStartTime = {};
+    uint32_t period_ns = 0, exec_ns = 0, latency_ns = 0,
+             latency_min_ns = 0, latency_max_ns = 0,
+             period_min_ns = 0, period_max_ns = 0,
+             exec_min_ns = 0, exec_max_ns = 0;
+#endif
+
+    clock_gettime(CLOCK_TO_USE, &wakeupTime);        //当前时间
+    static uint16_t command[NUM_SLAVES];             //状态字掩码
+    static uint16_t status[NUM_SLAVES];              //状态字
+    static uint16_t last_status[NUM_SLAVES];         //上个循环的状态字
+    static int inited = 0;                           //初始化
+
+    while(g_run.load(std::memory_order_relaxed)) {   //主循环
+        wakeupTime = timespec_add(wakeupTime, cycletime);
+        clock_nanosleep(CLOCK_TO_USE, TIMER_ABSTIME, &wakeupTime, NULL);    //使用绝对时间，精确等待下一个周期，避免循环执行的时间造成周期漂移
+
+        // Write application time to master
+        //
+        // It is a good idea to use the target time (not the measured time) as
+        // application time, because it is more stable.
+        //
+        ecrt_master_application_time(master, TIMESPEC2NS(wakeupTime));  // 更新主站应用时间
+
+#ifdef MEASURE_TIMING
+        clock_gettime(CLOCK_TO_USE, &startTime);
+        latency_ns = DIFF_NS(wakeupTime, startTime);
+        period_ns = DIFF_NS(lastStartTime, startTime);
+        exec_ns = DIFF_NS(lastStartTime, endTime);
+        lastStartTime = startTime;
+
+        if (latency_ns > latency_max_ns) {
+            latency_max_ns = latency_ns;
+        }
+        if (latency_ns < latency_min_ns) {
+            latency_min_ns = latency_ns;
+        }
+        if (period_ns > period_max_ns) {
+            period_max_ns = period_ns;
+        }
+        if (period_ns < period_min_ns) {
+            period_min_ns = period_ns;
+        }
+        if (exec_ns > exec_max_ns) {
+            exec_max_ns = exec_ns;
+        }
+        if (exec_ns < exec_min_ns) {
+            exec_min_ns = exec_ns;
+        }
+#endif
+        // receive process data
+        ecrt_master_receive(master);    //接收 EtherCAT 帧
+        ecrt_domain_process(domain1);   //处理域数据
+
+        // check process data state (optional)
+        check_domain1_state();      //检查域状态
+
+        if (counter) {
+            counter--;
+        } else { // do this at 1 Hz
+            counter = FREQUENCY;
+
+        // check for master state (optional)
+        check_master_state();               //检查主站状态
+		// check for slave configuration state(s)
+		check_slave_config_states();        //检查从站状态
+
+#ifdef MEASURE_TIMING
+            // output timing stats
+            printf("period     %10u ... %10u\n",
+                    period_min_ns, period_max_ns);
+            printf("exec       %10u ... %10u\n",
+                    exec_min_ns, exec_max_ns);
+            printf("latency    %10u ... %10u\n",
+                    latency_min_ns, latency_max_ns);
+            printf("status word: 0x%4x\n",status);
+            period_max_ns = 0;
+            period_min_ns = 0xffffffff;
+            exec_max_ns = 0;
+            exec_min_ns = 0xffffffff;
+            latency_max_ns = 0;
+            latency_min_ns = 0xffffffff;
+#endif
+
+        }
+        
+        if (!inited) {
+            for (int i = 0; i < NUM_SLAVES; ++i) {
+                command[i] = 0x004F;
+                status[i] = 0x000F;
+                last_status[i] = status[i];
+            }
+            inited = 1;
+        }
+
+        static uint8_t csp_initialized[NUM_SLAVES] = {0};                    // 进入CSP后是否已对齐过目标（防止冲击）
+        static uint8_t last_mode_cmd_inited = 0;  
+        static int8_t  last_mode_cmd[NUM_SLAVES] = {CYCLIC_VELOCITY};        // 记录上周期的期望模式（默认为CSV）  
+
+        // 读取从站状态字、当前位置、速度、扭矩、运行模式
+        for (int i = 0; i < NUM_SLAVES; ++i) 
+        {                          
+            if (i == 0)
+            // if (false)
+            {
+                uint16_t sw = EC_READ_U16(domain1_pd + zer_offsets[i].status_word);
+                int32_t  pv = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
+                int32_t  vv = EC_READ_S32(domain1_pd + zer_offsets[i].velocity_actual_value);
+                int16_t  tv = EC_READ_S16(domain1_pd + zer_offsets[i].torque_actual_value);
+                int8_t   md = EC_READ_S8 (domain1_pd + zer_offsets[i].modes_of_operation_display);
+
+                status[i] = sw;                                 //侦测子站状态字变化
+                if (status[i] != last_status[i]) {
+                    // printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
+                    last_status[i] = status[i];
+                }
+                
+                // ----- 1) 写共享状态（供ROS2 读取）-----
+                g_state.status_word[i].store(sw, std::memory_order_relaxed);
+                g_state.pos_act[i].store(pv,    std::memory_order_relaxed);
+                g_state.vel_act[i].store(vv,    std::memory_order_relaxed);
+                g_state.torque_act[i].store(tv, std::memory_order_relaxed);
+
+                // ----- 2) 读取本周期命令 -----
+                // 读CSP/CSV/CST/PVT 命令
+                const int8_t  mode_cmd   = g_cmd.mode[i].load(std::memory_order_relaxed);
+                const bool    run_enable = g_cmd.run_enable[i].load(std::memory_order_relaxed);
+                const int32_t vel_cmd    = g_cmd.target_vel[i].load(std::memory_order_relaxed);
+                const int32_t pos_cmd    = g_cmd.target_pos[i].load(std::memory_order_relaxed);
+                const int16_t tor_cmd    = g_cmd.target_torque[i].load(std::memory_order_relaxed);
+
+                //EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定
+
+                // 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
+                if (!last_mode_cmd_inited) {
+                    last_mode_cmd[i] = md;
+                    if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
+                }
+
+                if (md != mode_cmd) {
+                    // 1) 拉到 Ready to switch on（允许改模式）
+                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);
+                    // 2) 写目标模式
+                    EC_WRITE_S8 (domain1_pd + zer_offsets[i].operation_mode, mode_cmd); 
+                    // 3) 下一周期状态机会自动从 0x0021→0x0023→0x0027
+                    // 进入新模式时做必要初始化
+
+                    bool csp_mode = (mode_cmd == CYCLIC_POSITION);
+                    if (csp_mode) {
+                        // 初次进入CSP：把目标位置对齐当前实际位置，避免冲击
+                        // g_cmd.target_pos[i].store(pv, std::memory_order_relaxed); //把电机当前位置更新进上层
+                        // EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv);
+                        
+                        csp_initialized[i] = 1;
+                    } else {
+                        csp_initialized[i] = 0;
+                    }
+                    last_mode_cmd[i] = mode_cmd;
+                    continue; // 本周期先不再下其他目标，等模式生效
+                }
+                
+                // DS402 状态机 & 写控制/目标
+                if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
+                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0006);   // Ready to switch on
+                    command[i] = 0x006F;
+                    std::cout << " Motor : " << i << "  Ready to switch on" << std::endl;
+                } else if ((status[i] & command[i]) == 0x0021) {
+                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);   // Switched on
+                    command[i] = 0x006F;
+
+                    // 同步当前位置为目标位置
+                    int32_t  pv2 = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
+                    EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv2);
+                    g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
+
+                    motorCount1 ++;
+                    std::cout << " Motor : " << i  << " times : " << motorCount1 << " Switched on" << std::endl;
+                } else if ((status[i] & command[i]) == 0x0023) {
+                    EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x000f);   // Operation enabled
+                    command[i] = 0x006F;
+
+                    std::cout << " Motor : " << i << "  Operation enabled" << std::endl;
+                } else if ((status[i] & command[i]) == 0x0027) {      
+                    // ---- 4) 运行态写目标（按模式/运行意图） ----
+                    if (run_enable) {
+                        //EC_WRITE_U16(domain1_pd + offsets[i].ctrl_word, 0x001F); // 仅在PP模式下需要
+                        switch (mode_cmd) {
+                            case CYCLIC_VELOCITY: // 9
+                                EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, vel_cmd);
+                                break;
+                            case CYCLIC_POSITION: { // 8
+                                // ---- 绝对式目标生成，不再用 pv 做增量基准 ----
+                                // 每轴记住“上次下发的命令位置”
+                                static int32_t csp_cmd_pos[NUM_SLAVES];         // 上次下发到 0x607A 的值
+                                static uint8_t csp_inited[NUM_SLAVES] = {0};    // 是否已初始化
+                                // 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
+                                static int64_t vmax_acc[NUM_SLAVES] = {0};      // 单位：counts/s 累加器
+
+                                // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
+                                if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
+                                    csp_cmd_pos[i] = pv;       // 先对齐
+                                    vmax_acc[i]     = 0;
+                                    csp_inited[i]   = 1;
+                                }
+
+                                // 期望的“最终绝对目标”
+                                const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);
+
+                                // 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
+                                vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S;     // +v [counts/s]
+                                int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
+                                vmax_acc[i] -= (int64_t)max_step * FREQUENCY;          // 保留余数到下周期
+
+                                // 相对“命令轨迹”的误差（不是相对 pv）
+                                const int32_t err_cmd = goal - csp_cmd_pos[i];
+
+                                // 死区：足够近就贴合到 goal
+                                if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
+                                    csp_cmd_pos[i] = goal;
+                                } else {
+                                    // 限速迈步：命令轨迹只按时间往 goal 逼近
+                                    if (err_cmd > 0)      csp_cmd_pos[i] += (err_cmd >  max_step) ?  max_step : err_cmd;
+                                    else                  csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
+                                }
+
+                                // 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
+                                // const int32_t follow_err = csp_cmd_pos[i] - pv;
+                                // const int32_t FE_LIMIT =  (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
+                                // if (std::abs(follow_err) > FE_LIMIT) {
+                                //     // 例如：冻结或减半 max_step，给驱动时间追上
+                                // }
+
+                                // 下发“绝对目标”（不是增量）
+                                EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, csp_cmd_pos[i]);
+
+                                // 回显给上层（/joint_states_cmd 用）
+                                g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
+                                break;
+                            }
+                            case CYCLIC_TORQUE:   // 10
+                                EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, tor_cmd);
+                                break;
+                            default:
+                                // 未知模式：默认速度模式安全置 0
+                                //EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
+                                break;
+                                
+                        }
+                    } else {
+                        // run_enable=false：清零目标
+                        EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);
+                        // CSV 安全：速度清零
+                        EC_WRITE_S32(domain1_pd + zer_offsets[i].target_velocity, 0);
+                        // CSP 安全：目标位置=当前位置 -> 立即“冻结”
+                        
+                        int32_t pv_hold = EC_READ_S32(domain1_pd + zer_offsets[i].position_actual_value);
+                        EC_WRITE_S32(domain1_pd + zer_offsets[i].target_position, pv_hold);
+                        // 可选：扭矩模式也清零（最好维持急停状态，待修改）
+                        EC_WRITE_S16(domain1_pd + zer_offsets[i].target_torque, 0);
+                        EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
+                    }
+                }
+            }else{
+                uint16_t sw = EC_READ_U16(domain1_pd + mt_offsets[i].status_word);
+                int32_t  pv = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
+                int32_t  vv = EC_READ_S32(domain1_pd + mt_offsets[i].velocity_actual_value);
+                int16_t  tv = EC_READ_S16(domain1_pd + mt_offsets[i].torque_actual_value);
+                int8_t   md = EC_READ_S8 (domain1_pd + mt_offsets[i].modes_of_operation_display);
+
+                // printf("[S%02d] 状态字: 0x%04X, 位置: %d, 速度: %d, 力矩: %d, 模式: %d\n", i, sw, pv, vv, tv, md);
+
+                status[i] = sw;                                 //侦测子站状态字变化
+                if (status[i] != last_status[i]) {
+                    // printf("[S%02d] 状态改变为: 0x%04X\n", i, status[i]);
+                    last_status[i] = status[i];
+                }
+                
+                // ----- 1) 写共享状态（供ROS2 读取）-----
+                g_state.status_word[i].store(sw, std::memory_order_relaxed);
+                g_state.pos_act[i].store(pv,    std::memory_order_relaxed);
+                g_state.vel_act[i].store(vv,    std::memory_order_relaxed);
+                g_state.torque_act[i].store(tv, std::memory_order_relaxed);
+
+                // ----- 2) 读取本周期命令 -----
+                // 读CSP/CSV/CST/PVT 命令
+                const int8_t  mode_cmd   = g_cmd.mode[i].load(std::memory_order_relaxed);
+                const bool    run_enable = g_cmd.run_enable[i].load(std::memory_order_relaxed);
+                const int32_t vel_cmd    = g_cmd.target_vel[i].load(std::memory_order_relaxed);
+                const int32_t pos_cmd    = g_cmd.target_pos[i].load(std::memory_order_relaxed);
+                const int16_t tor_cmd    = g_cmd.target_torque[i].load(std::memory_order_relaxed);
+
+                //EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定
+
+                // 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
+                if (!last_mode_cmd_inited) {
+                    last_mode_cmd[i] = md;
+                    if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
+                }
+
+                if (md != mode_cmd) {
+                    // 1) 拉到 Ready to switch on（允许改模式）
+                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0006);
+                    // 2) 写目标模式
+                    EC_WRITE_S8 (domain1_pd + mt_offsets[i].operation_mode, mode_cmd); 
+                    // 3) 下一周期状态机会自动从 0x0021→0x0023→0x0027
+                    // 进入新模式时做必要初始化
+                    if (mode_cmd == CYCLIC_POSITION) {
+                        // 初次进入CSP：把目标位置对齐当前实际位置，避免冲击
+                        // g_cmd.target_pos[i].store(pv, std::memory_order_relaxed); //把电机当前位置更新进上层
+                        // EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv);
+                        csp_initialized[i] = 1;
+
+                        // std::cout << "new Goal : " << pv << std::endl;
+
+                    } else {
+                        csp_initialized[i] = 0;
+                    }
+                    last_mode_cmd[i] = mode_cmd;
+                    continue; // 本周期先不再下其他目标，等模式生效
+                }
+                
+                // DS402 状态机 & 写控制/目标
+                if ((status[i] & command[i]) == 0x0001 || (status[i] & command[i]) == 0x0040) {
+                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0006);   // Ready to switch on
+                    command[i] = 0x006F;
+                    std::cout << " Motor : " << i << "  Ready to switch on" << std::endl;
+                } else if ((status[i] & command[i]) == 0x0021) {
+                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);   // Switched on
+                    command[i] = 0x006F;
+
+                    // 同步当前位置为目标位置
+                    int32_t  pv2 = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
+                    EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv2);
+                    g_cmd.target_pos[i].store(pv2, std::memory_order_relaxed);
+
+                    motorCount2 ++;
+                    std::cout << " Motor : " << i << " times : " << motorCount2 << "  Switched on" << std::endl;
+                } else if ((status[i] & command[i]) == 0x0023) {
+                    EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x000f);   // Operation enabled
+                    command[i] = 0x006F;
+                    std::cout << " Motor : " << i << "  Operation enabled" << std::endl;
+                } else if ((status[i] & command[i]) == 0x0027) {
+                    // ---- 4) 运行态写目标（按模式/运行意图） ----
+                    if (run_enable) {
+                        switch (mode_cmd) {
+                            case CYCLIC_VELOCITY: // 9
+                                EC_WRITE_S32(domain1_pd + mt_offsets[i].target_velocity, vel_cmd);
+                                break;
+                            case CYCLIC_POSITION: { // 8
+                                // ---- 绝对式目标生成，不再用 pv 做增量基准 ----
+                                // 每轴记住“上次下发的命令位置”
+                                static int32_t csp_cmd_pos[NUM_SLAVES];         // 上次下发到 0x607A 的值
+                                static uint8_t csp_inited[NUM_SLAVES] = {0};    // 是否已初始化
+                                // 定点限速累计器：解决 v_max < FREQUENCY 时整除为 0 的问题
+                                static int64_t vmax_acc[NUM_SLAVES] = {0};      // 单位：counts/s 累加器
+
+                                // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
+                                if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
+                                    csp_cmd_pos[i] = pv;       // 先对齐
+                                    vmax_acc[i]     = 0;
+                                    csp_inited[i]   = 1;
+                                }
+
+                                // 期望的“最终绝对目标”
+                                const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);
+
+                                // std::cout << "goal: " << goal << std::endl;
+
+                                // 本周期允许的最大步长（counts/周期），用定点累加保证平均速度上限严格等于 CSP_MAX_VEL_COUNTS_PER_S
+                                vmax_acc[i] += (int64_t)CSP_MAX_VEL_COUNTS_PER_S;     // +v [counts/s]
+                                int32_t max_step = (int32_t)(vmax_acc[i] / FREQUENCY); // 下取整，得到本周期可用步长
+                                vmax_acc[i] -= (int64_t)max_step * FREQUENCY;          // 保留余数到下周期
+
+                                // 相对“命令轨迹”的误差（不是相对 pv）
+                                const int32_t err_cmd = goal - csp_cmd_pos[i];
+
+                                // 死区：足够近就贴合到 goal
+                                if (err_cmd > -CSP_POS_DEADBAND && err_cmd < CSP_POS_DEADBAND) {
+                                    csp_cmd_pos[i] = goal;
+                                } else {
+                                    // 限速迈步：命令轨迹只按时间往 goal 逼近
+                                    if (err_cmd > 0)      csp_cmd_pos[i] += (err_cmd >  max_step) ?  max_step : err_cmd;
+                                    else                  csp_cmd_pos[i] += (err_cmd < -max_step) ? -max_step : err_cmd;
+                                }
+
+                                // 可选：跟随误差防护（避免 following error 过大时继续“加命令”）
+                                // const int32_t follow_err = csp_cmd_pos[i] - pv;
+                                // const int32_t FE_LIMIT =  (int32_t) (你的驱动跟随误差阈值); // 参考驱动参数
+                                // if (std::abs(follow_err) > FE_LIMIT) {
+                                //     // 例如：冻结或减半 max_step，给驱动时间追上
+                                // }
+
+                                // 下发“绝对目标”（不是增量）
+                                EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, csp_cmd_pos[i]);
+
+                                // 回显给上层（/joint_states_cmd 用）
+                                g_state.pos_cmd[i].store(csp_cmd_pos[i], std::memory_order_relaxed);
+                                break;
+                            }
+                            case CYCLIC_TORQUE:   // 10
+                                EC_WRITE_S16(domain1_pd + mt_offsets[i].target_torque, tor_cmd);
+                                break;
+                            default:
+                                // 未知模式：默认速度模式安全置 0
+                                //EC_WRITE_S32(domain1_pd + offsets[i].target_velocity, 0);
+                                break;
+                                
+                        }
+                    } else {
+                        // run_enable=false：清零目标
+                        EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);
+                        // CSV 安全：速度清零
+                        EC_WRITE_S32(domain1_pd + mt_offsets[i].target_velocity, 0);
+                        // CSP 安全：目标位置=当前位置 -> 立即“冻结”
+                        int32_t pv_hold = EC_READ_S32(domain1_pd + mt_offsets[i].position_actual_value);
+                        EC_WRITE_S32(domain1_pd + mt_offsets[i].target_position, pv_hold);
+                        // 可选：扭矩模式也清零（最好维持急停状态，待修改）
+                        EC_WRITE_S16(domain1_pd + mt_offsets[i].target_torque, 0);
+                        EC_WRITE_U16(domain1_pd + mt_offsets[i].ctrl_word, 0x0007);//切换 switched on 状态
+                    }
+                }
+            }
+        }
+        
+        if (sync_ref_counter) {
+            sync_ref_counter--;
+        } else {
+            sync_ref_counter = 1; // sync every 2 cycle
+
+            clock_gettime(CLOCK_TO_USE, &time);
+            ecrt_master_sync_reference_clock_to(master, TIMESPEC2NS(time)); //每隔一个周期同步时钟
+        }
+        ecrt_master_sync_slave_clocks(master);
+
+        // send process data
+        ecrt_domain_queue(domain1);     //排队域数据
+        ecrt_master_send(master);   //发送ETHERCAT帧
+
+#ifdef MEASURE_TIMING
+        clock_gettime(CLOCK_TO_USE, &endTime);
+#endif
+    }
+}
+// ------------------- 启动/停机接口 -------------------
+namespace ethercat_core {
+
+//启动实时线程
+static void start_rt_cyclic_thread(){   
+  std::thread([]{
+    // 锁内存 & 实时优先级
+    mlockall(MCL_CURRENT | MCL_FUTURE); //将当前进程使用的所有内存（MCL_CURRENT）以及将来可能分配的所有内存（MCL_FUTURE）都锁定在物理 RAM 中，禁止将其交换到磁盘（Swap）
+    sched_param param{}; param.sched_priority = 90 ; //sched_get_priority_max(SCHED_FIFO); //; //将当前线程的调度策略设置为 SCHED_FIFO (First-In, First-Out)，并赋予最高优先级
+    pthread_setschedparam(pthread_self(), SCHED_FIFO, &param);
+    cyclic_task(); // 进入控制循环（阻塞）
+  }).detach();
+}
+
+/**
+ * 初始化 EtherCAT master / domain / PDO / DC 并启动实时循环
+ * return true 成功；false 失败（会复位 g_started 标志）
+ *
+ * 步骤：
+ *  1) ecrt_request_master / ecrt_master_create_domain；
+ *  2) 根据 use_pvt 选择配置 device_syncs 或 device_syncs_pvt；
+ *  3) ecrt_slave_config_dc 启用 DC 同步；
+ *  4) ecrt_domain_reg_pdo_entry_list 注册 PDO entry，保存 offsets[];
+ *  5) ecrt_master_activate + ecrt_domain_data 获取 process image 指针；
+ *  6) 设定上层默认模式（PVT 或 CSV），拉起 run_enable；
+ *  7) 启动实时线程。
+ */
+bool start()
+{
+    printf("Starting EtherCAT master...\n");
+    if (g_started.exchange(true)) {
+        // 已经启动
+        return true;
+    }
+    // 申请 master / 域
+    master = ecrt_request_master(0);
+    if (!master) { 
+        perror("ecrt_request_master"); 
+        g_started.store(false); return false; 
+    }
+    domain1 = ecrt_master_create_domain(master);
+    if (!domain1){ 
+        perror("ecrt_master_create_domain"); 
+        g_started.store(false); return false; 
+    }    
+
+    // 从站配置
+    for (int i = 0; i < NUM_SLAVES; ++i) {
+
+        std::cout << "Configuring slave " << i << "..." << std::endl;
+        
+        int position = i+1;
+
+        // if (i > 0)
+        // {
+        //     position = i+2;
+        // }
+
+        if (i == 0)
+        {
+            sc[i] = ecrt_master_slave_config(master, 0, position, ZER_VID_PID);
+            if (!sc[i]) 
+            { 
+                std::cout << "Failed slave cfg at pos " << i << std::endl;
+                // fprintf(stderr,"Failed slave cfg at pos %d\n", i); g_started.store(false); return false; 
+            }
+
+            if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs)) 
+            {
+                std::cout << "Failed PDO config " << i << std::endl;
+                //   fprintf(stderr,"[S%02d] Failed PDO config\n", i); g_started.store(false); return false;
+            }
+        }
+        else{
+
+            sc[i] = ecrt_master_slave_config(master, 0, position, MT_VID_PID);
+            if (!sc[i]) 
+            { 
+                std::cout << "Failed slave cfg at pos " << i << std::endl;
+            }
+
+            if (ecrt_slave_config_pdos(sc[i], EC_END, mt_device_syncs)) 
+            {
+                std::cout << "Failed PDO config " << i << std::endl;
+            }
+        }
+
+        // DC配置
+        ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 200, 0, 0);
+    }
+
+   
+    // 注册 PDO entry 偏移
+    for (int i = 0; i < NUM_SLAVES; ++i) {
+
+        int position = i+1;
+
+        // if (i > 1)
+        // {
+        //     position = i+2;
+        // }
+
+        if (i == 0)
+        {
+            // ---- CSP/CSV/CST PDO ----
+            const size_t N = sizeof(zer_domain1_regs)/sizeof(zer_domain1_regs[0]);
+            ec_pdo_entry_reg_t regs[N];
+            memcpy(regs, zer_domain1_regs, sizeof(zer_domain1_regs));
+            for (size_t j = 0; j + 1 < N; ++j) 
+            { 
+                regs[j].alias = 0; 
+                regs[j].position = position; 
+            }
+
+            regs[0].offset  = &zer_offsets[i].ctrl_word;
+            regs[1].offset  = &zer_offsets[i].target_position;
+            regs[2].offset  = &zer_offsets[i].target_velocity;
+            regs[3].offset  = &zer_offsets[i].target_torque;
+            regs[4].offset  = &zer_offsets[i].max_torque;
+            regs[5].offset  = &zer_offsets[i].operation_mode;
+            regs[6].offset  = &zer_offsets[i].reserved1;
+            regs[7].offset  = &zer_offsets[i].status_word;
+            regs[8].offset  = &zer_offsets[i].position_actual_value;
+            regs[9].offset  = &zer_offsets[i].velocity_actual_value;
+            regs[10].offset = &zer_offsets[i].torque_actual_value;
+            regs[11].offset = &zer_offsets[i].error_code;
+            regs[12].offset = &zer_offsets[i].modes_of_operation_display;
+            regs[13].offset = &zer_offsets[i].reserved2;
+
+            if (ecrt_domain_reg_pdo_entry_list(domain1, regs)) {
+                fprintf(stderr, "[S%02d] PDO entry reg failed\n", i);
+                g_started.store(false); return false;
+            }
+        }else{
+            // ---- CSP/CSV/CST PDO ----
+            const size_t N = sizeof(mt_domain1_regs)/sizeof(mt_domain1_regs[0]);
+            ec_pdo_entry_reg_t regs[N];
+            memcpy(regs, mt_domain1_regs, sizeof(mt_domain1_regs));
+            for (size_t j = 0; j + 1 < N; ++j) 
+            { 
+                regs[j].alias = 0; 
+                regs[j].position = position; 
+            }
+
+            regs[0].offset  = &mt_offsets[i].ctrl_word;
+            regs[1].offset  = &mt_offsets[i].target_position;
+            regs[2].offset  = &mt_offsets[i].target_velocity;
+            regs[3].offset  = &mt_offsets[i].target_torque;
+            regs[4].offset  = &mt_offsets[i].max_torque;
+            regs[5].offset  = &mt_offsets[i].operation_mode;
+            regs[6].offset  = &mt_offsets[i].reserved1;
+            regs[7].offset  = &mt_offsets[i].status_word;
+            regs[8].offset  = &mt_offsets[i].position_actual_value;
+            regs[9].offset  = &mt_offsets[i].velocity_actual_value;
+            regs[10].offset = &mt_offsets[i].torque_actual_value;
+            regs[11].offset = &mt_offsets[i].error_code;
+            regs[12].offset = &mt_offsets[i].modes_of_operation_display;
+            regs[13].offset = &mt_offsets[i].reserved2;
+
+            if (ecrt_domain_reg_pdo_entry_list(domain1, regs)) {
+                fprintf(stderr, "[S%02d] PDO entry reg failed\n", i);
+                g_started.store(false); return false;
+            }
+        }
+    }
+
+    // 5) 激活 & 取 process image 指针
+    if (ecrt_master_activate(master)) { perror("ecrt_master_activate"); g_started.store(false); return false; }
+    domain1_pd = ecrt_domain_data(domain1);
+    if (!domain1_pd) 
+    { 
+        fprintf(stderr,"domain1_pd null\n"); 
+        g_started.store(false); 
+        return false; 
+    }
+
+    std::cout << "Starting EtherCAT master done." << std::endl;
+
+    mc_init_defaults(OpMode::CSP);
+
+    mc_enable_all(true);
+    // mc_set_all_target_velocity(0);
+
+    // 启动实时循环线程
+    start_rt_cyclic_thread();
+    return true;
+}
+
+void stop_soft(){
+    // 软停：拉低 run_enable；保持 S7 switched on，速度清零
+    mc_enable_all(false);
+}
+void request_exit(){
+    g_run.store(false);                  // 让循环跳出
+}
+}    // namespace ethercat_core

ethercat_dev/src/ethercat_control/src/ethercat_csp_test.cpp

@@ -0,0 +1,166 @@
+#include <rclcpp/rclcpp.hpp>
+#include <std_msgs/msg/string.hpp>
+#include <sstream>
+#include <iomanip>
+#include <cmath>
+#include <chrono>
+#include <string>
+#include <vector>
+
+using namespace std::chrono_literals;
+
+class PosRampNode : public rclcpp::Node {
+public:
+  PosRampNode() : Node("pos_ramp_node"),
+                  steady_clock_(RCL_STEADY_TIME)
+  {
+    // 参数
+    declare_parameter<int>("num_joints", 6);                    // 轴数
+    declare_parameter<int>("start_pos", 0);                     // 起始位置（计数）
+    declare_parameter<int>("end_pos", 2000);                    // 终点位置（计数）
+    declare_parameter<double>("v_max", 8000.0);                 // 最大速度（counts/s）
+    declare_parameter<double>("a_max", 20000.0);                // 最大加速度（counts/s^2）
+    declare_parameter<double>("pause_sec", 5.0);                // 暂停时长（s）
+    declare_parameter<int>("rate_hz", 100);                     // 发布频率（Hz）
+
+    get_parameter("num_joints", num_joints_);
+    get_parameter("start_pos", start_pos_);
+    get_parameter("end_pos", end_pos_);
+    get_parameter("v_max", v_max_);
+    get_parameter("a_max", a_max_);
+    get_parameter("pause_sec", pause_sec_);
+    get_parameter("rate_hz", rate_hz_);
+
+    if (rate_hz_ <= 0) rate_hz_ = 100;
+    dt_ = 1.0 / static_cast<double>(rate_hz_);
+
+    pub_ = create_publisher<std_msgs::msg::String>("ethercat/set", 10);
+
+    // 轨迹状态初始化
+    phase_ = Phase::MoveOut;
+    pos_   = static_cast<double>(start_pos_);
+    vel_   = 0.0;
+    goal_  = static_cast<double>(end_pos_);
+    last_  = steady_clock_.now();
+
+    timer_ = create_wall_timer(
+      std::chrono::milliseconds(static_cast<int>(1000.0 / rate_hz_)),
+      std::bind(&PosRampNode::onTimer, this));
+
+    RCLCPP_INFO(get_logger(),
+      "pos_ramp_node started: num_joints=%d, start=%d, end=%d, v_max=%.1f, a_max=%.1f, pause=%.1fs, rate=%d Hz",
+      num_joints_, start_pos_, end_pos_, v_max_, a_max_, pause_sec_, rate_hz_);
+  }
+
+private:
+  enum class Phase { MoveOut, HoldOut, MoveBack, HoldStart };
+
+  void onTimer() {
+    // 用稳态时钟计算 dt，尽量跟墙钟定时器保持一致
+    const rclcpp::Time now = steady_clock_.now();
+    double dt = (now - last_).seconds();
+    last_ = now;
+    if (dt <= 0.0) dt = dt_;     // 回退到标称步长
+    // 为了轨迹稳定，限制 dt 的异常尖峰
+    if (dt > 3 * dt_) dt = dt_;
+
+    switch (phase_) {
+      case Phase::MoveOut:   stepMotion(+1, dt, static_cast<double>(end_pos_), Phase::HoldOut);   break;
+      case Phase::HoldOut:   stepHold(dt, Phase::MoveBack, static_cast<double>(start_pos_));      break;
+      case Phase::MoveBack:  stepMotion(-1, dt, static_cast<double>(start_pos_), Phase::HoldStart);break;
+      case Phase::HoldStart: stepHold(dt, Phase::MoveOut, static_cast<double>(end_pos_));         break;
+    }
+
+    // 发布到 ethercat/set：对所有轴发相同位置
+    const int pos_i = static_cast<int>(std::llround(pos_));
+    std::ostringstream ss;
+    for (int i = 0; i < num_joints_; ++i) {
+      if (i) ss << "; ";
+      ss << i << " pos " << pos_i;
+    }
+    std_msgs::msg::String msg;
+    msg.data = ss.str();
+    pub_->publish(msg);
+  }
+
+  // 运动步进：梯形速度（加速/匀速/减速），避免超调，平滑到达 goal
+  void stepMotion(int dir, double dt, double goal_target, Phase next_hold_phase) {
+    const double s_rem = (goal_target - pos_);          // 剩余位移
+    const double sign  = (dir >= 0) ? 1.0 : -1.0;
+    const double a     = a_max_ * sign;
+
+    // 需要的减速距离 d = v^2 / (2a_max)
+    const double d_decel = (vel_*vel_) / (2.0 * a_max_ + 1e-9);
+
+    // 决策：加速还是减速（或保持）
+    if (sign * s_rem <= d_decel) {
+      // 该减速了
+      vel_ -= a_max_ * sign * dt;
+      // 过零保护
+      if (sign * vel_ < 0.0) vel_ = 0.0;
+    } else {
+      // 加速直到 v_max
+      vel_ += a * dt;
+      const double v_lim = v_max_;
+      if (std::fabs(vel_) > v_lim) vel_ = v_lim * sign;
+    }
+
+    // 积分位置
+    pos_ += vel_ * dt;
+
+    // 过冲保护：若越过目标，截断到目标并进入下一相位
+    if ((sign > 0.0 && pos_ >= goal_target) ||
+        (sign < 0.0 && pos_ <= goal_target)) {
+      pos_ = goal_target;
+      vel_ = 0.0;
+      phase_ = next_hold_phase;
+      hold_t_ = 0.0;
+      goal_   = (next_hold_phase == Phase::HoldOut)
+                  ? static_cast<double>(end_pos_)
+                  : static_cast<double>(start_pos_);
+      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,
+                           "Reached %.3f, entering hold.", goal_target);
+    }
+  }
+
+  // 暂停相位：计时到 pause_sec_ 后切换到下一运动相位，并设置新目标
+  void stepHold(double dt, Phase next_move_phase, double next_goal) {
+    hold_t_ += dt;
+    pos_     = goal_;   // 保持
+    vel_     = 0.0;
+    if (hold_t_ >= pause_sec_) {
+      phase_ = next_move_phase;
+      goal_  = next_goal;
+      RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 1000,
+                           "Leaving hold, next goal=%.3f", next_goal);
+    }
+  }
+
+  // ---- members ----
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  rclcpp::Clock steady_clock_;
+  rclcpp::Time  last_;
+
+  int   num_joints_{6};
+  int   start_pos_{0};
+  int   end_pos_{2000};
+  int   rate_hz_{100};
+  double v_max_{8000.0};
+  double a_max_{20000.0};
+  double pause_sec_{5.0};
+  double dt_{0.01};
+
+  Phase  phase_{Phase::MoveOut};
+  double pos_{0.0};
+  double vel_{0.0};
+  double goal_{0.0};
+  double hold_t_{0.0};
+};
+
+int main(int argc, char **argv) {
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<PosRampNode>());
+  rclcpp::shutdown();
+  return 0;
+}

ethercat_dev/src/ethercat_control/src/ethercat_node.cpp

@@ -0,0 +1,420 @@
+/**
+ * ROS2 节点：与 EtherCAT 周期线程共享指令/状态，提供文本话题指令解析、
+ * JointState 发布、CSP 指令回显与 CSV 记录、超时看门狗等。
+ *
+ * 主要功能：
+ *  - 启动底层 EtherCAT 周期线程（见 ethercat_core::start）
+ *  - 订阅字符串命令话题，解析并写入 MotorCommand（g_cmd）
+ *  - 周期发布：
+ *      * joint_states（实际位置/速度/扭矩）
+ *      * joint_states_cmd（CSP 目标位置的回显）
+ *      * csp_table（时间戳+每轴的 csp_cmd/real/err）
+ *  - 将表格同时写入 CSV 文件
+ *  - 看门狗：一段时间未收到上层命令则将速度/扭矩清零
+ *
+ * 使用示例：
+ *  - 切到 CSP 并给 0 号轴位置：
+ *      ros2 topic pub /ethercat/set std_msgs/msg/String "data: '0 pos 10000'"
+ *  - 切到 CSV 并给 0 号轴速度：
+ *      ros2 topic pub /ethercat/set std_msgs/msg/String "data: '0 vel 2000'"
+ *  - PVT 模式设置与给值（多条用分号隔开）：
+ *      ros2 topic pub --rate 10 /ethercat/set std_msgs/msg/String \
+ *        "data: '0 pvt; 0 kp 5; 0 kd 5; 0 pvtpos 10000; 0 pvtvel 2000; 0 pvttor 0'"
+ */
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+#include "sensor_msgs/msg/joint_state.hpp"
+#include "ethercat_control/ethercat_core.hpp"
+#include "ethercat_control/motor_control.hpp"
+#include <sstream>
+#include <cmath>
+#include <fstream>
+#include <iomanip>
+#include <filesystem>
+
+using std::placeholders::_1;
+
+class EthercatNode : public rclcpp::Node {
+public:
+  /**
+   * 构造函数：初始化 EtherCAT、参数、话题与定时器
+   *
+   * 过程：
+   *  1) 调用 ethercat_core::start() 启动底层主站与 1kHz 周期线程
+   *  2) 读取参数 num_joints/csv_path
+   *  3) 创建订阅（/ethercat/set）与发布者（joint_states / joint_states_cmd / csp_table）
+   *  4) 创建 100Hz 的状态发布定时器
+   *  5) 打开 CSV 并写表头
+   *  6) 启动看门狗定时器（100ms 检查一次）
+   *
+   */
+  EthercatNode() : Node("ethercat_node") {
+    // 启动 EtherCAT 周期线程
+    if (!ethercat_core::start()) {
+      RCLCPP_FATAL(this->get_logger(), "EtherCAT core start failed.");
+      throw std::runtime_error("ethercat start failed");
+    }
+
+    // 关节名（给 JointState 用）
+    declare_parameter<int>("num_joints", NUM_SLAVES);
+    get_parameter("num_joints", num_joints_);
+    joint_names_.resize(num_joints_);
+    for (int i = 0; i < num_joints_; ++i) {
+      joint_names_[i] = "joint_" + std::to_string(i);
+    }
+
+    // CSV 文件路径参数，默认存到工作目录下
+    declare_parameter<std::string>("csv_path", "csp_log.csv");
+    get_parameter("csv_path", csv_path_);
+
+    // 订阅 set 文本命令：例如 "set 0 vel 1000; 1 pos 3000"
+    set_sub_ = create_subscription<std_msgs::msg::String>(
+        "ethercat/set", 10, std::bind(&EthercatNode::onSet, this, _1));
+
+    // 发布 JointState（100Hz）
+    js_pub_ = create_publisher<sensor_msgs::msg::JointState>("ec_joint_states", 10);
+    js_cmd_pub_ = create_publisher<sensor_msgs::msg::JointState>("ec_joint_states_cmd", 10);
+
+    // 表格话题，把“轴i csp指令值/真实位置/误差”拼成一行
+    csp_table_pub_ = create_publisher<std_msgs::msg::String>("csp_table", 10);
+
+    // 定时器:每 100ms 发布一次状态
+    js_timer_ = create_wall_timer(
+        std::chrono::milliseconds(100),
+        std::bind(&EthercatNode::publishJointState, this));
+
+    // 打开 CSV 并写表头
+    // open_csv_and_write_header();
+    start_time_steady_ = steady_clock_.now();
+
+    RCLCPP_INFO(get_logger(), "ethercat_node ready.");
+
+    // 看门狗，每100ms检测指令是否收到
+    last_command_time_ = this->get_clock()->now();
+    watchdog_timer_ = this->create_wall_timer(
+        std::chrono::milliseconds(100),
+        std::bind(&EthercatNode::watchdogCallback, this));
+  }
+
+  // 析构时收尾
+  ~EthercatNode() override {
+    // if (csv_.is_open()) {
+    //   csv_.flush();
+    //   csv_.close();
+    // }
+  }
+
+private:
+  void onSet(const std_msgs::msg::String::SharedPtr msg) {
+  last_command_time_ = this->get_clock()->now();  //刷新看门狗时间
+  // 对读到的控制指令进行处理
+  // 简易去空白工具
+  auto ltrim=[](std::string& s){ auto p=s.find_first_not_of(" \t"); if(p!=std::string::npos) s.erase(0,p); };
+  auto rtrim=[](std::string& s){ auto p=s.find_last_not_of(" \t");  if(p!=std::string::npos) s.erase(p+1); };
+
+  std::stringstream ss(msg->data);       //整条命令
+  std::string chunk;                     //单条命令（整条命令切割而来）
+
+  while (std::getline(ss, chunk, ';')) { //将指令用；分割
+    ltrim(chunk); rtrim(chunk);
+    if (chunk.empty()) continue;         //空段跳过
+
+    std::istringstream si(chunk);
+    int idx = -1;                        // 轴号
+    std::string kind;                    // 操作类型
+    std::string vstr;                    // 操作值
+
+    si >> idx >> kind;  //第一个token为轴号，第二个token为操作类型
+    if (!si || !mc_valid_index(idx)) {
+      // RCLCPP_WARN(get_logger(), "[set] bad: '%s'", chunk.c_str());
+      continue;
+    }
+    // 第三个Token为参数值
+    if (!(si >> vstr)) vstr.clear();
+
+    // 转换函数 字符串转换为long long和double形式
+    auto to_ll = [](const std::string& s)->long long {
+      try { size_t pos=0; long long v = std::stoll(s, &pos, 0); (void)pos; return v; }
+      catch (...) { return 0LL; }
+    };
+    auto to_d = [](const std::string& s)->double {
+      try { size_t pos=0; double v = std::stod(s, &pos); (void)pos; return v; }
+      catch (...) { return 0.0; }
+    };
+
+    // ======= PVT 指令解析处理 =======
+    if (kind == "pvt") {
+      mc_set_mode(idx, OpMode::PVT);
+      RCLCPP_INFO(get_logger(), "[S%d] switch to PVT mode", idx);
+      continue;
+    }
+    else if (kind == "kp" || kind == "pvt_kp") {
+      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] kp needs a value", idx); continue; }
+      const double kp = to_d(vstr);
+      // 取当前 kd，避免把 kd 覆盖没了
+      const double kd_cur = g_cmd.pvt_kd[idx].load(std::memory_order_relaxed);
+      mc_set_pvt_gains(idx, kp, kd_cur);
+      RCLCPP_INFO(get_logger(), "[S%d] PVT_KP=%.6f (KD=%.6f keep)", idx, kp, kd_cur);
+      continue;
+    }
+    else if (kind == "kd" || kind == "pvt_kd") {
+      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] kd needs a value", idx); continue; }
+      const double kd = to_d(vstr);
+      // 取当前 kp，避免把 kp 覆盖没了
+      const double kp_cur = g_cmd.pvt_kp[idx].load(std::memory_order_relaxed);
+      mc_set_pvt_gains(idx, kp_cur, kd);
+      RCLCPP_INFO(get_logger(), "[S%d] PVT_KD=%.6f (KP=%.6f keep)", idx, kd, kp_cur);
+      continue;
+    }
+    // 在 PVT 模式下下发目标（为了简单，提供带前缀的别名）
+    else if (kind == "pvtpos" || kind == "pvt_pos") {
+      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvtpos needs a value", idx); continue; }
+      mc_set_mode(idx, OpMode::PVT);
+      mc_set_target_position(idx, static_cast<int32_t>(to_ll(vstr)));
+      RCLCPP_INFO(get_logger(), "[S%d] PVT pos=%s", idx, vstr.c_str());
+      continue;
+    }
+    else if (kind == "pvtvel" || kind == "pvt_vel") {
+      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvtvel needs a value", idx); continue; }
+      mc_set_mode(idx, OpMode::PVT);
+      mc_set_target_velocity(idx, static_cast<int32_t>(to_ll(vstr)));
+      RCLCPP_INFO(get_logger(), "[S%d] PVT vel=%s", idx, vstr.c_str());
+      continue;
+    }
+    else if (kind == "pvttor" || kind == "pvt_tor") {
+      if (vstr.empty()) { RCLCPP_WARN(get_logger(), "[S%d] pvttor needs a value", idx); continue; }
+      mc_set_mode(idx, OpMode::PVT);
+      mc_set_target_torque(idx, static_cast<int16_t>(to_ll(vstr)));
+      RCLCPP_INFO(get_logger(), "[S%d] PVT tor=%s", idx, vstr.c_str());
+      continue;
+    }
+
+    // ======= CSP / CSV / CST 指令解析处理 =======
+    if (kind=="pos" || kind=="position") {
+      if (vstr.empty()) { 
+        RCLCPP_WARN(get_logger(), "[S%d] pos needs a value; ignored", idx);
+        continue;
+      }
+
+      // switch (idx)
+      // {
+      // case 0:
+      //   idx = 2;
+      //   break;
+      // case 1:
+      //   idx = 3;
+      //   break;
+      // case 2:
+      //   idx = 0;
+      //   break;
+      // case 3:
+      //   idx = 1;
+      //   break;
+      // case 4:
+      //   idx = 5;
+      //   break;
+      // case 5:
+      //   idx = 4;
+      //   break;
+      // default:
+      //   break;
+      // }
+
+      mc_set_mode(idx, OpMode::CSP);
+      mc_set_target_position(idx, static_cast<int32_t>(to_ll(vstr)));
+      // RCLCPP_INFO(get_logger(), "[S%d] CSP pos=%s", idx, vstr.c_str());
+    } else if (kind=="vel" || kind=="velocity" || kind=="speed") {
+      if (vstr.empty()) { 
+        RCLCPP_WARN(get_logger(), "[S%d] vel needs a value; ignored", idx);
+        continue;
+      }
+      mc_set_mode(idx, OpMode::CSV);
+      mc_set_target_velocity(idx, static_cast<int32_t>(to_ll(vstr)));
+      RCLCPP_INFO(get_logger(), "[S%d] CSV vel=%s", idx, vstr.c_str());
+    } else if (kind=="tor" || kind=="torque") {
+      if (vstr.empty()) { 
+        RCLCPP_WARN(get_logger(), "[S%d] tor needs a value; ignored", idx);
+        continue;
+      }
+      mc_set_mode(idx, OpMode::CST);
+      mc_set_target_torque(idx, static_cast<int16_t>(to_ll(vstr)));
+      RCLCPP_INFO(get_logger(), "[S%d] CST tor=%s", idx, vstr.c_str());
+    } else if (kind=="ena" || kind=="enable") {
+      if (vstr.empty()) { 
+        RCLCPP_WARN(get_logger(), "[S%d] tor needs a value; ignored", idx);
+        continue;
+      }
+      mc_enable_run(idx, to_ll(vstr));
+      RCLCPP_INFO(get_logger(), "[S%d] enable=%s", idx, vstr.c_str());
+    }else {
+      RCLCPP_WARN(get_logger(), "[set] unknown field: '%s'", kind.c_str());
+    }
+  }
+}
+
+  // 计时：以节点启动时为零，使用稳态时钟
+  rclcpp::Clock steady_clock_{RCL_STEADY_TIME};     
+  rclcpp::Time  start_time_steady_{0, 0, RCL_STEADY_TIME}; 
+
+  // 关节状态发布（实际值和命令值）
+  void publishJointState() {
+    // 实际值
+    sensor_msgs::msg::JointState js;
+    js.header.stamp = now();
+    js.name = joint_names_;
+    js.position.resize(num_joints_);
+    js.velocity.resize(num_joints_);
+    js.effort.resize(num_joints_);
+    for (int i = 0; i < num_joints_; ++i) {
+      uint16_t sw; int32_t pos, vel; int16_t tq;
+      mc_get_state(i, sw, pos, vel, tq);
+      js.position[i] = static_cast<double>(pos);
+      js.velocity[i] = static_cast<double>(vel);
+      js.effort[i]   = static_cast<double>(tq);
+    }
+    js_pub_->publish(js);
+
+    // 命令位置（CSP 回显），与实际值用同一时间戳
+    sensor_msgs::msg::JointState js_cmd;
+    js_cmd.header.stamp = js.header.stamp;
+    js_cmd.name = joint_names_;
+    js_cmd.position.resize(num_joints_);
+    for (int i = 0; i < num_joints_; ++i) {
+      const int32_t pc = g_state.pos_cmd[i].load(std::memory_order_relaxed); // 读取csp值
+      js_cmd.position[i] = static_cast<double>(pc);
+    }
+    js_cmd_pub_->publish(js_cmd);
+
+    // 计算相对启动的时间（ms），并量化到 0.1 ms
+    const rclcpp::Duration dt = steady_clock_.now() - start_time_steady_;
+    double t_ms = dt.seconds() * 1000.0;
+    t_ms = std::round(t_ms * 10.0) / 10.0;  // 四舍五入到 0.1ms
+
+    // 生成“表格行”：timestamp,轴1csp指令值,轴1真实位置,误差,轴2csp指令值,轴2真实位置,误差,...
+    std::ostringstream row;
+    row.setf(std::ios::fixed);
+    // 先写时间戳
+    row << std::setprecision(1) << t_ms;
+
+    if (num_joints_ > 0) row << ",";       
+    for (int i = 0; i < num_joints_; ++i) {
+      const long long csp = static_cast<long long>(js_cmd.position[i]);
+      const long long act = static_cast<long long>(js.position[i]);
+      const long long err = csp - act;
+      row << csp << ',' << act << ',' << err;
+      if (i != num_joints_ - 1) row << ','; 
+    }
+    const std::string line = row.str();
+
+    // 发布到 csp_table 话题
+    std_msgs::msg::String msg;
+    msg.data = line;
+    csp_table_pub_->publish(msg);
+
+    // 写入 CSV（首行已写表头）
+    // if (csv_.is_open()) {
+    //   csv_ << line << '\n';
+    //   if ((++csv_lines_ % 100) == 0) csv_.flush(); // 每100行刷一次盘，减轻开销
+    // }
+  }
+
+  //  打开 CSV 并写表头
+  void open_csv_and_write_header() {
+    try {
+      std::filesystem::path p(csv_path_);
+      if (p.has_parent_path()) {
+        std::error_code ec;
+        std::filesystem::create_directories(p.parent_path(), ec);
+        if (ec) {
+          RCLCPP_WARN(get_logger(), "Failed to create dir for CSV: %s", ec.message().c_str());
+        }
+      }
+      csv_.open(csv_path_, std::ios::out | std::ios::trunc);
+      if (!csv_) {
+        RCLCPP_ERROR(get_logger(), "Cannot open CSV file: %s", csv_path_.c_str());
+        return;
+      }
+      // 表头：轴1csp指令值,轴1真实位置,误差,轴2csp指令值,...
+      std::ostringstream header;
+      header << "time(ms)";
+      if (num_joints_ > 0) header << ",";  // 后面还有列再加逗号
+      for (int i = 0; i < num_joints_; ++i) {
+        header << "axis" << (i+1) << "csp_command"
+               << "," << "axis" << (i+1) << "real_position"
+               << "," << "error";
+        if (i != num_joints_ - 1) header << ",";
+      }
+      csv_ << header.str() << '\n';
+      csv_.flush();
+
+      // 同步把表头也发到 topic
+      std_msgs::msg::String head_msg;
+      head_msg.data = header.str();
+      csp_table_pub_->publish(head_msg);
+
+      RCLCPP_INFO(get_logger(), "Logging CSP table to: %s", csv_path_.c_str());
+    } catch (const std::exception& e) {
+      RCLCPP_ERROR(get_logger(), "open_csv_and_write_header exception: %s", e.what());
+    }
+  }
+
+  int num_joints_{0};
+  std::vector<std::string> joint_names_;
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr set_sub_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr js_pub_;
+  rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr js_cmd_pub_;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr csp_table_pub_; // NEW
+  rclcpp::TimerBase::SharedPtr js_timer_;
+  rclcpp::TimerBase::SharedPtr watchdog_timer_;
+  rclcpp::Time last_command_time_;
+
+  // CSV
+  std::string csv_path_;
+  std::ofstream csv_;
+  size_t csv_lines_{0};
+  
+  //看门狗
+  void watchdogCallback() {
+    const rclcpp::Time now = this->get_clock()->now();
+    const rclcpp::Duration dt = now - last_command_time_;
+    if (dt.seconds() > 0.2) {
+      //RCLCPP_WARN(get_logger(), "Command timeout! Stopping all motors."); //看门狗超时信息
+      for (int i = 0; i < num_joints_; ++i) {
+        mc_set_target_velocity(i, 0);
+        mc_set_target_torque(i, 0);
+      }
+      last_command_time_ = now;
+    }
+  }
+};
+
+int main(int argc, char** argv){
+  rclcpp::init(argc, argv);
+  // auto node = rclcpp::Node::make_shared("ethercat_node");
+  // int thread_priority = node->declare_parameter("thread_priority",50);
+  // pthread_t thread = pthread_self();
+
+  // struct sched_param param;
+
+  // param.sched_priority = thread_priority;
+  // int ret = pthread_setschedparam(thread,SCHED_FIFO,&param);
+  // if (ret != 0)
+  // {
+  //   RCLCPP_ERROR(node->get_logger(),"Failed to set thread priority : %s", strerror(ret));
+  // }
+  // else
+  // {
+  //   RCLCPP_INFO(node->get_logger(),"thread priority set to : %s", thread_priority);
+  // }
+  
+  
+
+  rclcpp::on_shutdown([]{
+    ethercat_core::stop_soft();
+    std::this_thread::sleep_for(std::chrono::milliseconds(50));
+    ethercat_core::request_exit();
+  });
+  rclcpp::spin(std::make_shared<EthercatNode>());
+  rclcpp::shutdown();
+  return 0;
+}

ethercat_dev/src/ethercat_control/src/ethercat_topic_test.cpp

@@ -0,0 +1,157 @@
+#include <chrono>
+#include <cmath>
+#include <memory>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <algorithm>
+
+#include "rclcpp/rclcpp.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+
+class EthercatVelLoop : public rclcpp::Node
+{
+public:
+  EthercatVelLoop() : Node("ethercat_vel_loop")
+  {
+    // 参数
+    topic_ = declare_parameter<std::string>("topic", "/ethercat/set");
+    max_vel_ = declare_parameter<int>("max_vel", 30000);
+
+    // 兼容旧参数名 forward_duration_sec（如果外部还在用）
+    double forward_duration = declare_parameter<double>("forward_duration_sec", 3.0);
+    ramp_duration_ = declare_parameter<double>("ramp_duration_sec", forward_duration);
+
+    decel_duration_ = declare_parameter<double>("decel_duration_sec", 1.0);
+    stop_duration_  = declare_parameter<double>("stop_duration_sec", 1.0);
+    rate_hz_        = declare_parameter<double>("rate_hz", 10.0);
+    motor_ids_      = declare_parameter<std::vector<int64_t>>("motor_ids", {0, 1, 2});
+
+    if (rate_hz_ <= 0.0) {
+      RCLCPP_WARN(get_logger(), "rate_hz <= 0, force to 10 Hz");
+      rate_hz_ = 10.0;
+    }
+
+    pub_ = create_publisher<std_msgs::msg::String>(topic_, 10);
+
+    timer_period_ = std::chrono::duration<double>(1.0 / rate_hz_);
+    timer_ = create_wall_timer(
+      std::chrono::duration_cast<std::chrono::milliseconds>(timer_period_),
+      std::bind(&EthercatVelLoop::onTimer, this));
+
+    RCLCPP_INFO(get_logger(),
+      "Started ethercat_vel_loop: topic=%s, max_vel=%d, ramp=%.3fs, decel=%.3fs, stop=%.3fs, rate=%.1fHz",
+      topic_.c_str(), max_vel_, ramp_duration_, decel_duration_, stop_duration_, rate_hz_);
+  }
+
+private:
+  enum class Phase { FWD_ACCEL, FWD_DECEL, STOP1, REV_ACCEL, REV_DECEL, STOP2 };
+
+  void onTimer()
+  {
+    time_in_phase_ += std::chrono::duration<double>(timer_period_).count();
+
+    int target_vel = 0;
+
+    switch (phase_) {
+      case Phase::FWD_ACCEL: {
+        // 0 -> +max_vel_，线性加速，持续 ramp_duration_
+        double frac = ramp_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / ramp_duration_) : 1.0;
+        target_vel = static_cast<int>(std::round(max_vel_ * frac));
+        if (time_in_phase_ >= ramp_duration_) {
+          // 加速完成后：如果需要减速缓冲，进入减速段，否则直接停
+          if (decel_duration_ > 0.0) switchPhase(Phase::FWD_DECEL);
+          else                       switchPhase(Phase::STOP1);
+        }
+        break;
+      }
+      case Phase::FWD_DECEL: {
+        // +max_vel_ -> 0，线性减速，持续 decel_duration_
+        double frac = decel_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / decel_duration_) : 1.0;
+        target_vel = static_cast<int>(std::round(max_vel_ * (1.0 - frac)));
+        if (time_in_phase_ >= decel_duration_) {
+          switchPhase(Phase::STOP1);
+        }
+        break;
+      }
+      case Phase::STOP1: {
+        target_vel = 0;
+        if (time_in_phase_ >= stop_duration_) {
+          switchPhase(Phase::REV_ACCEL);
+        }
+        break;
+      }
+      case Phase::REV_ACCEL: {
+        // 0 -> -max_vel_，线性加速，持续 ramp_duration_
+        double frac = ramp_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / ramp_duration_) : 1.0;
+        target_vel = -static_cast<int>(std::round(max_vel_ * frac));
+        if (time_in_phase_ >= ramp_duration_) {
+          if (decel_duration_ > 0.0) switchPhase(Phase::REV_DECEL);
+          else                       switchPhase(Phase::STOP2);
+        }
+        break;
+      }
+      case Phase::REV_DECEL: {
+        // -max_vel_ -> 0，线性减速，持续 decel_duration_
+        double frac = decel_duration_ > 0.0 ? std::min(1.0, time_in_phase_ / decel_duration_) : 1.0;
+        target_vel = -static_cast<int>(std::round(max_vel_ * (1.0 - frac)));
+        if (time_in_phase_ >= decel_duration_) {
+          switchPhase(Phase::STOP2);
+        }
+        break;
+      }
+      case Phase::STOP2: {
+        target_vel = 0;
+        if (time_in_phase_ >= stop_duration_) {
+          switchPhase(Phase::FWD_ACCEL);
+        }
+        break;
+      }
+    }
+
+    // 拼装字符串："0 vel XXX; 1 vel XXX; 2 vel XXX"
+    std::ostringstream oss;
+    for (size_t i = 0; i < motor_ids_.size(); ++i) {
+      if (i > 0) oss << "; ";
+      oss << motor_ids_[i] << " vel " << target_vel;
+    }
+
+    std_msgs::msg::String msg;
+    msg.data = oss.str();
+    pub_->publish(msg);
+  }
+
+  void switchPhase(Phase next)
+  {
+    phase_ = next;
+    time_in_phase_ = 0.0;
+  }
+
+  // 参数
+  std::string topic_;
+  int max_vel_;
+  double ramp_duration_;
+  double decel_duration_;
+  double stop_duration_;
+  double rate_hz_;
+  std::vector<int64_t> motor_ids_;
+
+  // 发布与定时
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
+  rclcpp::TimerBase::SharedPtr timer_;
+  std::chrono::duration<double> timer_period_;
+
+  // 状态机
+  Phase phase_{Phase::FWD_ACCEL};
+  double time_in_phase_{0.0};
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<EthercatVelLoop>());
+  rclcpp::shutdown();
+  return 0;
+}

ethercat_dev/src/ethercat_control/src/motor_control.cpp

@@ -0,0 +1,147 @@
+/**
+ * ROS2上位机和实时循环的通信桥梁
+ *
+ * 模块功能：
+ *  - 读取实时循环状态 g_state
+ *  - 给实时循环发送指令 g_cmd 
+ *
+ * 线程模型与并发：
+ *  - 多生产者（ROS2 回调/控制线程） --> 写 g_cmd / 读 g_state；
+ *  - 单消费者（1kHz 实时循环）      --> 读 g_cmd / 写 g_state。
+ *  - 全部原子变量使用 memory_order_relaxed，低延迟、低开销；
+ *
+ * 数据与单位约定（与从站字典保持一致）：
+ *  - 位置：编码器计数（counts）
+ *  - 速度：counts/s
+ *  - 扭矩：
+ *  - 模式：DS402 0x6060（CSP=8, CSV=9, CST=10, PVT=5）
+ *  - PVT 增益：这里以 double 表示的无量纲系数
+ *
+ */
+#include "ethercat_control/motor_control.hpp"
+
+MotorCommand g_cmd;   //全局命令缓冲
+MotorState   g_state; //全局状态缓冲
+
+static inline int clamp_index(int idx) { return std::clamp(idx, 0, NUM_SLAVES - 1); }
+
+// 初始化所有轴的缺省命令与状态
+void mc_init_defaults(OpMode default_mode) {
+  for (int i = 0; i < NUM_SLAVES; ++i) {
+    g_cmd.target_vel[i].store(0,          std::memory_order_relaxed);
+    g_cmd.target_pos[i].store(0,          std::memory_order_relaxed);
+    g_cmd.target_torque[i].store(0,       std::memory_order_relaxed);
+    g_cmd.mode[i].store(static_cast<int8_t>(default_mode), std::memory_order_relaxed);
+    g_cmd.run_enable[i].store(false,      std::memory_order_relaxed); 
+    g_cmd.fault_reset[i].store(false,     std::memory_order_relaxed);
+    g_cmd.quick_stop[i].store(false,      std::memory_order_relaxed);
+
+    g_state.status_word[i].store(0,       std::memory_order_relaxed);
+    g_state.pos_act[i].store(0,           std::memory_order_relaxed);
+    g_state.vel_act[i].store(0,           std::memory_order_relaxed);
+    g_state.torque_act[i].store(0,        std::memory_order_relaxed);
+    g_state.pos_cmd[i].store(0,           std::memory_order_relaxed);
+    // 新增：PVT 缺省增益（可按需改）
+    g_cmd.pvt_kp[i].store(0.0,            std::memory_order_relaxed);
+    g_cmd.pvt_kd[i].store(0.0,            std::memory_order_relaxed);
+
+  }
+}
+//设置工作模式（轴号，操作模式）
+void mc_set_mode(int idx, OpMode mode) {
+  if (!mc_valid_index(idx)) return;
+  g_cmd.mode[idx].store(static_cast<int8_t>(mode), std::memory_order_relaxed);
+}
+//设置速度（轴号，速度）
+void mc_set_target_velocity(int idx, int32_t vel) {
+  if (!mc_valid_index(idx)) return;
+  g_cmd.target_vel[idx].store(vel, std::memory_order_relaxed);
+}
+//设置位置（轴号，位置）
+void mc_set_target_position(int idx, int32_t pos) {
+  if (!mc_valid_index(idx)) return;
+  g_cmd.target_pos[idx].store(pos, std::memory_order_relaxed);
+}
+//设置扭矩（轴号，扭矩）
+void mc_set_target_torque(int idx, int16_t tq) {
+  if (!mc_valid_index(idx)) return;
+  g_cmd.target_torque[idx].store(tq, std::memory_order_relaxed);
+}
+// 设置所有电机PVT kp kd
+void mc_set_all_pvt_gains(double kp, double kd) {
+  for (int i = 0; i < NUM_SLAVES; ++i) {
+    g_cmd.pvt_kp[i].store(kp, std::memory_order_relaxed);
+    g_cmd.pvt_kd[i].store(kd, std::memory_order_relaxed);
+  }
+}
+// 设置PVT kp kd （轴号，kp，kd）
+void mc_set_pvt_gains(int axis, double kp, double kd) {
+  if (axis < 0 || axis >= NUM_SLAVES) return;
+  g_cmd.pvt_kp[axis].store(kp, std::memory_order_relaxed);
+  g_cmd.pvt_kd[axis].store(kd, std::memory_order_relaxed);
+}
+//使能/禁止运行（轴号）
+void mc_enable_run(int idx, bool enable) {
+  if (!mc_valid_index(idx)) return;
+  g_cmd.run_enable[idx].store(enable, std::memory_order_relaxed);
+}
+//设置所有电机运行模式（模式）
+void mc_set_all_mode(OpMode mode) {
+  for (int i = 0; i < NUM_SLAVES; ++i) mc_set_mode(i, mode);
+}
+//设置所有电机速度（速度）
+void mc_set_all_target_velocity(int32_t vel) {
+  for (int i = 0; i < NUM_SLAVES; ++i) mc_set_target_velocity(i, vel);
+}
+//批量使能/禁止运行
+void mc_enable_all(bool enable) {
+  for (int i = 0; i < NUM_SLAVES; ++i) mc_enable_run(i, enable);
+}
+//故障复位，待开发
+void mc_fault_reset(int idx) {
+  if (!mc_valid_index(idx)) return;
+  g_cmd.fault_reset[idx].store(true, std::memory_order_relaxed);
+}
+//急停（轴号）
+void mc_quick_stop(int idx) {
+  if (!mc_valid_index(idx)) return;
+  g_cmd.quick_stop[idx].store(true, std::memory_order_relaxed);
+}
+
+
+
+//读取轴状态（轴号，状态字，位置，速度，扭矩）
+void mc_get_state(int idx, uint16_t &status, int32_t &pos, int32_t &vel, int16_t &torque) {
+  idx = clamp_index(idx);
+
+  // This should be done in HAL
+  // slave index transport into joint index.
+  // switch (idx)
+  //   {
+  //   case 0:
+  //     idx = 2;
+  //     break;
+  //   case 1:
+  //     idx = 3;
+  //     break;
+  //   case 2:
+  //     idx = 0;
+  //     break;
+  //   case 3:
+  //     idx = 1;
+  //     break;
+  //   case 4:
+  //     idx = 5;
+  //     break;
+  //   case 5:
+  //     idx = 4;
+  //     break;
+  //   default:
+  //     break;
+  //   }
+
+  status = g_state.status_word[idx].load(std::memory_order_relaxed);
+  pos    = g_state.pos_act[idx].load(std::memory_order_relaxed);
+  vel    = g_state.vel_act[idx].load(std::memory_order_relaxed);
+  torque = g_state.torque_act[idx].load(std::memory_order_relaxed);
+}

game_pad/game_pad/game_pad_node.py

@@ -55,12 +55,13 @@ class JoyControlNode(Node):
         self.get_logger().info('game pad node initialized')
 
     def joy_callback(self, msg):
+        print("joy_callback: ")
         # 处理按键
         for i in range(len(msg.buttons)):
             if msg.buttons[i] != self.previous_buttons[i]:
                 self.previous_buttons[i] = msg.buttons[i]
                 command = self.button_map[i]+","+str(msg.buttons[i])
-                self.get_logger().info("key:"+command+","+str(msg.buttons[i]))
+                self.get_logger().info("key:"+command)
                 self.command_publisher.publish(String(data=command))
                 
         #处理axis

gripper_dev/gripper_dev/gripper_dev_node.py

@@ -0,0 +1,151 @@
+
+from jodellSdk.jodellSdkDemo import RgClawControl
+import time
+import rclpy
+from rclpy.node import Node
+from rclpy.action import ActionServer
+from rclpy.action import ActionServer
+from std_msgs.msg import String
+from interfaces.action import GripperCmd
+import sys
+import os
+
+class GripperDevNode(Node):
+    def __init__(self):
+        super().__init__('gripper_dev_node')
+        self.declare_parameter('port','/dev/ttyUSB0')
+        param = self.get_parameter('port')
+        self.com_dev = param.value
+        self.declare_parameter('devid',9)
+        param = self.get_parameter('devid')
+        self.devid = param.value
+
+        self.declare_parameter('name','/gripper_cmd0')
+        param = self.get_parameter('name')
+        self.srv_name = param.value
+        print('param:',self.com_dev,self.devid,self.srv_name)
+        self.get_logger().info(f'ros param:{self.com_dev},{self.devid},{self.srv_name}')
+        self.action=ActionServer(self,GripperCmd,self.srv_name,self.gripper_cmd_callback_action)
+        
+        self.clawControl = RgClawControl()
+        self.target_loc=0
+        self.target_torque=0
+        self.target_speed=0
+        self.cur_loc=-1
+        self.cur_torque=-1
+        self.cur_goal=None
+        self.timer_on=False
+        self.target_mode=0
+        self.enable_ok=False
+        flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
+        if flag==1:
+            ret = self.clawControl.enableClamp(self.devid, True)
+            if ret==1:
+                self.enable_ok=True
+            else:
+                print(self.comd_dev,'enble fail!')
+        print('gripper_dev_node init:',self.com_dev)
+        #self.feedback_timer = self.create_timer(0.1, self.update_feedback) 
+    def gripper_cmd_callback_action(self,goal):
+        self.cur_goal=goal
+        req=goal.request
+        self.target_loc=req.loc
+        self.target_speed=req.speed
+        self.target_torque=req.torque
+        self.target_mode=req.mode
+        print('recv goal',self.devid,self.target_loc,self.target_speed,self.target_torque,self.target_mode)
+        #flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
+        #if flag==1:
+        #    ret = self.clawControl.enableClamp(self.devid, True)
+        #    if ret!=1:
+        #        print('enble fail!')
+        #        result=GripperCmd.Result()
+        #        result.state="gripper enable fail!"
+        #        result.result=0
+        #        goal.abort()
+        #        return result
+        #    print('start run...')
+        if self.enable_ok:
+            self.clawControl.runWithParam(self.devid,self.target_loc,self.target_speed,self.target_torque)
+            ##feedback
+            loop_flag=True
+            loop_times=0
+            while loop_flag:
+                loc = self.clawControl.getClampCurrentLocation(self.devid)
+                speed = self.clawControl.getClampCurrentSpeed(self.devid)
+                torque = self.clawControl.getClampCurrentTorque(self.devid)
+                self.cur_loc=loc[0]
+                self.cur_speed=speed[0]
+                if self.target_mode==0:
+                    loop_flag=abs(self.cur_loc-self.target_loc)>2
+                elif self.target_mode==1:
+                    loop_flag=abs(self.cur_torque-self.target_torque)>2
+                else:
+                    loop_flag=abs(self.cur_loc-self.target_loc)>2 and abs(self.cur_loc-self.target_loc)>2
+                #print('feedback:',self.cur_loc,speed,torque)
+                feedback_msg = GripperCmd.Feedback()
+                feedback_msg.loc=loc[0]
+                feedback_msg.speed=speed[0]
+                feedback_msg.torque=torque[0]
+                print('update feedback:',feedback_msg)
+                self.cur_goal.publish_feedback(feedback_msg)
+                time.sleep(1)
+                loop_times+=1
+                if loop_times>15:
+                    result=GripperCmd.Result()
+                    result.state="loop timeout!!!"
+                    result.result=0
+                    goal.abort()
+                    loop_times=0
+                    return result
+
+        else:
+            result=GripperCmd.Result()
+            result.state="uart open fail & enable fail!"
+            result.result=0
+            goal.abort()
+            return result
+        #print('start timer')
+        self.timer_on=True
+        print('location diff:',self.cur_loc,self.target_loc)
+        state = self.clawControl.getClampCurrentState(self.devid)
+        result=GripperCmd.Result()
+        result.state=state[0]
+        result.result=1
+        goal.succeed()
+        print('action finish',state)
+        return result
+    def destroy_node(self):
+        super.destroy_node()
+        flag = self.clawControl.serialOperation(self.com_dev, 115200, False)
+
+    def gripper_cmd_callback(self, request, response):
+        self.cur_loc=request.loc
+        self.cur_speed=request.speed
+        self.cur_torque=request.torque
+        print('recv gripper cmd:',self.cur_loc,self.cur_speed,self.cur_torque)
+        flag = self.clawControl.serialOperation(self.com_dev, 115200, True)
+        if flag == 1:
+            #self.runCmd()
+            #self.runCmd()
+            response.state = self.clawControl.getClampCurrentState(self.devid)
+            self.clawControl.serialOperation(self.com_dev, 115200, False)
+            print('send gripper sta:',self.devid,response.state)
+        else:
+            response.state = 'uart open failed'
+        return response
+def main():
+    rclpy.init()
+    node=GripperDevNode()
+    rclpy.spin(node)
+    node.destroy_node()
+    rclpy.shutdown()
+    pass
+if __name__ == '__main__':
+    main()
+
+
+
+
+
+

gripper_dev/launch/gripper_dev.launch.py

@@ -0,0 +1,7 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+def generate_launch_description():
+    return LaunchDescription([
+        Node(package="gripper_dev",executable="gripper_dev_node",name="gripper_dev_node",
+             parameters=[{'port':'/dev/ttyUSB0'}, {'name':'/gripper_cmd0'}, {'devid':6}])
+    ])

gripper_dev/package.xml

@@ -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>gripper_dev</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="you@example.com">h</maintainer>
+  <license>TODO: License declaration</license>
+
+ <buildtool_depend>ament_python</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>
+  <depend>std_msgs</depend>
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

gripper_dev/setup.cfg

@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gripper_dev
+[install]
+install_scripts=$base/lib/gripper_dev

gripper_dev/setup.py

@@ -0,0 +1,24 @@
+from setuptools import setup
+import os
+from glob import glob
+package_name='gripper_dev'
+setup(
+    name='gripper_dev',
+    version='0.1',
+    description='A simple package for gripper_dev',
+    author='Gripper Dev',
+    author_email='gripper_dev@example.com',
+    url='https://github.com/gripper_dev',
+    packages=['gripper_dev'],
+    data_files=[
+        ('share/ament_index/resource_index/packages', ['resource/' + package_name]),
+        ('share/' + package_name, ['package.xml']),
+        (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+    ],
+    install_requires=[
+        'setuptools',
+    ],
+    entry_points={
+        'console_scripts':['gripper_dev_node = gripper_dev.gripper_dev_node:main']
+    }
+)

img_dev/CMakeLists.txt

@@ -12,11 +12,12 @@ find_package(rclcpp REQUIRED)
 find_package(rosidl_default_generators REQUIRED) 
 find_package(message_filters REQUIRED)
 find_package(realsense2_camera_msgs REQUIRED)
+find_package(interfaces REQUIRED)
 
-rosidl_generate_interfaces(${PROJECT_NAME}
-  "msg/ImgMsg.msg"
-  DEPENDENCIES sensor_msgs
-)
+#rosidl_generate_interfaces(${PROJECT_NAME}
+#  "msg/ImgMsg.msg"
+#  DEPENDENCIES sensor_msgs
+#)
 include_directories(
   include
   ${CMAKE_CURRENT_BINARY_DIR}
@@ -24,18 +25,16 @@ include_directories(
 )
 
 add_executable(img_dev_node src/main.cpp src/img_dev_node.cpp)
-add_dependencies(img_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
+#add_dependencies(img_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
 
 target_link_libraries(img_dev_node
-    ${PROJECT_NAME}__rosidl_typesupport_cpp
-    ${PROJECT_NAME}__rosidl_typesupport_c
     message_filters::message_filters
 )
 
 
-ament_target_dependencies(img_dev_node rclcpp sensor_msgs realsense2_camera_msgs)
+ament_target_dependencies(img_dev_node rclcpp sensor_msgs realsense2_camera_msgs interfaces)
 install(TARGETS img_dev_node DESTINATION lib/${PROJECT_NAME})
-install(DIRECTORY msg/ DESTINATION share/${PROJECT_NAME}) 
+# install(DIRECTORY msg/ DESTINATION share/${PROJECT_NAME}) 
 install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch}) 
 ament_export_dependencies(rosidl_default_runtime)
 

img_dev/include/img_dev/img_dev_node.hpp

@@ -5,7 +5,7 @@
 #include "message_filters/subscriber.h"
 #include "message_filters/time_synchronizer.h"
 #include "message_filters/sync_policies/approximate_time.hpp"
-#include "img_dev/msg/img_msg.hpp"
+#include "interfaces/msg/img_msg.hpp"
 using namespace std;
 namespace img_dev{
     
@@ -14,7 +14,7 @@ namespace img_dev{
             ImageSubscriber() ;
 	    void pub_msg(const sensor_msgs::msg::Image& c_img,const sensor_msgs::msg::Image& d_img,const ImgCfg& cfg);
         private:
-            rclcpp::Publisher<img_dev::msg::ImgMsg>::SharedPtr img_pub;
-            std::shared_ptr<img_dev::msg::ImgMsg> img_msg; 
+            rclcpp::Publisher<interfaces::msg::ImgMsg>::SharedPtr img_pub;
+            std::shared_ptr<interfaces::msg::ImgMsg> img_msg; 
     };
 }

img_dev/launch/img_dev.launch.py

@@ -7,32 +7,14 @@ from launch.conditions import IfCondition
 def generate_launch_description():
     return LaunchDescription([
         # 启动orbbec_camera
-        ExecuteProcess(
-            cmd=[
-                'ros2',
-                'launch',
-                'orbbec_camera',
-                'multi_camera.launch.py'
-            ],
-            output='screen'
-        ),
-
-        ExecuteProcess(
-            cmd=[
-                'ros2',
-                'launch',
-                'realsense2_camera',
-                'rs_launch.py'
-            ],
-            output='screen'
-        ),
+        #ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'multi_camera.launch.py'],output='screen'),
+        ExecuteProcess(cmd=['ros2','launch', 'orbbec_camera', 'gemini_330_series.launch.py'],output='screen'),
+        # ExecuteProcess(cmd=['sleep', '2'],output='screen'),
+        #ExecuteProcess(cmd=['ros2','launch', 'realsense2_camera','rs_launch.py'],output='screen'),
 
+        ExecuteProcess(cmd=['sleep', '2'],output='screen'),
         
         # 等待一段时间确保相机启动完成
-        ExecuteProcess(
-            cmd=['sleep', '3'],
-            output='screen'
-        ),
         
         # 启动img_dev_node
         Node(

img_dev/msg/ImgMsg.msg

@@ -1,7 +0,0 @@
-sensor_msgs/Image image_depth
-sensor_msgs/Image image_color
-float64[] karr
-float64[] darr
-string source
-string position
-string type

img_dev/src/img_dev_node.cpp

@@ -1,6 +1,6 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
-#include "img_dev/msg/img_msg.hpp"// 自定义消息头文件
+#include "interfaces/msg/img_msg.hpp"// 自定义消息头文件
 #include "img_dev/img_cfg.hpp"
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.hpp>
@@ -8,8 +8,8 @@
 #include"img_dev/img_dev_node.hpp"
 namespace img_dev{
 ImageSubscriber::ImageSubscriber() : Node("image_subscriber") {
-    img_msg= std::make_shared<img_dev::msg::ImgMsg>();
-    img_pub = this->create_publisher<img_dev::msg::ImgMsg>("/img_msg", 10);
+    img_msg= std::make_shared<interfaces::msg::ImgMsg>();
+    img_pub = this->create_publisher<interfaces::msg::ImgMsg>("/img_msg", 10);
 }
 static long long pub_cnt=0;
 void ImageSubscriber::pub_msg(const sensor_msgs::msg::Image& c_img,const sensor_msgs::msg::Image& d_img,const ImgCfg& cfg){
@@ -21,8 +21,9 @@ void ImageSubscriber::pub_msg(const sensor_msgs::msg::Image& c_img,const sensor_
     img_msg->karr=cfg.k_arr;
     img_msg->darr=cfg.d_arr;
     img_pub->publish(*img_msg);
-    pub_cnt+=1;
-    std::cout<<"pub msg["<<pub_cnt<<"]:"<<img_msg->darr[0]<<","<<img_msg->darr[1]<<",k:"<<img_msg->karr[0]<<","<<img_msg->karr[1]<<std::endl;
+    // pub_cnt+=1;
+    
+    std::cout<<"pub msg["<<pub_cnt<<"]:"<<img_msg->image_color.width<<"x"<<img_msg->image_color.height<<";"<<img_msg->darr[0]<<","<<img_msg->darr[1]<<",k:"<<img_msg->karr[0]<<","<<img_msg->karr[1]<<std::endl;
 }
 
 }

img_dev/src/main.cpp

@@ -1,7 +1,7 @@
 #include <rclcpp/rclcpp.hpp>
 #include <sensor_msgs/msg/image.hpp>
 #include <sensor_msgs/msg/camera_info.hpp>
-#include "img_dev/msg/img_msg.hpp"
+#include "interfaces/msg/img_msg.hpp"
 #include "img_dev/img_cfg.hpp"
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.hpp>
@@ -15,10 +15,11 @@ using namespace std;
 using namespace img_dev;
 using SyncPolicy=message_filters::sync_policies::ApproximateTime<sensor_msgs::msg::Image,sensor_msgs::msg::Image>;
 static shared_ptr<ImageSubscriber> cur_node=nullptr;
-ImgCfg cfg0=ImgCfg("orbbec", "myType","left","/camera_01/color/image_raw","/camera_01/depth/image_raw");
-ImgCfg cfg1=ImgCfg("orbbec", "myType","right","/camera_02/color/image_raw","/camera_02/depth/image_raw");
+ImgCfg cfg0=ImgCfg("orbbec", "myType","left","/camera/color/image_raw","/camera/depth/image_raw");
+ImgCfg cfg1=ImgCfg("orbbec", "myType","top","/camera_02/color/image_raw","/camera_02/depth/image_raw");
 //
-ImgCfg cfg2=ImgCfg("realsense", "myType","front","/camera/color/image_raw","/camera/depth/image_raw");
+ImgCfg cfg2=ImgCfg("realsense", "myType","right","/camera/camera/rgbd","/camera/camera/rgbd");
+ImgCfg cfg3=ImgCfg("realsense", "myType","hand_r","/camera2/camera2/rgbd","/camera2/camera2/rgbd");
 void sync_cb0(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Image& d_img) {
     cur_node->pub_msg(c_img,d_img,cfg0);
 }
@@ -26,14 +27,17 @@ void sync_cb1(const sensor_msgs::msg::Image& c_img, const sensor_msgs::msg::Imag
     cur_node->pub_msg(c_img,d_img,cfg1);
 }
 int main(int argc, char* argv[]) {
+    setenv("ROS_DISABLE_SHM", "1", 1); //禁用共享内存
     rclcpp::init(argc, argv);
     cur_node=make_shared<ImageSubscriber>();
     if(!cur_node){
         RCLCPP_ERROR(rclcpp::get_logger("main"),"img_dev node create error!!!!");
         return -1;
     }
+
+    #if 0
     //////////////////奥比中光1 START///////////////////////
-    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera01_info=cur_node->create_subscription<sensor_msgs::msg::CameraInfo>("/camera_01/color/camera_info", 10,
+    rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera01_info=cur_node->create_subscription<sensor_msgs::msg::CameraInfo>("/camera/color/camera_info", 10,
         [&](const sensor_msgs::msg::CameraInfo& info){
             if(info.k.empty()||info.d.empty())
                 return;
@@ -52,7 +56,8 @@ int main(int argc, char* argv[]) {
     auto sync0 = make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), c_sub0, d_sub0);
     sync0->registerCallback(&sync_cb0);
     //////////////////奥比中光1 END///////////////////////
-
+#endif
+   #if 0
 
     //////////////////奥比中光2 START///////////////////////
     rclcpp::Subscription<sensor_msgs::msg::CameraInfo>::SharedPtr camera02_info=cur_node->create_subscription<sensor_msgs::msg::CameraInfo>("/camera_02/color/camera_info", 10,
@@ -73,9 +78,9 @@ int main(int argc, char* argv[]) {
     auto sync1 = make_shared<message_filters::Synchronizer<SyncPolicy>>(SyncPolicy(10), c_sub1, d_sub1);
     sync1->registerCallback(&sync_cb1);
     //////////////////奥比中光2 END///////////////////////
-
+#endif
     //////////////////realsense START///////////////////////
-    rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera/camera/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
+    rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd1_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera/camera/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
         if(cfg2.k_arr.size()==0||cfg2.d_arr.size()==0){
                 cfg2.k_arr.assign(msg.rgb_camera_info.k.begin(), msg.rgb_camera_info.k.end());
                 cfg2.d_arr.assign(msg.rgb_camera_info.d.begin(), msg.rgb_camera_info.d.end());
@@ -85,6 +90,23 @@ int main(int argc, char* argv[]) {
         cur_node->pub_msg(msg.rgb,msg.depth,cfg2);
       });
     //////////////////realsense END///////////////////////
+#if 0
+//////////////////realsense START///////////////////////
+    rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd2_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera2/camera2/rgbd", 10, [&](const realsense2_camera_msgs::msg::RGBD& msg) {
+        if(cfg3.k_arr.size()==0||cfg3.d_arr.size()==0){
+                cfg3.k_arr.assign(msg.rgb_camera_info.k.begin(), msg.rgb_camera_info.k.end());
+                cfg3.d_arr.assign(msg.rgb_camera_info.d.begin(), msg.rgb_camera_info.d.end());
+                cfg3.arr_copy+=1;
+                cout<<"copy:"<<"k:"<<cfg3.k_arr[0]<<"d:"<<cfg3.d_arr[0]<<endl;
+        }
+        cur_node->pub_msg(msg.rgb,msg.depth,cfg3);
+      });
+    //////////////////realsense END///////////////////////
+#endif
+
+
+
+
     rclcpp::spin(cur_node); 
     rclcpp::shutdown(); 
     return 0;

imu_dev/CMakeLists.txt

@@ -10,20 +10,21 @@ find_package(ament_cmake REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(rclcpp REQUIRED)
-find_package(rosidl_default_generators REQUIRED)
+find_package(interfaces REQUIRED)
 
 
-rosidl_generate_interfaces(${PROJECT_NAME} "msg/ImuMsg.msg" DEPENDENCIES sensor_msgs)
+####rosidl_generate_interfaces(${PROJECT_NAME} "msg/ImuMsg.msg" DEPENDENCIES sensor_msgs)
 include_directories(include ${CMAKE_CURRENT_BINARY_DIR}  ${CMAKE_CURRENT_BINARY_DIR}/rosidl_generator_cpp)
 add_executable(imu_dev_node src/main.cpp)
 
-ament_target_dependencies(imu_dev_node rclcpp std_msgs sensor_msgs)
-add_dependencies(imu_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
-target_link_libraries(imu_dev_node
-    ${PROJECT_NAME}__rosidl_typesupport_cpp
-    ${PROJECT_NAME}__rosidl_typesupport_c
-)
+ament_target_dependencies(imu_dev_node rclcpp std_msgs sensor_msgs interfaces)
+###add_dependencies(imu_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
+#target_link_libraries(imu_dev_node
+#    ${PROJECT_NAME}__rosidl_typesupport_cpp
+#    ${PROJECT_NAME}__rosidl_typesupport_c
+#)
 install(TARGETS imu_dev_node DESTINATION lib/${PROJECT_NAME})
+install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}/launch)
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

imu_dev/launch/imu_dev.launch.py

@@ -0,0 +1,22 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='imu',
+            executable='imu_node',
+            name='imu_node',
+            parameters=[{
+                'port_name': '/dev/ttyUSB0',
+            }]
+        ),
+        
+        Node(
+            package='imu_dev',
+            executable='imu_dev_node',
+            name='imu_dev_node',
+            output='screen'
+        )
+    ])
+    

imu_dev/msg/ImuMsg.msg

@@ -1,4 +0,0 @@
-string source
-string type
-string position
-sensor_msgs/Imu imu

imu_dev/src/main.cpp

@@ -1,23 +1,93 @@
 #include "rclcpp/rclcpp.hpp"
-#include "imu_dev/msg/imu_msg.hpp"
+#include "interfaces/msg/imu_msg.hpp"
 #include "sensor_msgs/msg/imu.hpp"
+#include <fstream>
 using namespace std;
+
+void QuaternionToRPYRad(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
+{
+    double norm = sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
+    if (norm < 1e-12) {  // 避免除以零
+        roll = 0;
+        pitch = 0;
+        yaw = 0;
+        return;
+    }
+
+    qw /= norm;
+    qx /= norm;
+    qy /= norm;
+    qz /= norm;
+    
+    // 计算Roll角（X轴）
+    roll = atan2(2 * (qw*qx + qy*qz), 1 - 2 * (qx*qx + qy*qy));
+    
+    // 计算Pitch角（Y轴），限制输入范围避免数值误差（asin输入需在[-1,1]）
+    double sin_pitch = 2 * (qw*qy - qz*qx);
+    sin_pitch = std::max(std::min(sin_pitch, 1.0), -1.0); 
+    pitch = asin(sin_pitch);
+    
+    // 计算Yaw角（Z轴）
+    yaw = atan2(2 * (qw*qz + qx*qy), 1 - 2 * (qy*qy + qz*qz));
+}
+
+// std::ofstream rpy_file("rpy.txt");
+void QuaternionToRPYDeg(double qw, double qx, double qy, double qz, double &roll, double &pitch, double &yaw)
+{
+    QuaternionToRPYRad(qw, qx, qy, qz, roll, pitch, yaw);
+
+    roll = roll * 180.0 / 3.1415926;
+    pitch = pitch * 180.0 / 3.1415926;
+    yaw = yaw * 180.0 / 3.1415926;
+    // rpy_file<<roll<<","<<pitch<<","<<yaw<<std::endl;
+    // std::cout << "Roll: " << roll << ", Pitch: " << pitch << ", Yaw: " << yaw << std::endl;
+}
+
 int main(int argc, char **argv)
 {
   rclcpp::init(argc, argv);
   auto node = rclcpp::Node::make_shared("imu_dev_node");
-  auto imu_pub = node->create_publisher<imu_dev::msg::ImuMsg>("/imu_msg", 10);
+  auto imu_pub = node->create_publisher<interfaces::msg::ImuMsg>("/imu_msg", 10);
   auto imu_sub=node->create_subscription<sensor_msgs::msg::Imu>("/imu/data", 10, [=](const sensor_msgs::msg::Imu::SharedPtr msg){
       //cout<<msg->linear_acceleration.x<<","<<msg->linear_acceleration.y<<","<<msg->linear_acceleration.z<<endl;
       //cout<<msg->angular_velocity.x<<","<<msg->angular_velocity.y<<","<<msg->angular_velocity.z<<endl;
       //cout<<msg->orientation.x<<","<<msg->orientation.y<<","<<msg->orientation.z<<","<<msg->orientation.w<<endl;
-      imu_dev::msg::ImuMsg imu_msg;
+      interfaces::msg::ImuMsg imu_msg;
       imu_msg.source="waveshare";
       imu_msg.position="front";
       imu_msg.type="None";
       imu_msg.imu=*msg;
+
+      double gyroValues_[3];
+
+      QuaternionToRPYDeg(
+        imu_msg.imu.orientation.w, 
+        imu_msg.imu.orientation.x, 
+        imu_msg.imu.orientation.y, 
+        imu_msg.imu.orientation.z, 
+        gyroValues_[0], 
+        gyroValues_[1], 
+        gyroValues_[2]
+      );
+
       imu_pub->publish(imu_msg);
   });
+///livox/imu
+  auto imu_sub_1=node->create_subscription<sensor_msgs::msg::Imu>("/livox/imu",10,[=](const sensor_msgs::msg::Imu::SharedPtr msg){
+		  
+		  interfaces::msg::ImuMsg imu_msg;
+      imu_msg.source="livox";
+      imu_msg.position="front";
+      imu_msg.type="None";
+      imu_msg.imu=*msg;
+      imu_pub->publish(imu_msg);
+
+		  
+		  
+		  });
+
+
   rclcpp::spin(node);
   rclcpp::shutdown();
-}
+  // rpy_file.close();
+}

input_dev/CMakeLists.txt

@@ -12,6 +12,7 @@ find_package(std_msgs REQUIRED)
 find_package(sensor_msgs REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 find_package(message_filters REQUIRED)
+find_package(interfaces REQUIRED)
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights
@@ -23,10 +24,10 @@ if(BUILD_TESTING)
   set(ament_cmake_cpplint_FOUND TRUE)
   ament_lint_auto_find_test_dependencies()
 endif()
-rosidl_generate_interfaces(${PROJECT_NAME}
-        "msg/InputMsg.msg"
-  DEPENDENCIES sensor_msgs
-)
+#rosidl_generate_interfaces(${PROJECT_NAME}
+#        "msg/InputMsg.msg"
+#  DEPENDENCIES sensor_msgs
+#)
 include_directories(
   include
   ${CMAKE_CURRENT_BINARY_DIR}
@@ -34,17 +35,15 @@ include_directories(
 )
 
 add_executable(input_dev_node src/main.cpp src/input_dev_node.cpp src/key_map.cpp)
-add_dependencies(input_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
+#add_dependencies(input_dev_node ${PROJECT_NAME}__rosidl_typesupport_cpp)
 
 target_link_libraries(input_dev_node
-    ${PROJECT_NAME}__rosidl_typesupport_cpp
-    ${PROJECT_NAME}__rosidl_typesupport_c
     message_filters::message_filters
 )
 
 
-ament_target_dependencies(input_dev_node rclcpp sensor_msgs)
+ament_target_dependencies(input_dev_node rclcpp sensor_msgs interfaces)
 install(TARGETS input_dev_node DESTINATION lib/${PROJECT_NAME})
-install(DIRECTORY msg/ DESTINATION share/${PROJECT_NAME})
-install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch})
+#install(DIRECTORY msg/ DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch)
 ament_package()

input_dev/include/input_dev/input_dev_node.hpp

@@ -4,7 +4,7 @@
 #include "message_filters/subscriber.h"
 #include "message_filters/time_synchronizer.h"
 #include "message_filters/sync_policies/approximate_time.hpp"
-#include "input_dev/msg/input_msg.hpp"
+#include "interfaces/msg/input_msg.hpp"
 using namespace std;
 namespace input_dev{
     
@@ -15,8 +15,8 @@ namespace input_dev{
 	    void pub_msg(const int key,const InputCfg& cfg);
         void pub_msg(const int key,const float val,const InputCfg& cfg);
         private:
-            rclcpp::Publisher<input_dev::msg::InputMsg>::SharedPtr input_pub;
-            std::shared_ptr<input_dev::msg::InputMsg> input_msg; 
+            rclcpp::Publisher<interfaces::msg::InputMsg>::SharedPtr input_pub;
+            std::shared_ptr<interfaces::msg::InputMsg> input_msg; 
             
     };
 }

input_dev/launch/input_dev.launch.py

@@ -12,7 +12,7 @@ def generate_launch_description():
                 'ros2',
                 'launch',
                 'game_pad',
-                'gampe_pad.launch.py'
+                'game_pad.launch.py'
             ],
             output='screen'
         ),

input_dev/msg/InputMsg.msg

@@ -1,5 +0,0 @@
-string source
-string type
-string position
-int32 code
-float32 value

input_dev/src/input_dev_node.cpp

@@ -1,5 +1,5 @@
 #include <rclcpp/rclcpp.hpp>
-#include "input_dev/msg/input_msg.hpp"// 自定义消息头文件
+#include "interfaces/msg/input_msg.hpp"// 自定义消息头文件
 #include "input_dev/input_cfg.hpp"
 #include <message_filters/subscriber.h>
 #include <message_filters/sync_policies/approximate_time.hpp>
@@ -7,8 +7,8 @@
 #include"input_dev/input_dev_node.hpp"
 namespace input_dev{
 InputSub::InputSub() : Node("InputSub") {
-    input_msg= std::make_shared<input_dev::msg::InputMsg>();
-    input_pub = this->create_publisher<input_dev::msg::InputMsg>("/input_msg", 10);
+    input_msg= std::make_shared<interfaces::msg::InputMsg>();
+    input_pub = this->create_publisher<interfaces::msg::InputMsg>("/input_msg", 10);
 }
 static long long pub_cnt=0;
 void InputSub::pub_msg(const int key,const float val,const InputCfg& cfg){

input_dev/src/main.cpp

@@ -1,5 +1,5 @@
 #include <rclcpp/rclcpp.hpp>
-#include "input_dev/msg/input_msg.hpp"
+#include "interfaces/msg/input_msg.hpp"
 #include "std_msgs/msg/string.hpp"
 #include "input_dev/input_cfg.hpp"
 #include <message_filters/subscriber.h>

led_dev/led_dev/led_dev_node.py

@@ -0,0 +1,136 @@
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String
+import serial
+import time
+import threading
+class TM512AC_RS485_Driver:
+    def __init__(self, port, baudrate=250000, num_ic=5):
+        self.ser = serial.Serial(
+            port=port,
+            baudrate=baudrate,
+            bytesize=serial.EIGHTBITS,
+            parity=serial.PARITY_NONE,
+            stopbits=serial.STOPBITS_TWO,
+            timeout=0.1,
+            rtscts=False,
+            dsrdtr=False
+        )
+        self.num_ic = num_ic  # TM512芯片数量
+        self.channels = 3 * num_ic  # 总通道数（RGB模式）
+
+    def _send_break(self):
+        self.ser.sendBreak(0.000088)  # 88μs Break
+        time.sleep(0.000008)  # 8μs MAB
+
+    def _send_dmx_byte(self, value):
+        self.ser.write(bytes([value]))
+
+    def send_frame(self, rgb_data):
+        self._send_break()
+        self._send_dmx_byte(0x00)
+        for val in rgb_data[:self.channels]:
+            self._send_dmx_byte(val)
+        remaining = 512 - self.channels
+        if remaining > 0:
+            self.ser.write(bytes([0] * remaining))
+
+    def close(self):
+        self.ser.close()
+class LedCommand(Node):
+    def __init__(self):
+        super().__init__('led_dev_node')
+        self.create_subscription(String,'/led_cmd',self.led_cmd_cb,10)
+        self.driver = TM512AC_RS485_Driver(port='/dev/ttyUSB0', num_ic=5)
+        self.cur_color='green'
+        self.cur_val=5
+        self.update=0
+        self.get_logger().info("led_dev_node will start....")
+        self.th1=threading.Thread(target=self.com_task)
+        self.th1.start()
+        
+    def com_task(self):
+        idx=0
+        while True:
+            #self.led_blink(self.cur_color,self.cur_val)
+            self.all_color(self.cur_color, -self.cur_val)
+            self.all_color(self.cur_color, self.cur_val)
+    def wait_task(self):
+        self.th1.join()
+    def all_color(self,rgb_data,xnum):
+        num_ic=4
+        rgb = [0, 0, 0,0] * num_ic
+        start_id=0
+        end_id=256
+        if xnum<0:
+            start_id=256
+            end_id=0
+        val=0
+        if xnum==0:
+            val=255
+            if rgb_data=='red':
+                rgb = [val, 0, 0,0] * num_ic
+            elif rgb_data=='green':
+                rgb = [0, val, 0,0] * num_ic
+            elif rgb_data=='blue':
+                rgb = [0, 0, val,0] * num_ic
+            elif rgb_data=='cyan':#青色
+                rgb = [0, val, val,0] * num_ic
+            elif rgb_data=='yellow':#黄色
+                rgb = [val, val, 0,0] * num_ic
+            elif rgb_data=='purple':#紫色
+                rgb = [val, 0, val,0] * num_ic
+            elif rgb_data=='white':#紫色
+                rgb = [val, val, val,0] * num_ic
+            # print(rgb)
+            self.driver.send_frame(rgb)
+            return
+        for i in range(start_id,end_id,xnum):
+            #offset=i*5
+            val=i##offset
+            if val > 255:
+                val=255
+            if rgb_data=='red':
+                rgb = [val, 0, 0,0] * num_ic 
+            elif rgb_data=='green':
+                rgb = [0, val, 0,0] * num_ic
+            elif rgb_data=='blue':
+                rgb = [0, 0, val,0] * num_ic
+            elif rgb_data=='cyan':#青色
+                rgb = [0, val, val,0] * num_ic
+            elif rgb_data=='yellow':#黄色
+                rgb = [val, val, 0,0] * num_ic
+            elif rgb_data=='purple':#紫色
+                rgb = [val, 0, val,0] * num_ic
+            elif rgb_data=='white':#紫色
+                rgb = [val, val, val,0] * num_ic
+            # print(rgb)
+            self.driver.send_frame(rgb)
+            if self.update==1:
+                self.update=0
+                break
+            time.sleep(0.01)
+    def led_blink(self,color,val):
+        self.all_color(color, -val)
+        self.all_color(color, val)
+    def led_cmd_cb(self,msg):
+        xcmd=msg.data
+        print('led cmd:',xcmd)
+        cmd,val=xcmd.split(',')
+        if cmd in ['red','green','blue','cyan','yellow','purple','white','off']:
+            self.cur_color=cmd
+            self.cur_val=int(val)
+            self.update=1
+        ###self.led_blink(cmd,int(val))
+def main():
+    rclpy.init()
+    node=LedCommand()
+    rclpy.spin(node)
+    node.wait_task()
+    node.destroy_node()
+    rclpy.shutdown()
+
+if __name__=='__main__':
+    main()
+
+

led_dev/package.xml

@@ -0,0 +1,21 @@
+<?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>led_dev</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="you@example.com">h</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>std_msgs</depend>
+  <depend>rclpy</depend>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>