openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

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

Browse Source
This commit is contained in:
shenchenyang 2026-04-02 17:39:38 +08:00
parent 470d94315f
commit b1e945ed92
2 changed files with 19 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

17
src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py
View File

@ -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.处理目标发送后的反馈；

8
src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py
View File

@ -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}")