From f7d2a2ceb223c8c81528a40e2633bd092d8ef1af Mon Sep 17 00:00:00 2001 From: Rob9315 Date: Sun, 5 Jan 2025 05:39:37 +0000 Subject: [PATCH 1/3] add 'relay_sources' option --- joint_state_publisher/README.md | 1 + .../joint_state_publisher/joint_state_publisher.py | 5 +++++ 2 files changed, 6 insertions(+) diff --git a/joint_state_publisher/README.md b/joint_state_publisher/README.md index 7d3b6af..02a0052 100644 --- a/joint_state_publisher/README.md +++ b/joint_state_publisher/README.md @@ -24,6 +24,7 @@ Parameters * `use_mimic_tags` (bool) - Whether to honor `` tags in the URDF. Defaults to True. * `use_smallest_joint_limits` (bool) - Whether to honor `` tags in the URDF. Defaults to True. * `source_list` (array of strings) - Each string in this array represents a topic name. For each string, create a subscription to the named topic of type `sensor_msgs/msg/JointStates`. Publication to that topic will update the joints named in the message. Defaults to an empty array. +* `relay_sources` (bool) - Whether to immediately republish messages coming in on topics from `source_list`. Makes for smoother joints sometimes. Defaults to False. * `delta` (double) - How much to automatically move joints during each iteration. Defaults to 0.0. #### Mapped Parameters diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py index fc2ae96..565b725 100644 --- a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py +++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py @@ -364,6 +364,8 @@ def __init__(self, description_file): ParameterDescriptor(type=ParameterType.PARAMETER_INTEGER)) self.declare_ros_parameter('source_list', [], ParameterDescriptor(type=ParameterType.PARAMETER_STRING_ARRAY)) + self.declare_ros_parameter('relay_sources', True, + ParameterDescriptor(type=ParameterType.PARAMETER_BOOL)) self.declare_ros_parameter('use_mimic_tags', True, ParameterDescriptor(type=ParameterType.PARAMETER_BOOL)) self.declare_ros_parameter('use_smallest_joint_limits', True, @@ -458,6 +460,9 @@ def source_cb(self, msg): if self.source_update_cb is not None: self.source_update_cb() + + if self.get_param('relay_sources'): + self.pub.publish(msg) def set_source_update_cb(self, user_cb): self.source_update_cb = user_cb From 6feecfe8b69aea2a69196afe451dbdeca830afe7 Mon Sep 17 00:00:00 2001 From: Rob9315 Date: Sun, 5 Jan 2025 08:12:18 +0100 Subject: [PATCH 2/3] remove whitespace (linter) --- .../joint_state_publisher/joint_state_publisher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py index 565b725..206dac0 100644 --- a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py +++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py @@ -460,7 +460,7 @@ def source_cb(self, msg): if self.source_update_cb is not None: self.source_update_cb() - + if self.get_param('relay_sources'): self.pub.publish(msg) From eb2283637106bdf8890d5098c8326973edf79852 Mon Sep 17 00:00:00 2001 From: Rob9315 Date: Tue, 14 Jan 2025 19:40:00 +0100 Subject: [PATCH 3/3] Actually default to False --- .../joint_state_publisher/joint_state_publisher.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py index 206dac0..b67c93e 100644 --- a/joint_state_publisher/joint_state_publisher/joint_state_publisher.py +++ b/joint_state_publisher/joint_state_publisher/joint_state_publisher.py @@ -364,7 +364,7 @@ def __init__(self, description_file): ParameterDescriptor(type=ParameterType.PARAMETER_INTEGER)) self.declare_ros_parameter('source_list', [], ParameterDescriptor(type=ParameterType.PARAMETER_STRING_ARRAY)) - self.declare_ros_parameter('relay_sources', True, + self.declare_ros_parameter('relay_sources', False, ParameterDescriptor(type=ParameterType.PARAMETER_BOOL)) self.declare_ros_parameter('use_mimic_tags', True, ParameterDescriptor(type=ParameterType.PARAMETER_BOOL))