From 62fd1b110de2404e4b9d06539ca95c805c6b3a54 Mon Sep 17 00:00:00 2001 From: David Sharpe Date: Sun, 8 Mar 2026 03:57:28 -0500 Subject: [PATCH] refactor: remedy QoS profiles --- src/arm_pkg/arm_pkg/arm_node.py | 24 ++++++++++++------------ src/core_pkg/core_pkg/core_node.py | 6 +++--- src/headless_pkg/src/headless_node.py | 17 +++++------------ 3 files changed, 20 insertions(+), 27 deletions(-) diff --git a/src/arm_pkg/arm_pkg/arm_node.py b/src/arm_pkg/arm_pkg/arm_node.py index 79ea3ea..7465045 100644 --- a/src/arm_pkg/arm_pkg/arm_node.py +++ b/src/arm_pkg/arm_pkg/arm_node.py @@ -14,16 +14,16 @@ from control_msgs.msg import JointJog from astra_msgs.msg import SocketFeedback, DigitFeedback, ArmManual from astra_msgs.msg import ArmFeedback, VicCAN, RevMotorState -# control_qos = qos.QoSProfile( -# history=qos.QoSHistoryPolicy.KEEP_LAST, -# depth=1, -# reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, -# durability=qos.QoSDurabilityPolicy.VOLATILE, -# deadline=1000, -# lifespan=500, -# liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, -# liveliness_lease_duration=5000 -# ) +control_qos = qos.QoSProfile( + history=qos.QoSHistoryPolicy.KEEP_LAST, + depth=2, + reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, # Best Effort subscribers are still compatible with Reliable publishers + durability=qos.QoSDurabilityPolicy.VOLATILE, + # deadline=Duration(seconds=1), + # lifespan=Duration(nanoseconds=500_000_000), # 500ms + # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, + # liveliness_lease_duration=Duration(seconds=5), +) thread = None @@ -112,11 +112,11 @@ class ArmNode(Node): # Manual: /arm/manual_new is published by Servo or Basestation self.jointjog_pub_ = self.create_subscription( - JointJog, "/arm/manual_new", self.jointjog_callback, 1 + JointJog, "/arm/manual_new", self.jointjog_callback, qos_profile=control_qos ) # IK: /joint_commands is published by JointTrajectoryController via topic_based_control self.joint_command_sub_ = self.create_subscription( - JointState, "/joint_commands", self.joint_command_callback, 1 + JointState, "/joint_commands", self.joint_command_callback, qos_profile=control_qos ) # Feedback diff --git a/src/core_pkg/core_pkg/core_node.py b/src/core_pkg/core_pkg/core_node.py index b5ae8d8..edb5be4 100644 --- a/src/core_pkg/core_pkg/core_node.py +++ b/src/core_pkg/core_pkg/core_node.py @@ -31,10 +31,10 @@ CORE_WHEEL_RADIUS = 0.171 # meters CORE_GEAR_RATIO = 100.0 # Clucky: 100:1, Testbed: 64:1 control_qos = qos.QoSProfile( - # history=qos.QoSHistoryPolicy.KEEP_LAST, + history=qos.QoSHistoryPolicy.KEEP_LAST, depth=2, - # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, - # durability=qos.QoSDurabilityPolicy.VOLATILE, + reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, # Best Effort subscribers are still compatible with Reliable publishers + durability=qos.QoSDurabilityPolicy.VOLATILE, # deadline=Duration(seconds=1), # lifespan=Duration(nanoseconds=500_000_000), # 500ms # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, diff --git a/src/headless_pkg/src/headless_node.py b/src/headless_pkg/src/headless_node.py index 7d840cc..ee05160 100755 --- a/src/headless_pkg/src/headless_node.py +++ b/src/headless_pkg/src/headless_node.py @@ -37,23 +37,16 @@ ARM_STOP_MSG = ArmManual() # " BIO_STOP_MSG = BioControl() # " control_qos = qos.QoSProfile( - # history=qos.QoSHistoryPolicy.KEEP_LAST, + history=qos.QoSHistoryPolicy.KEEP_LAST, depth=2, - # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, - # durability=qos.QoSDurabilityPolicy.VOLATILE, + reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, + durability=qos.QoSDurabilityPolicy.VOLATILE, # deadline=Duration(seconds=1), # lifespan=Duration(nanoseconds=500_000_000), # 500ms # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT, # liveliness_lease_duration=Duration(seconds=5), ) -arm_ik_qos = qos.QoSProfile( - history=qos.QoSHistoryPolicy.KEEP_LAST, - depth=1, - reliability=qos.QoSReliabilityPolicy.BEST_EFFORT, - durability=qos.QoSDurabilityPolicy.VOLATILE, -) - STICK_DEADZONE = float(os.getenv("STICK_DEADZONE", "0.05")) @@ -187,10 +180,10 @@ class Headless(Node): ) self.arm_ik_twist_publisher = self.create_publisher( - TwistStamped, "/servo_node/delta_twist_cmds", arm_ik_qos + TwistStamped, "/servo_node/delta_twist_cmds", qos_profile=control_qos ) self.arm_ik_jointjog_publisher = self.create_publisher( - JointJog, "/servo_node/delta_joint_cmds", arm_ik_qos + JointJog, "/servo_node/delta_joint_cmds", qos_profile=control_qos ) ##################################################