force update status

after control

ecrt_dev/config/r1controllers.yaml

@@ -0,0 +1,106 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    gpio_command_controller:
+      type: gpio_controllers/GpioCommandController
+
+trajectory_controller:
+  ros__parameters:
+    joints: 
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+      - joint_7
+      - joint_8
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+      - velocity
+    state_publish_rate: 200.0
+    action_monitor_rate: 20.0
+    allow_partial_joints_goal: false
+    open_loop_control: true
+    allow_integration_in_goal_trajectories: true
+    constraints:
+      goal_time: 0.5
+      stopped_velocity_tolerance: 0.02
+gpio_command_controller:
+  ros__parameters:
+    gpios: 
+      - joint_1
+      - joint_2
+      - joint_3
+      - joint_4
+      - joint_5
+      - joint_6
+      - joint_7
+      - joint_8
+    command_interfaces:
+      joint_1:
+        - interfaces:
+          - fault
+          - enable
+      joint_2:
+        - interfaces:
+          - fault
+          - enable
+      joint_3:
+        - interfaces:
+          - fault
+          - enable
+      joint_4:
+        - interfaces:
+          - fault
+          - enable
+      joint_5:
+        - interfaces:
+          - fault
+          - enable
+      joint_6:
+        - interfaces:
+          - fault
+          - enable
+      joint_7:
+        - interfaces:
+          - fault
+          - enable
+      joint_8:
+        - interfaces:
+          - fault
+          - enable
+    state_interfaces:
+      joint_1:
+        - interfaces:
+          - fault
+      joint_2:
+        - interfaces:
+          - fault
+      joint_3:
+        - interfaces:
+          - fault
+      joint_4:
+        - interfaces:
+          - fault
+      joint_5:
+        - interfaces:
+          - fault
+      joint_6:
+        - interfaces:
+          - fault
+      joint_7:
+        - interfaces:
+          - fault
+      joint_8:
+        - interfaces:
+          - fault

ecrt_dev/description/r1_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="r1_motor_drive.ros2_control.xacro" />
+
+  <xacro:motor_drive/>
+</robot>

ecrt_dev/description/r1_motor_drive.ros2_control.xacro

@@ -0,0 +1,58 @@
+<?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="fault"/>
+
+      <!-- 命令接口（位置控制/故障重置/使能） -->
+      
+      <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="joint_1" ec_position="1" />
+      <xacro:single_joint_config joint_name="joint_2" ec_position="2" />
+      <xacro:single_joint_config joint_name="joint_3" ec_position="3" />
+      <xacro:single_joint_config joint_name="joint_4" ec_position="4" />
+      <xacro:single_joint_config joint_name="joint_5" ec_position="5" />
+      <xacro:single_joint_config joint_name="joint_6" ec_position="6" />
+      <xacro:single_joint_config joint_name="joint_7" ec_position="7" />
+      <xacro:single_joint_config joint_name="joint_8" ec_position="8" />
+    </ros2_control>
+  </xacro:macro>
+
+</robot>

ecrt_dev/include/ethercat_driver/ethercat_driver.hpp

@@ -75,12 +75,12 @@ public:
   void check_domain1_state();
   void check_slave_config_states();
   void set_motor_enable(int id,bool enable){
-    if(id>0&&id<13){
+    if(id>0&&id<9){
         motor_enable_arr[id-1].store(enable);
     }
   };
   bool get_motor_enable(int id){
-    if(id>0&&id<13){
+    if(id>0&&id<9){
         return motor_enable_arr[id-1].load();
     }
     return false;

ecrt_dev/include/ethercat_driver/zer_config.hpp

@@ -1,6 +1,6 @@
 #include "ecrt.h"
 #include <math.h>
-#define NUM_SLAVES 12
+#define NUM_SLAVES 8
 #define ZER_VID_PID          0x5a65726f,0x00029252
 typedef struct {
     unsigned int ctrl_word;                   // 0x6040:00

ecrt_dev/launch/motor_drive.launch copy.py

@@ -0,0 +1,95 @@
+
+
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution
+from launch.actions import DeclareLaunchArgument,TimerAction
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'description_file',
+            default_value='motor_drive.config.xacro',
+            description='URDF/XACRO description file with the axis.',
+        )
+    )
+
+    description_file = LaunchConfiguration('description_file')
+    
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ecrt_driver"),
+                    "description",
+                    description_file,
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ecrt_driver"),
+            "config",
+            "controllers.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers,
+                        {
+                            "lock_memory": True,
+                            "thread_priority": 90
+                        }
+                    ],
+        output="both",
+    )
+
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[robot_description],
+        output='screen'
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+    )
+
+    gpio_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gpio_command_controller", "-c", "/controller_manager"],
+    )
+
+    trajectory_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["trajectory_controller", "-c", "/controller_manager"],
+    )
+    delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        delay_node,
+    ]#position_controller_spawner,
+
+    return LaunchDescription(
+        declared_arguments +
+        nodes)

ecrt_dev/launch/motor_drive.launch.py

@@ -1,95 +1,144 @@
-
-
+#!/usr/bin/env python3
 from launch import LaunchDescription
-from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration, TextSubstitution
-from launch.actions import DeclareLaunchArgument,TimerAction
-
+# 补全必要导入，增强跨设备兼容性
+from launch.substitutions import (
+    Command, FindExecutable, PathJoinSubstitution, 
+    LaunchConfiguration, TextSubstitution, EnvironmentVariable
+)
+from launch.actions import (
+    DeclareLaunchArgument, TimerAction, OpaqueFunction
+)
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
-
+# 新增：导入系统路径模块，避免路径解析异常
+import os
 
 def generate_launch_description():
-
-    # Declare arguments
+    # -------------------------- 1. 声明参数（增强默认值和兼容性） --------------------------
     declared_arguments = []
     declared_arguments.append(
         DeclareLaunchArgument(
             'description_file',
-            default_value='motor_drive.config.xacro',
+            default_value=TextSubstitution(text='motor_drive.config.xacro'),  # 显式用TextSubstitution，避免解析歧义
             description='URDF/XACRO description file with the axis.',
         )
     )
+    # 新增：声明工作空间路径参数，适配不同设备的路径差异
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'pkg_share_path',
+            default_value=FindPackageShare("ecrt_driver"),
+            description='Path to ecrt_driver package share directory'
+        )
+    )
 
+    # 绑定参数，规范命名
     description_file = LaunchConfiguration('description_file')
+    pkg_share_path = LaunchConfiguration('pkg_share_path')
     
-    # Get URDF via xacro
+    # -------------------------- 2. 重构URDF生成逻辑（解决拼接解析异常） --------------------------
+    # 优化：用TextSubstitution替换纯空格，避免跨设备解析时的空格截断问题
     robot_description_content = Command(
         [
-            PathJoinSubstitution([FindExecutable(name="xacro")]),
-            " ",
+            FindExecutable(name="xacro"),
+            TextSubstitution(text=" "),  # 替换原纯空格，规范Substitution拼接
             PathJoinSubstitution(
                 [
-                    FindPackageShare("ecrt_driver"),
-                    "description",
+                    pkg_share_path,  # 使用声明的参数，增强兼容性
+                    TextSubstitution(text="description"),
                     description_file,
                 ]
             ),
+            TextSubstitution(text=" "),  # 确保xacro命令参数分隔清晰
         ]
     )
     robot_description = {"robot_description": robot_description_content}
 
+    # -------------------------- 3. 重构控制器配置路径（避免路径解析失败） --------------------------
     robot_controllers = PathJoinSubstitution(
         [
-            FindPackageShare("ecrt_driver"),
-            "config",
-            "controllers.yaml",
+            pkg_share_path,
+            TextSubstitution(text="config"),
+            TextSubstitution(text="controllers.yaml"),
         ]
     )
 
+    # -------------------------- 4. 节点配置（增强跨设备兼容性） --------------------------
+    # 控制节点：添加emulate_tty=True，修复日志输出和权限问题；参数格式规范化
     control_node = Node(
         package="controller_manager",
-        executable="ros2_control_node",
-        parameters=[robot_description, robot_controllers,
-                        {
-                            "lock_memory": True,
-                            "thread_priority": 90
-                        }
-                    ],
+        executable="ros2_control_node",  # 确保executable为纯字符串，无替换对象
+        parameters=[
+            robot_description, 
+            robot_controllers,
+            {
+                "lock_memory": True,
+                "thread_priority": 90,
+                "use_sim_time": False  # 新增：显式关闭仿真时间，避免环境歧义
+            }
+        ],
         output="both",
+        emulate_tty=True,  # 关键：修复跨设备终端输出解析问题
+        namespace="/ecrt_driver"  # 新增：添加命名空间，避免节点冲突
     )
 
+    # 机器人状态发布节点：优化输出和参数
     robot_state_pub_node = Node(
         package="robot_state_publisher",
-        executable="robot_state_publisher",
-        parameters=[robot_description],
-        output='screen'
+        executable="robot_state_publisher",  # 纯字符串，无替换对象
+        parameters=[
+            robot_description,
+            {"use_sim_time": False}  # 显式指定，增强兼容性
+        ],
+        output='screen',
+        emulate_tty=True,  # 修复TextSubstitution相关的输出解析问题
+        namespace="/ecrt_driver"
     )
 
+    # 关节状态广播器：参数格式规范化
     joint_state_broadcaster_spawner = Node(
         package="controller_manager",
-        executable="spawner",
+        executable="spawner",  # 纯字符串，无替换对象
         arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+        output='screen',
+        emulate_tty=True,
+        namespace="/ecrt_driver"
     )
 
+    # GPIO控制器：参数格式规范化
     gpio_controller_spawner = Node(
         package="controller_manager",
-        executable="spawner",
+        executable="spawner",  # 纯字符串，无替换对象
         arguments=["gpio_command_controller", "-c", "/controller_manager"],
+        output='screen',
+        emulate_tty=True,
+        namespace="/ecrt_driver"
     )
 
+    # 轨迹控制器：参数格式规范化
     trajectory_controller_spawner = Node(
         package="controller_manager",
-        executable="spawner",
+        executable="spawner",  # 纯字符串，无替换对象
         arguments=["trajectory_controller", "-c", "/controller_manager"],
+        output='screen',
+        emulate_tty=True,
+        namespace="/ecrt_driver"
     )
-    delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
+
+    # -------------------------- 5. 延时节点（修复执行顺序解析问题） --------------------------
+    # 优化：TimerAction的actions用列表包裹，避免解析时的类型歧义
+    delay_node = TimerAction(
+        period=1.0,
+        actions=[gpio_controller_spawner, trajectory_controller_spawner],
+    )
+
+    # 组装所有节点
     nodes = [
         control_node,
         robot_state_pub_node,
         joint_state_broadcaster_spawner,
         delay_node,
-    ]#position_controller_spawner,
+    ]
 
-    return LaunchDescription(
-        declared_arguments +
-        nodes)
+    # 返回LaunchDescription，确保参数声明和节点列表拼接规范
+    return LaunchDescription(declared_arguments + nodes)

ecrt_dev/launch/r1_motor_drive.launch.py

@@ -0,0 +1,88 @@
+
+
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+from launch.actions import DeclareLaunchArgument,TimerAction
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'description_file',
+            default_value='r1_motor_drive.config.xacro',
+            description='URDF/XACRO description file with the axis.',
+        )
+    )
+
+    description_file = LaunchConfiguration('description_file')
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ecrt_driver"),
+                    "description",
+                    description_file,
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ecrt_driver"),
+            "config",
+            "r1controllers.yaml",
+        ]
+    )
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers,
+                        {
+                            "lock_memory": True,
+                            "thread_priority": 90
+                        }
+                    ],
+        output="both",
+    )
+    print('444444')
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        parameters=[robot_description],
+        output='screen'
+    )
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+    )
+    gpio_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["gpio_command_controller", "-c", "/controller_manager"],
+    )
+    trajectory_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["trajectory_controller", "-c", "/controller_manager"],
+    )
+    delay_node=TimerAction(period=1.0,actions=[gpio_controller_spawner,trajectory_controller_spawner])
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        delay_node,
+    ]#position_controller_spawner,
+    return LaunchDescription(
+        declared_arguments +
+        nodes)

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

@@ -4,7 +4,7 @@
 #include <cstdint>
 #include <algorithm>
 
-constexpr int NUM_SLAVES = 1; //定义电机数量
+constexpr int NUM_SLAVES = 8; //定义电机数量
 
 //运行模式
 enum class OpMode : int8_t {

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

@@ -1,80 +0,0 @@
-#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/src/ethercat_core.cpp

@@ -28,9 +28,6 @@
 #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"
@@ -148,11 +145,11 @@ void check_slave_config_states(void)
         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);
+            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");
+            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 ");
+            printf("[S%02d] %soperational.\n", i, s.operational ? "" : "Not ");
         sc_state[i] = s;
     }
 }
@@ -289,326 +286,173 @@ void cyclic_task()
         // 读取从站状态字、当前位置、速度、扭矩、运行模式
         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);
+            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];
+            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;
                 }
-                
-                // ----- 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);
+                last_mode_cmd[i] = mode_cmd;
+                continue; // 本周期先不再下其他目标，等模式生效
+            }
 
-                // ----- 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);
+            if ((status[i]&0x006f)==0x0008){
+                uint16_t err = EC_READ_U16(domain1_pd + zer_offsets[i].error_code);
 
-                //EC_WRITE_S8 (domain1_pd + offsets[i].operation_mode, mode_cmd); //模式设定
+                EC_WRITE_U16(domain1_pd + zer_offsets[i].ctrl_word, 0x0080); 
+                std::cout << "clear : " << i << "  " << err << std::endl;
+            }
 
-                // 第一次运行时把 last_mode_cmd 初始化为实际显示模式，避免“假沿”
-                if (!last_mode_cmd_inited) {
-                    last_mode_cmd[i] = md;
-                    if (i == NUM_SLAVES - 1) last_mode_cmd_inited = 1;
-                }
+            // 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;
 
-                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
-                    // 进入新模式时做必要初始化
+                // 同步当前位置为目标位置
+                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);
 
-                    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;
+                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;
 
-                    // 同步当前位置为目标位置
-                    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);
+                //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 累加器
 
-                    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;
+                            // 进入/首次运行：把命令位置对齐到“当前实际位置”，避免冲击
+                            if (!csp_inited[i] || last_mode_cmd[i] != CYCLIC_POSITION) {
+                                csp_cmd_pos[i] = pv;       // 先对齐
+                                vmax_acc[i]     = 0;
+                                csp_inited[i]   = 1;
                             }
-                            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);
+                            // 期望的“最终绝对目标”
+                            const int32_t goal = g_cmd.target_pos[i].load(std::memory_order_relaxed);
 
-                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);
+                            // 本周期允许的最大步长（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;          // 保留余数到下周期
 
-                // ----- 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);
+                            // 相对“命令轨迹”的误差（不是相对 pv）
+                            const int32_t err_cmd = goal - csp_cmd_pos[i];
 
-                //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;
+                            // 死区：足够近就贴合到 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;
                             }
-                            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;
-                                
+
+                            // 可选：跟随误差防护（避免 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;
                         }
-                    } 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 状态
+                        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 状态
                 }
             }
+            
         }
         
         if (sync_ref_counter) {
@@ -683,39 +527,19 @@ bool start()
         
         int position = i+1;
 
-        // if (i > 0)
-        // {
-        //     position = i+2;
-        // }
+        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 (i == 0)
+        if (ecrt_slave_config_pdos(sc[i], EC_END, zer_device_syncs)) 
         {
-            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;
-            }
+            std::cout << "Failed PDO config " << i << std::endl;
+            //   fprintf(stderr,"[S%02d] Failed PDO config\n", i); g_started.store(false); return false;
         }
+        
 
         // DC配置
         ecrt_slave_config_dc(sc[i], 0x0300, PERIOD_NS, 200, 0, 0);
@@ -727,72 +551,34 @@ bool start()
 
         int position = i+1;
 
-        // if (i > 1)
-        // {
-        //     position = i+2;
-        // }
+        // ---- 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; 
+        }
 
-        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;
 
-            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;
-            }
+        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;
         }
     }
 

gripper_dev/gripper_dev/gripper_dev_node.py

@@ -4,26 +4,32 @@ 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.msg import GripperStatus
+from rclpy.executors import MultiThreadedExecutor
 from interfaces.action import GripperCmd
 import sys
 import os
+import threading
 
 class GripperDevNode(Node):
     def __init__(self):
         super().__init__('gripper_dev_node')
-        self.declare_parameter('port','/dev/ttyUSB0')
+        self.lock = threading.Lock()
+        self.declare_parameter('port','/dev/ttyUSB1')
         param = self.get_parameter('port')
         self.com_dev = param.value
-        self.declare_parameter('devid',9)
+        self.declare_parameter('devid',6)
         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.declare_parameter('status_topic','gripper/status')
+        status_param = self.get_parameter('status_topic')
+        self.status_topic = status_param.value
+        self.get_logger().info(f'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)
         
@@ -36,8 +42,52 @@ class GripperDevNode(Node):
         self.cur_goal=None
         self.timer_on=False
         self.target_mode=0
-        print('gripper_dev_node init:',self.com_dev)
+        self.enable_ok=False
+        self.state=''
+        self.status_pub = self.create_publisher(GripperStatus, self.status_topic, 10)
+        self.status_timer = self.create_timer(0.1, self.status_timer_callback)
+
+        with self.lock:
+            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:
+                    self.get_logger().info(f'{self.com_dev} enable fail!')
+
+        self.get_logger().info(f'gripper_dev_node init: {self.com_dev}')
         #self.feedback_timer = self.create_timer(0.1, self.update_feedback) 
+
+    def status_timer_callback(self):
+        if not self.enable_ok:
+            return
+
+        # Query all status in one lock cycle to minimize context switching
+        with self.lock:
+            try:
+                loc = self.clawControl.getClampCurrentLocation(self.devid)
+                speed = self.clawControl.getClampCurrentSpeed(self.devid)
+                torque = self.clawControl.getClampCurrentTorque(self.devid)
+                state = self.clawControl.getClampCurrentState(self.devid)
+            except Exception as e:
+                self.get_logger().error(f"Serial read error: {e}")
+                return
+
+        if loc and speed and torque and state:
+            self.cur_loc = loc[0]
+            self.cur_speed = speed[0]
+            self.cur_torque = torque[0]
+            self.state = state[0]
+
+            msg = GripperStatus()
+            msg.header.stamp = self.get_clock().now().to_msg()
+            msg.loc = float(self.cur_loc)
+            msg.speed = float(self.cur_speed)
+            msg.torque = float(self.cur_torque)
+            msg.state = self.state
+            self.status_pub.publish(msg)
+
     def gripper_cmd_callback_action(self,goal):
         self.cur_goal=goal
         req=goal.request
@@ -45,78 +95,163 @@ class GripperDevNode(Node):
         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!')
+        self.get_logger().info(f'\n----------\nrecv goal {self.devid} {self.target_loc} {self.target_speed} {self.target_torque} {self.target_mode}')
+        if self.enable_ok:
+
+            # Use local copies of latest status to avoid blocking the serial port again
+            self.get_logger().info(f"origin state: {self.state}")
+            self.get_logger().info(f"origin loc: {self.cur_loc} -> {self.target_loc}")
+            self.get_logger().info(f"origin torque: {self.cur_torque} -> {self.target_torque}")
+
+            if (self.target_mode==0 or self.target_mode==1 or self.target_mode==2) : # loc/torque/both mode
+                if (self.target_torque == 0 or self.target_loc == 0) : # open gripper
+                    if (self.cur_loc == 0 and self.state == "手指已到达指定的位置，没有检测到物体") :
+                        result=GripperCmd.Result()
+                        result.state=self.state
+                        result.result=1
+                        goal.succeed()
+                        self.get_logger().info(f'result: {result}\n')
+                        return result
+                else:   # close gripper
+                    if (self.cur_loc > 0 and self.state == "手指在闭合检测到物体") :
+                        result=GripperCmd.Result()
+                        result.state=self.state
+                        result.result=1
+                        goal.succeed()
+                        self.get_logger().info(f'result: {result}\n')
+                        return result
+            else:
                 result=GripperCmd.Result()
-                result.state="gripper enable fail!"
+                result.state="mode error!"
+                result.result=0
+                goal.abort()
+                self.get_logger().info(f'result: {result}\n')
+                return result
+
+            # Retry sending command up to 3 times to ensure stability
+            max_retries = 3
+            cmd_sent = False
+            for i in range(max_retries):
+                with self.lock:
+                    try:
+                        self.clawControl.runWithParam(self.devid, self.target_loc, self.target_speed, self.target_torque)
+                        time.sleep(0.05)
+                        # Force update status immediately after command
+                        loc = self.clawControl.getClampCurrentLocation(self.devid)
+                        speed = self.clawControl.getClampCurrentSpeed(self.devid)
+                        torque = self.clawControl.getClampCurrentTorque(self.devid)
+                        state = self.clawControl.getClampCurrentState(self.devid)
+
+                        if loc and speed and torque and state:
+                            self.cur_loc = loc[0]
+                            self.cur_speed = speed[0]
+                            self.cur_torque = torque[0]
+                            self.state = state[0]
+
+                        cmd_sent = True
+                        break
+                    except Exception as e:
+                        self.get_logger().warn(f"Failed to send command (attempt {i+1}): {e}")
+                        time.sleep(0.05)
+            
+            if not cmd_sent:
+                result=GripperCmd.Result()
+                result.state="Serial command failed after retries"
                 result.result=0
                 goal.abort()
                 return result
-            print('start run...')
-            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
+                # Use cached values from the periodic status timer to reduce serial traffic and collisions
+                # The timer runs at 10Hz, which is frequent enough for the action monitor
+                self.get_logger().info(f"mode: {self.target_mode}")
+                self.get_logger().info(f"torque: {self.cur_torque} {self.target_torque}")
+                self.get_logger().info(f"loc: {self.cur_loc} {self.target_loc}")
+                self.get_logger().info(f"state: {self.state}")
+
+                if (self.cur_loc == 0 and (self.state == "手指已到达指定的位置，没有检测到物体" or self.state == "手指在张开检测到物体")) : #手指张开到位
+                    result=GripperCmd.Result()
+                    result.state=self.state
+                    result.result=1
+                    goal.succeed()
+                    self.get_logger().info(f'result: {result}\n')
+                    return result
+                elif (self.cur_loc > 0 and self.state == "手指在闭合检测到物体") : #手指闭合，并检测到物体
+                    result=GripperCmd.Result()
+                    result.state=self.state
+                    result.result=1
+                    goal.succeed()
+                    self.get_logger().info(f'result: {result}\n')
+                    return result
+                elif (self.cur_loc > 0 and self.cur_loc == self.target_loc and self.state == "手指已到达指定的位置，没有检测到物体"): #手指闭合，未检测到物体
+                    result=GripperCmd.Result()
+                    result.state=self.state
+                    result.result=0
+                    goal.abort()
+                    self.get_logger().info(f'result: {result}\n')
+                    return result
                 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)
+                    pass
+                    
                 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)
+                feedback_msg.loc=int(self.cur_loc)
+                feedback_msg.speed=int(self.cur_speed)
+                feedback_msg.torque=int(self.cur_torque)
+                try:
+                    # Check if the interface supports state in feedback
+                    feedback_msg.state=self.state
+                except AttributeError:
+                    pass
+                
+                self.get_logger().info(f'update feedback: {feedback_msg}')
+                goal.publish_feedback(feedback_msg)
+                time.sleep(1) # Monitor every 1 second
+                loop_times+=1
+                if loop_times>20: # Increased timeout since we are less invasive now
+                    result=GripperCmd.Result()
+                    result.state="loop timeout!!!"
+                    result.result=0
+                    goal.abort()
+                    loop_times=0
+                    self.get_logger().info(f'result: {result}\n')
+                    return result
         else:
             result=GripperCmd.Result()
-            result.state="uart open fail!"
+            result.state="uart open fail & enable fail!"
             result.result=0
             goal.abort()
+            self.get_logger().info(f'result: {result}\n')
             return result
-        #print('start timer')
+
         self.timer_on=True
-        print('location diff:',self.cur_loc,self.target_loc)
+        self.get_logger().info(f'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)
+        self.get_logger().info(f'action finish {state}\n**********\n')
+        
         return result
         
-    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 destroy_node(self):
+        super().destroy_node()
+        with self.lock:
+            flag = self.clawControl.serialOperation(self.com_dev, 115200, False)
 def main():
     rclpy.init()
     node=GripperDevNode()
-    rclpy.spin(node)
-    rclpy.shutdown()
+    # Using more threads to ensure the timer and multiple action/service requests don't block each other
+    executor = MultiThreadedExecutor(num_threads=10)
+    executor.add_node(node)
+    try:
+        executor.spin()
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
     pass
 if __name__ == '__main__':
     main()

gripper_dev/launch/gripper_dev.launch.py

@@ -2,6 +2,10 @@ 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}])
+        Node(package="gripper_dev",executable="gripper_dev_node",name="gripper0_dev_node",
+             parameters=[{'port':'/dev/robot/gripper0'}, {'name':'/gripper_cmd0'}, {'devid':9},
+                         {'status_topic':'/gripper0/status'}]),
+        Node(package="gripper_dev",executable="gripper_dev_node",name="gripper1_dev_node",
+             parameters=[{'port':'/dev/robot/gripper1'}, {'name':'/gripper_cmd1'}, {'devid':6},
+                         {'status_topic':'/gripper1/status'}])
     ])

gripper_dev/package.xml

@@ -13,6 +13,7 @@
 <exec_depend>rosidl_default_runtime</exec_depend>
 <member_of_group>rosidl_interface_packages</member_of_group>
   <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
   <depend>rclcpp</depend>
   <depend>rclpy</depend>
 

img_dev/launch/img_dev.launch.py

@@ -9,8 +9,9 @@ def generate_launch_description():
         # 启动orbbec_camera
         #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=['ros2','launch', 'realsense2_camera','rs_launch.py'],output='screen'),
+        ExecuteProcess(cmd=['ros2', 'launch', 'realsense2_camera', 'rs_multi_camera_launch.py', 'serial_no1:=\'405622072723\'', 'serial_no2:=\'247122074159\''], output='screen'),
 
         ExecuteProcess(cmd=['sleep', '2'],output='screen'),
         

img_dev/src/img_dev_node.cpp

@@ -23,7 +23,7 @@ void ImageSubscriber::pub_msg(const sensor_msgs::msg::Image& c_img,const sensor_
     img_pub->publish(*img_msg);
     // 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;
+    // 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

@@ -15,11 +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/color/image_raw","/camera/depth/image_raw");
-ImgCfg cfg1=ImgCfg("orbbec", "myType","top","/camera_02/color/image_raw","/camera_02/depth/image_raw");
+ImgCfg cfg0=ImgCfg("orbbec", "myType","top","/camera/color/image_raw","/camera/depth/image_raw");
+ImgCfg cfg1=ImgCfg("orbbec", "myType","null","/camera_02/color/image_raw","/camera_02/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");
+ImgCfg cfg2=ImgCfg("realsense", "myType","left","/camera/camera/rgbd","/camera/camera/rgbd");
+ImgCfg cfg3=ImgCfg("realsense", "myType","right","/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);
 }
@@ -35,7 +35,7 @@ int main(int argc, char* argv[]) {
         return -1;
     }
 
-    #if 0
+    #if 1
     //////////////////奥比中光1 START///////////////////////
     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){
@@ -80,7 +80,7 @@ int main(int argc, char* argv[]) {
     //////////////////奥比中光2 END///////////////////////
 #endif
     //////////////////realsense START///////////////////////
-    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) {
+    rclcpp::Subscription<realsense2_camera_msgs::msg::RGBD>::SharedPtr rgbd1_sub_ = cur_node->create_subscription<realsense2_camera_msgs::msg::RGBD>("/camera1/camera1/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());
@@ -90,7 +90,7 @@ int main(int argc, char* argv[]) {
         cur_node->pub_msg(msg.rgb,msg.depth,cfg2);
       });
     //////////////////realsense END///////////////////////
-#if 0
+#if 1
 //////////////////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){

imu_dev/launch/imu_dev.launch.py

@@ -1,22 +1,39 @@
 from launch import LaunchDescription
 from launch_ros.actions import Node
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_ros.substitutions import FindPackageShare
+from launch.substitutions import PathJoinSubstitution
 
 def generate_launch_description():
+    imu_node = Node(
+        package='imu',
+        executable='imu_node',
+        name='imu_node',
+        parameters=[{
+            'port_name': '/dev/ttyUSB1',
+        }]
+    )
+
+    imu_dev_node = Node(
+        package='imu_dev',
+        executable='imu_dev_node',
+        name='imu_dev_node',
+        output='screen'
+    )
+
+    openzen_dev = FindPackageShare(package="openzen_driver")
+    openzen_dev_launch_path = PathJoinSubstitution(
+        [openzen_dev, "launch", "openzen_lpms.launch.py"]
+    )
+
+    include_openzen_dev = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(openzen_dev_launch_path),
+    )
+
     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_node,
+        #imu_dev_node,
+        include_openzen_dev  
     ])
-    

motor_dev/include/motor_dev/rs485_driver.hpp

@@ -5,7 +5,7 @@
 #include <vector>
 #include <termios.h>
 #include <mutex>
-#include<map>
+
 namespace motor_dev
 {
     class RS485Driver
@@ -16,7 +16,7 @@ namespace motor_dev
         
         // 打开串口
         bool openPort(const std::string &port_name, int baud_rate = 115200);
-        std::map<uint8_t, float> speedMap;
+        
         // 关闭串口
         void closePort();
         
@@ -25,14 +25,13 @@ namespace motor_dev
         
         // 接收数据
         bool receiveData(std::vector<uint8_t> &data, size_t max_length, int timeout_ms = 100);
-        std::vector<uint8_t> sendCan(uint8_t cmd,const std::vector<uint8_t> data);
         //本末
-        int bm_set_enable(uint8_t enable);
+        int bm_set_enable(uint8_t motor_id,uint8_t enable);
         int bm_set_mode(uint8_t motor_id, uint8_t mode);
         int bm_set_pos(float angle1, float angle2);
-        int bm_set_speed(float speed1, float speed2,float speed3,float speed4);
+        int bm_set_speed(float speed1, float speed2);
         float bm_get_pos(uint8_t motor_id);
-        int bm_get_speed();
+        float bm_get_speed(uint8_t motor_id);
         int bm_query_id();
         int bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val);
         

motor_dev/src/main.cpp

@@ -3,95 +3,169 @@
 #include"interfaces/msg/motor_cmd.hpp"
 #include"interfaces/msg/motor_pos.hpp"
 #include"interfaces/srv/motor_param.hpp"
+#include"interfaces/srv/motor_info.hpp"
 using namespace motor_dev;
 RS485Driver rs485_driver_;
 
+static auto g_program_start_time = std::chrono::steady_clock::now();
+
+// 带时间戳的 printf 风格日志函数
+void 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();
+
+    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;
+    }
+
+    std::fprintf(stdout, "[%.3f] %s\n", seconds, buffer);
+    std::fflush(stdout); // 确保立即输出（对调试很重要）
+}
+
+
+
 int main(int argc,char* argv[]){
     rclcpp::init(argc,argv);
     auto node=rclcpp::Node::make_shared("motor_dev_node");
     ////auto pos_pub = node->create_publisher<motor_dev::msg::MotorPos>("/motor_pos", 10);
     std::cout << "open RS485 port.." << std::endl;
-    if (!rs485_driver_.openPort("/dev/ttyACM0", 921600)){
+    if (!rs485_driver_.openPort("/dev/ttyS1", 115200)){
         std::cout << "Failed to open RS485 port." << std::endl;
     }
-    usleep(100000);
-    rs485_driver_.speedMap.clear();
-    rs485_driver_.bm_set_enable(1);
+    usleep(500000);
+    rs485_driver_.bm_set_enable(1,1);
+    rs485_driver_.bm_set_enable(2,1);
 
+    rs485_driver_.bm_set_mode(1,4);
+    rs485_driver_.bm_set_enable(1,2);
+    rs485_driver_.bm_set_mode(2,4);
+    rs485_driver_.bm_set_enable(2,2);
     
-
-    rs485_driver_.bm_set_param(1,28,3);
-    rs485_driver_.bm_set_param(2,28,3);
-    rs485_driver_.bm_set_param(3,28,3);
-    rs485_driver_.bm_set_param(4,28,3);
-    rs485_driver_.bm_set_enable(2);
-
-    rs485_driver_.bm_set_param(1,84,100);
-    rs485_driver_.bm_set_param(2,84,100);
-    rs485_driver_.bm_set_param(3,84,100);
-    rs485_driver_.bm_set_param(4,84,100);
-
-
+    rs485_driver_.bm_set_param(1,77,51);
+    rs485_driver_.bm_set_param(1,78,8);
+    rs485_driver_.bm_set_param(1,79,8);
+    //rs485_driver_.bm_set_param(1,28,4);
+    rs485_driver_.bm_set_param(2,77,51);
+    rs485_driver_.bm_set_param(2,78,8);
+    rs485_driver_.bm_set_param(2,79,8);
+    //rs485_driver_.mt_set_pos(1,-5);
     //init
     /////rs485_driver_.bm_set_mode(1,3);//位置模式
     auto rs485_sub_=node->create_subscription<interfaces::msg::MotorCmd>("/motor_cmd",10,[](const interfaces::msg::MotorCmd::SharedPtr msg){
             ///
+     ///       printf("##############-----------------------");
             if (msg->target == "rs485") {
                 if(msg->type=="bm"){
                     size_t n = msg->motor_id.size();
                     if(n<2){
-                        printf("bm msg need four angle\n");
+                        printf("bm msg need two angle\n");
                         return;
                     }
-                    float speed1=msg->motor_speed[0];
-                    float speed2=msg->motor_speed[1];
-                    float speed3=msg->motor_speed[3];
-                    float speed4=msg->motor_speed[4];
-                    printf("###:%s,speed:%.1f,%.1f,%.1f,%.1f\n",msg->type.c_str(),speed1,speed2,speed3,speed4);
-                    rs485_driver_.bm_set_speed(speed1,speed2,speed3,speed4);
+                    float angle1    =msg->motor_angle[0];
+                    float angle2    =msg->motor_angle[1];
+
+                   // float speed1=msg->motor_speed[0];
+                    //float speed2=msg->motor_speed[1];
+                    log_printf("recv topic:%s,angle:%.1f,%.1f",msg->type.c_str(),angle1,angle2);
+                    rs485_driver_.bm_set_pos(angle1,angle2);
+                   // rs485_driver_.bm_set_speed(speed1,speed2);
                 }else if(msg->type=="mt"){
                     uint8_t id=msg->motor_id[0];
                     float angle    =msg->motor_angle[0];
                     rs485_driver_.mt_set_pos(id,angle);
                 }
+            }else if(msg->target=="ethercat"){
+                //TODO
             }
         });
+#if 1
     auto motor_pub=node->create_publisher<interfaces::msg::MotorPos>("/motor_pos",10);
     //读取电机位置
-    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(5000), [=]() { 
+    rclcpp::TimerBase::SharedPtr timer_=node->create_wall_timer(std::chrono::milliseconds(2000), [=]() { 
         static int pub_cnt=0;
+	// log_printf("query motor pos");
         interfaces::msg::MotorPos motor_pos;
-        rs485_driver_.bm_get_speed();
- 
+        float angle1=rs485_driver_.bm_get_pos(1);
+        float angle2=rs485_driver_.bm_get_pos(2);
+
+        //float speed1=rs485_driver_.bm_get_speed(1);
+        //float speed2=rs485_driver_.bm_get_speed(2);
+        ///printf("bm cur angle:%.1f\n",angle);
+        ///rs485_driver_.bm_query_id();
+        //rs485_driver_.mt_query_id();//3e cd 08 79 00 01 00 00 00 00 00 f8 95 
         motor_pos.motor_id.clear();
         motor_pos.motor_angle.clear();
         motor_pos.motor_speed.clear();
+        ////printf("pub msg[%03d],%.1f,%.1f\n",pub_cnt,angle1,angle2);
+        //if(angle1>-1&&angle2>-1)
         {
             motor_pos.source = "rs485";
             motor_pos.type = "bm";
             motor_pos.position="none";
-            for(const auto& kv:rs485_driver_.speedMap){
-                printf("%d:%.1f,",kv.first,kv.second);
-                motor_pos.motor_id.push_back(kv.first);
-                motor_pos.motor_speed.push_back(kv.second);
-            }
-            printf("\n");
+            motor_pos.motor_id.push_back(1);
+            motor_pos.motor_id.push_back(2);
+            motor_pos.motor_angle.push_back(angle1);
+            motor_pos.motor_angle.push_back(angle2);
+            //motor_pos.motor_speed.push_back(speed1);
+            //motor_pos.motor_speed.push_back(speed2);
             motor_pub->publish(motor_pos);
             
+            //std::cout<<"pub msg["<<pub_cnt<<"]:"<<angle1<<","<<angle2<<std::endl;
+            // log_printf("pub pos:%.1f,%.1f",angle1,angle2);
         }
         pub_cnt+=1;
+        #if 0
+        motor_pos.motor_id.clear();
+        motor_pos.motor_angle.clear();
+        float angle=rs485_driver_.mt_get_pos(1);
+        if(angle!=9999){
+            motor_pos.type = "mt";
+            motor_pos.motor_id.push_back(1);
+            motor_pos.motor_angle.push_back(angle);
+            motor_pub->publish(motor_pos);
+        }
+        #endif
     });
+#endif
     auto motor_param_srv=node->create_service<interfaces::srv::MotorParam>("motor_param",[=](const std::shared_ptr<interfaces::srv::MotorParam::Request> req,std::shared_ptr<interfaces::srv::MotorParam::Response> res){
-            res->ret=0;
-            printf("recv sv:%d,%d\n",req->motor_id,req->accel);
-            if(req->accel<500){
-                rs485_driver_.bm_set_param(1,84,300);
-                res->ret=1;
-                printf("svc:%d\n",res->ret);
+            uint8_t mid=req->motor_id;
+            res->ret=-1;
+            log_printf("recv sv:%d,%d,%d,%d",req->motor_id,req->max_speed,req->add_acc,req->dec_acc);
+            //if(mid>-1&&mid<500)
+	    {
+                if(req->max_speed<500&&req->add_acc<500&&req->dec_acc<500){
+                    rs485_driver_.bm_set_param(mid,77,req->max_speed);
+                    rs485_driver_.bm_set_param(mid,78,req->add_acc);
+                    rs485_driver_.bm_set_param(mid,79,req->dec_acc);
+                    res->ret=0;
+                    printf("svc:%d\n",res->ret);
+                }     
             }
 		});
+#if 0
+auto motor_info_srv=node->create_service<interfaces::srv::MotorInfo>("motor_info",[=](const std::shared_ptr<interfaces::srv::MotorInfo::Request> req,std::shared_ptr<interfaces::srv::MotorInfo::Response> res){
+            printf("recv motor_info:%s,%d\n",req->type.c_str(),req->motor_ids[0]);
+	    res->ret=false;
+	    if(req->type=="bm"){
+	    	  res->motor_angles.clear();
+	          for(auto d:req->motor_ids){
+                      float angle=rs485_driver_.bm_get_pos(d);
+	      	      res->motor_angles.push_back(angle);
+                      printf("motor_info %d, angle:%.1f\n",d,angle);
+		  }
+                  res->ret=true;
+	    }
+     });
 
-
+#endif
 
     rclcpp::spin(node);
     printf("now close rs485 port\n");

motor_dev/src/rs485_driver.cpp

@@ -10,52 +10,6 @@
 namespace motor_dev
 {
 
-    std::vector<uint8_t> RS485Driver::sendCan(uint8_t cmd,const std::vector<uint8_t> data) {
-        std::vector<uint8_t> packet;
-        packet.push_back(0x55);  // 同步字节1
-        packet.push_back(0xAA);  // 同步字节2
-        packet.push_back(0x1E);  // 30字节
-        
-        packet.push_back(0x01);
-        packet.push_back(0x01);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x0a);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-
-        packet.push_back(cmd);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        packet.push_back(0x08);
-        packet.push_back(0x00);
-        packet.push_back(0x00);
-        
-        for (uint8_t i = 0; i < 8; i++) {
-            packet.push_back(data[i]); 
-        }
-        packet.push_back(0x00);
-        bool ret = sendData(packet);
-        std::vector<uint8_t> rx_data;
-        if(ret){
-            rx_data.clear();
-            ret=receiveData(rx_data, 100, 1);
-            if(ret){
-                printf("<---:");
-                for(auto d:rx_data)
-                    printf("%02x ",d);
-                printf("\n");
-                return rx_data;
-            }
-        }
-        return rx_data;
-    }
-
     const unsigned char CRC8Table[]={  
         0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65,  
         157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220,  
@@ -220,10 +174,12 @@ namespace motor_dev
     {
         static int idx=0;
         idx+=1;
+#ifdef hehe
         printf("[%03d]-->",idx);
         for(auto d:data)
             printf("%02x ",d);
         printf("\n");
+#endif
         ssize_t bytes_written = write(com_fd_, data.data(), data.size());
         if (bytes_written != static_cast<ssize_t>(data.size()))
         {
@@ -294,7 +250,7 @@ namespace motor_dev
         bool ret = sendData(command);
         if(ret){
             std::vector<uint8_t> data;
-            bool ret=receiveData(data, 20, 1);
+            bool ret=receiveData(data, 20, 20);
             if(ret){
                 printf("recv:%02x,%02x,%02x,%02x\n",data[0],data[1],data[2],data[3]);
             }else{
@@ -313,19 +269,19 @@ namespace motor_dev
         command.push_back(0x36);
         command.push_back(motor_id);
         command.push_back(0x1c);
+        command.push_back(mode);
         command.push_back(0);
         command.push_back(0);
         command.push_back(0);
-        command.push_back(2);
         command.push_back(0);
         command.push_back(0);
-        //uint8_t crc8 = CRC8_Table(command.data(), 10);
-        //command.push_back(crc8);
+        uint8_t crc8 = CRC8_Table(command.data(), 10);
+        command.push_back(crc8);
         bool ret = sendData(command);
         if(ret){
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 20, 20);
             if(ret){
                 printf("<---:");
                 for(auto d:data)
@@ -337,6 +293,65 @@ namespace motor_dev
     }
     
     
+     int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
+        std::vector<uint8_t> packet;
+        packet.push_back(0);
+        packet.push_back(0x36);
+        packet.push_back(motor_id);
+        packet.push_back(param);
+        
+        packet.push_back((uint8_t)(val));
+        packet.push_back((uint8_t)(val>>8));
+        packet.push_back((uint8_t)(val>>16));
+        packet.push_back((uint8_t)(val>>24));
+        packet.push_back(0);
+        packet.push_back(0);
+        uint8_t crc8 = CRC8_Table(packet.data(), 10);
+        packet.push_back(crc8);
+        bool ret = sendData(packet);
+        if(ret){
+            std::vector<uint8_t> data;
+            data.clear();
+            ret=receiveData(data, 20, 20);
+            if(ret){
+                printf("<---:");
+                for(auto d:data)
+                    printf("%02x ",d);
+                printf("\n");
+            }
+        }
+        return 0;
+    }
+    int RS485Driver::bm_set_enable(uint8_t motor_id,uint8_t enable){
+        std::vector<uint8_t> command;
+        command.push_back(0);
+        command.push_back(0x38);
+
+        command.push_back(motor_id);
+        command.push_back(enable);
+        command.push_back(0);
+        command.push_back(0);
+        command.push_back(0);
+        command.push_back(0);
+        command.push_back(0);
+        command.push_back(0);
+      
+        uint8_t crc8 = CRC8_Table(command.data(), 10);
+        command.push_back(crc8);
+        bool ret = sendData(command);
+        if(ret){
+            std::vector<uint8_t> data;
+            data.clear();
+            ret=receiveData(data, 20, 20);
+            if(ret){
+                printf("<---:");
+                for(auto d:data)
+                    printf("%02x ",d);
+                printf("\n");
+            }
+        }
+        return 0;
+    }
     int RS485Driver::bm_set_pos(float angle1,float angle2){
         std::vector<uint8_t> command;
         //if(angle>360.0)
@@ -362,10 +377,11 @@ namespace motor_dev
         unsigned char crc8 = CRC8_Table(command.data(), 10);
         command.push_back(crc8);
         bool ret = sendData(command);
+#if 0
         if(ret){
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 20, 20);
             if(ret){
                 printf("<---:");
                 for(auto d:data)
@@ -373,62 +389,47 @@ namespace motor_dev
                 printf("\n");
             }
         }
+#endif
         return 0;
     }
-    int RS485Driver::bm_set_speed(float speed1,float speed2,float speed3,float speed4){
+    int RS485Driver::bm_set_speed(float speed1,float speed2){
         std::vector<uint8_t> command;
+        //if(angle>360.0)
+        //    angle=360.0;
         int32_t pos=speed1*10;
         uint8_t high1 = (pos >> 8) & 0xFF;  // 高8位
         uint8_t low1  = pos & 0xFF;         // 低8位
         pos=speed2*10;
         uint8_t high2 = (pos >> 8) & 0xFF;  // 高8位
         uint8_t low2  = pos & 0xFF;         // 低8位
-        pos=speed3*10;
-        uint8_t high3 = (pos >> 8) & 0xFF;  // 高8位
-        uint8_t low3  = pos & 0xFF;         // 低8位
-        pos=speed4*10;
-        uint8_t high4 = (pos >> 8) & 0xFF;  // 高8位
-        uint8_t low4  = pos & 0xFF;         // 低8位
-
-        command.push_back(high1);
-        command.push_back(low1);
-        command.push_back(high2);
-        command.push_back(low2);
-        command.push_back(high3);
-        command.push_back(low3);
-        command.push_back(high4);
-        command.push_back(low4);
-        printf("set_speed.%.1f\n",speed1);
-        sendCan(0x32,command);
-        return 0;
-    }
-    int RS485Driver::bm_set_enable(uint8_t enable){
-        std::vector<uint8_t> command;
-        command.push_back(enable);
-        command.push_back(enable);
-        command.push_back(enable);
-        command.push_back(enable);
-        command.push_back(0);
-        command.push_back(0);
-        command.push_back(0);
-        command.push_back(0);
-        printf("set_enable\n");
-        sendCan(0x38,command);
-        return 0;
-    }
-     int RS485Driver::bm_set_param(uint8_t motor_id, uint8_t param,uint32_t val){
-        std::vector<uint8_t> command;
-        command.push_back(motor_id);
-        command.push_back(param);
-        
-        command.push_back((uint8_t)(val));
-        command.push_back((uint8_t)(val>>8));
-        command.push_back((uint8_t)(val>>16));
-        command.push_back((uint8_t)(val>>24));
-        command.push_back(0);
-        command.push_back(0);
-        printf("set_param:%d,%d,%d\n",motor_id,param,val);
-        sendCan(0x36,command);
+        //if(motor_id<5){
+            command.push_back(0x00);
+            command.push_back(0x32);
+            command.push_back(high1);
+            command.push_back(low1);
+            command.push_back(high2);
+            command.push_back(low2);
+            command.push_back(0);
+            command.push_back(0);
+            command.push_back(0);
+            command.push_back(0);
+        //}
+        unsigned char crc8 = CRC8_Table(command.data(), 10);
+        command.push_back(crc8);
+        bool ret = sendData(command);
+        if(ret){
+            std::vector<uint8_t> data;
+            data.clear();
+            ret=receiveData(data, 20, 5);
+            if(ret){
+#ifdef hehe
+                printf("<---:");
+                for(auto d:data)
+                    printf("%02x ",d);
+                printf("\n");
+#endif
+            }
+        }
         return 0;
     }
     float RS485Driver::bm_get_pos(uint8_t motor_id){
@@ -449,12 +450,14 @@ namespace motor_dev
         if(ret){
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 11, 1);
             if(ret){
+#ifdef hehe
                 printf("<---:");
                 for(auto d:data)
                     printf("%02x ",d);
                 printf("\n");
+#endif
                 uint8_t nid=0x70+motor_id;
                 if(data[0]==0&&data[1]==nid){
                     //uint16_t pos = (data[2]<<8)+(data[3]&0xff);
@@ -475,26 +478,54 @@ namespace motor_dev
         }
         return -1;
     }
-    int RS485Driver::bm_get_speed(){
+    float RS485Driver::bm_get_speed(uint8_t motor_id){
         std::vector<uint8_t> command;
+        command.push_back(0);
+        command.push_back(0x35);
+        command.push_back(motor_id);
+        //command.push_back(0x0d);
+        //command.push_back(4);
+        //command.push_back(0x0b);
+        command.push_back(0);
+        command.push_back(0);
+        command.push_back(0);
         command.push_back(1);
-        command.push_back(5);
-        command.push_back(11);
-        command.push_back(13);
         command.push_back(0);
         command.push_back(0);
         command.push_back(0);
-        command.push_back(0);
-        std::vector<uint8_t> rx=sendCan(0x35,command);
-        speedMap.clear();
-        for(int x=0;x<rx.size();x++)
-            if(rx[x]==0xaa&&rx[x+1]==0x11&&rx[x+2]==0x08){
-                uint8_t motor_id=rx[x+3]-0x70;
-                int16_t speed=(rx[x+7]<<8)+(rx[x+8]&0xff);
-                printf("id:%d,speed:%d\n",motor_id,speed);
-                speedMap[motor_id]=speed/10.0f;
+        unsigned char crc8 = CRC8_Table(command.data(), 10);
+        command.push_back(crc8);
+        bool ret = sendData(command);
+        if(ret){
+            std::vector<uint8_t> data;
+            data.clear();
+            ret=receiveData(data, 20, 50);
+            if(ret){
+                printf("<---:");
+                for(auto d:data)
+                    printf("%02x ",d);
+                printf("\n");
+                uint8_t nid=0x70+motor_id;
+                if(data[0]==0&&data[1]==nid){
+                    //uint16_t pos = (data[2]<<8)+(data[3]&0xff);
+                    //float angle=360.0*pos/32768.0;
+                    int16_t pos=(data[6]<<8)+(data[7]&0xff);
+                    float angle=3.60f*pos;
+                    pos=(data[8]<<8)+(data[9]&0xff);
+                    float speed=pos/10.0f;
+                    //printf("id:%d,pos:%d\n",motor_id,pos);
+                    return speed;
+                    ///printf("pos:%d,angle:%.1f\n",pos,angle);
+                }
+                else{
+
+                }
+                //float angle = pos * 360.0 /255.0;
+                ///float angle=0;
+                
             }
-        return 0;
+        }
+        return -1;
     }
     //mt
     void RS485Driver::add_crc16(std::vector<uint8_t>& data) {
@@ -559,7 +590,7 @@ namespace motor_dev
             #if 1
             std::vector<uint8_t> data;
             data.clear();
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 20, 20);
             if(ret){
                 if(data[0]==0x3e&&data[1]==motor_id&&data[3]==0x92){
                     uint32_t pos = (data[10]<<24)+(data[9]<<16)+(data[8]<<8)+(data[7]&0xff);
@@ -600,7 +631,7 @@ namespace motor_dev
         bool ret=sendData(packet);
         if(ret){
             std::vector<uint8_t> data;
-            ret=receiveData(data, 20, 1);
+            ret=receiveData(data, 20, 20);
             if(ret){
                 uint8_t id= data[10];
                 return id;

openzen_dev/launch/openzen_lpms.launch.py

@@ -13,11 +13,11 @@ def generate_launch_description():
         # Note: topic tools not ported to ROS2 yet, so no easy conversion
         #       from quaternion to euler available (https://github.com/ros2/ros2/issues/857)
 
-        Node(
-            package="rqt_plot",
-            executable="rqt_plot",
-            name="ig1_data_plotter",
-            namespace="openzen",
-            arguments=["/openzen/data/angular_velocity"]
-        )
+        # Node(
+        #     package="rqt_plot",
+        #     executable="rqt_plot",
+        #     name="ig1_data_plotter",
+        #     namespace="openzen",
+        #     arguments=["/openzen/data/angular_velocity"]
+        # )
     ])

waveshare/launch/imu_view.launch.py

@@ -9,7 +9,7 @@ from launch.launch_description_sources import PythonLaunchDescriptionSource
 def generate_launch_description():
     # Declare launch arguments
     port_name_arg = DeclareLaunchArgument(
-        'port_name', default_value='/dev/ttyUSB0',
+        'port_name', default_value='/dev/ttyUSB1',
         description='Choose which port to use'
     )
 

waveshare/src/imu_node.cpp

@@ -23,7 +23,7 @@ int main(int argc, char *argv[])
     setlocale(LC_ALL,"");//Set encoding to prevent Chinese garbled characters
     rclcpp::init(argc, argv);
     auto node = std::make_shared<rclcpp::Node>("imu_node");
-    node->declare_parameter<std::string>("port_name", "/dev/ttyUSB0");
+    node->declare_parameter<std::string>("port_name", "/dev/ttyUSB0");//这个设备路径对不对？要确认夏
     std::string port_name;
     node->get_parameter<std::string>("port_name", port_name);//Get parameters