From b1e945ed92075eda83b42f1af19fb6dc60be5037 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Thu, 2 Apr 2026 17:39:38 +0800 Subject: [PATCH] =?UTF-8?q?=E5=A2=9E=E5=8A=A0=E9=98=9F=E5=88=97=E7=BC=93?= =?UTF-8?q?=E5=AD=98=EF=BC=8C=E9=99=8D=E4=BD=8E=E5=8F=91=E5=B8=83=E9=A2=91?= =?UTF-8?q?=E7=8E=87?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../openarm_joint_sensor/joint_control.py | 19 +++++++++++++++---- .../openarm_joint_sensor/joint_sensor_can.py | 8 ++++---- 2 files changed, 19 insertions(+), 8 deletions(-) diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py index 2e0e443..1aff38e 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py +++ b/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,7 +135,14 @@ class ProgressActionClient(Node): else: arm_pos_to_send = arm_pos - self.send_goal(arm_pos_to_send) + 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): arm_joint = ['openarm_right_joint1', 'openarm_right_joint2', 'openarm_right_joint3', 'openarm_right_joint4', 'openarm_right_joint5', 'openarm_right_joint6', 'openarm_right_joint7'] @@ -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.处理目标发送后的反馈； diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py index 9f2caee..484f9db 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py +++ b/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( - f"Published JointState: id=0x{msg.arbitration_id:X} " - f"joint={self.joint_names[joint_index]} pos={angle:.6f} vel={vel:.6f}" - ) + # self.get_logger().info( + # f"Published JointState: id=0x{msg.arbitration_id:X} " + # 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}")