增加队列缓存，降低发布频率

src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py

@@ -1,5 +1,6 @@
 import rclpy
 from rclpy.node import Node
+from collections import deque
 # from control_msgs.msg import JointTrajectoryControllerState
 from control_msgs.action import FollowJointTrajectory
 from rclpy.action import ActionClient
@@ -67,7 +68,7 @@ class ProgressActionClient(Node):
         # Parameters for optional position filtering
         self.declare_parameter('enable_filter', True)
         self.declare_parameter('filter_type', 'ema')
-        self.declare_parameter('ema_alpha', 0.5)
+        self.declare_parameter('ema_alpha', 0.15)
         self.declare_parameter('kalman_q', 1e-5)
         self.declare_parameter('kalman_r', 1e-2)
 
@@ -85,6 +86,9 @@ class ProgressActionClient(Node):
                                        r=self.kalman_r)
                         for _ in range(self.num_joints)]
 
+        # Buffer outgoing joint commands and send them at timer rate.
+        self.command_queue = deque(maxlen=100)
+
         self.subscriber_ = self.create_subscription(
             JointState, 
             "/joint_sensor_states", 
@@ -95,7 +99,7 @@ class ProgressActionClient(Node):
         self.float64MultiArrayPublisher_ = self.create_publisher(Float64MultiArray, "/right_forward_position_controller/commands", 10)
         # 3-1.创建动作客户端；
         self._action_client = ActionClient(self, FollowJointTrajectory, '/right_joint_trajectory_controller/follow_joint_trajectory')
- 
+        self.create_timer(0.01, self.timer_callback)  # 定时器，周期为0.1秒
         # 3. 创建订阅者
         # 参数1：消息类型（如String，与发布者一致）
         # 参数2：话题名称（如"chatter"，与发布者一致）
@@ -131,6 +135,13 @@ class ProgressActionClient(Node):
         else:
             arm_pos_to_send = arm_pos
 
+        self.command_queue.append(arm_pos_to_send)
+        # self.send_goal(arm_pos_to_send)
+    
+    def timer_callback(self):
+        # 定时器回调函数，可以用于周期性任务
+        if self.command_queue:
+            arm_pos_to_send = self.command_queue.popleft()
             self.send_goal(arm_pos_to_send)
 
     def send_goal(self, arm_pos):
@@ -151,7 +162,7 @@ class ProgressActionClient(Node):
         msg = Float64MultiArray()
         msg.data = arm_pos
         self.float64MultiArrayPublisher_.publish(msg)
-        self.get_logger().debug(f'Published joint commands: {arm_pos}')
+        self.get_logger().info(f'Published joint commands: {arm_pos}')
  
     def goal_response_callback(self, future):
         # 3-3.处理目标发送后的反馈；

src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py

@@ -99,10 +99,10 @@ class JointSensorNode(Node):
             js.velocity = self.current_velocities
             self.publisher_.publish(js)
 
-            self.get_logger().info(
+            # self.get_logger().info(
-                f"Published JointState: id=0x{msg.arbitration_id:X} "
+            #     f"Published JointState: id=0x{msg.arbitration_id:X} "
-                f"joint={self.joint_names[joint_index]} pos={angle:.6f} vel={vel:.6f}"
+            #     f"joint={self.joint_names[joint_index]} pos={angle:.6f} vel={vel:.6f}"
-            )
+            # )
         except Exception as e:
             self.get_logger().error(f"Error handling CAN msg: {e}")