From 55735ac85c73dd38caea76392b15fcebc9ddd258 Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Tue, 13 Jan 2026 15:49:16 +0000 Subject: [PATCH 01/10] Better trajectory republishing --- .../republish_trajectory_v2.py | 81 +++++++++++++++++++ 1 file changed, 81 insertions(+) create mode 100755 sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py new file mode 100755 index 00000000..6b34f1f9 --- /dev/null +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -0,0 +1,81 @@ +#!/usr/bin/env python3 + +# Copyright 2026 Shadow Robot Company Ltd. +# +# This program is free software: you can redistribute it and/or modify it +# under the terms of the GNU General Public License as published by the Free +# Software Foundation version 2 of the License. +# +# This program is distributed in the hope that it will be useful, but WITHOUT +# ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or +# FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for +# more details. +# +# You should have received a copy of the GNU General Public License along +# with this program. If not, see . + +# WHEN TO USE: +# Use this script when you have a rosbag and wand to play the trajectory data to a hand +# or when you are using a right glove (i.e. cyberglove) and you are using a left shadow hand. +# +# HOW TO USE: +# To use the trajectory republisher, select if you are using a left hand. +# Then, select which joints you want to move (don't change the prefix). +# Run your publisher and then start your rosbag by remapping your topic for instance +# by adding the following line in the end of your rosbag command +# +# /rh_trajectory_controller/command:=/rh_trajectory_controller/command_remapped +# +# Your hand should be start moving. + +from threading import Lock +from functools import partial +import rospy +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint + + +class RePubTrajectory: + def __init__(self, topics_to_republish, subscribed_subtopic, published_subtopic): + self._mutex = Lock() + self._topics_first_msg_timestamp = None # Timestamp of first msg received amongst all topics to republish + self._current_time_of_first_msg = None # Current time when first msg was received + + for topic in topics_to_republish: + self._traj_pub = rospy.Publisher(topic + published_subtopic, + JointTrajectory, + queue_size=10) + self._bag_tf_sub = rospy.Subscriber(topic + subscribed_subtopic, + JointTrajectory, + partial(self._bag_traj_cb, self._traj_pub)) + + def _bag_traj_cb(self, republisher, data): + if self._topics_first_msg_timestamp is None: + with self._mutex: + self._topics_first_msg_timestamp = data.header.stamp + self._first_msg_current_time = rospy.Time.now() + + new_traj = JointTrajectory() + + new_traj.header = data.header + new_traj.header.stamp = data.header.stamp - self._topics_first_msg_timestamp + self._first_msg_current_time + new_traj.header.stamp += rospy.Time.from_sec(50 / 1000) # Shift timestamp to 50ms in the future + + new_traj.points = data.points + new_traj.joint_names = data.joint_names + + republisher.publish(new_traj) + + +if __name__ == "__main__": + rospy.init_node("republish_trajectory") + + topics_to_republish = ["/rh_trajectory_controller", + "/lh_trajectory_controller", + "/ra_trajectory_controller", + "/la_trajectory_controller", + "/rh_wr_trajectory_controller", + "/lh_wr_trajectory_controller"] + + republishers = RePubTrajectory(topics_to_republish, "/command_remapped", "/command") + + rospy.spin() From 253d72dbcc2e91e5bb27085fab715739d78e2f79 Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Wed, 14 Jan 2026 10:35:17 +0000 Subject: [PATCH 02/10] tidying and bug fixes --- .../sr_utilities_common/republish_trajectory_v2.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index 6b34f1f9..25965f4a 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -28,14 +28,15 @@ # # Your hand should be start moving. +from typing import List from threading import Lock from functools import partial import rospy -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from trajectory_msgs.msg import JointTrajectory class RePubTrajectory: - def __init__(self, topics_to_republish, subscribed_subtopic, published_subtopic): + def __init__(self, topics_to_republish: List[str], subscribed_subtopic: str, published_subtopic: str): self._mutex = Lock() self._topics_first_msg_timestamp = None # Timestamp of first msg received amongst all topics to republish self._current_time_of_first_msg = None # Current time when first msg was received @@ -48,7 +49,7 @@ def __init__(self, topics_to_republish, subscribed_subtopic, published_subtopic) JointTrajectory, partial(self._bag_traj_cb, self._traj_pub)) - def _bag_traj_cb(self, republisher, data): + def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): if self._topics_first_msg_timestamp is None: with self._mutex: self._topics_first_msg_timestamp = data.header.stamp @@ -58,7 +59,7 @@ def _bag_traj_cb(self, republisher, data): new_traj.header = data.header new_traj.header.stamp = data.header.stamp - self._topics_first_msg_timestamp + self._first_msg_current_time - new_traj.header.stamp += rospy.Time.from_sec(50 / 1000) # Shift timestamp to 50ms in the future + new_traj.header.stamp += rospy.Duration.from_sec(10 / 1000) # Shift timestamp to 10ms in the future new_traj.points = data.points new_traj.joint_names = data.joint_names From 46f186ace914048716f378f42a12a997fbdc7872 Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Wed, 14 Jan 2026 10:49:58 +0000 Subject: [PATCH 03/10] Better docs --- .../republish_trajectory_v2.py | 29 ++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index 25965f4a..23f4c657 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -14,19 +14,22 @@ # You should have received a copy of the GNU General Public License along # with this program. If not, see . -# WHEN TO USE: -# Use this script when you have a rosbag and wand to play the trajectory data to a hand -# or when you are using a right glove (i.e. cyberglove) and you are using a left shadow hand. -# -# HOW TO USE: -# To use the trajectory republisher, select if you are using a left hand. -# Then, select which joints you want to move (don't change the prefix). -# Run your publisher and then start your rosbag by remapping your topic for instance -# by adding the following line in the end of your rosbag command -# -# /rh_trajectory_controller/command:=/rh_trajectory_controller/command_remapped -# -# Your hand should be start moving. +''' +WHEN TO USE: +Use this script when you have a rosbag and wand to play the trajectory data to a hand or hand and arm system. +This tool is intended to work on all hand and arm variations (unimanual, bimanual, hand only, etc). + +HOW TO USE: +1. Start the hardware control loop of the system +2. Start the republisher node +3. Play the rosbag you want to replay the trajectory from. Ensure you define all topics you want to play and remap + them. An example command would be: + ``` + rosbag play my_trajectory.bag --topics /rh_trajectory_controller/command /ra_trajectory_controller/command + /rh_trajectory_controller/command:=/rh_trajectory_controller/command_remapped + /ra_trajectory_controller/command:=/ra_trajectory_controller/command_remapped + ``` +''' from typing import List from threading import Lock From 760a337bb9246a3f12444153020ad194d4157a91 Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Wed, 14 Jan 2026 11:00:09 +0000 Subject: [PATCH 04/10] Const --- .../scripts/sr_utilities_common/republish_trajectory_v2.py | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index 23f4c657..e4c77c19 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -39,6 +39,11 @@ class RePubTrajectory: + + # Amount by which republished msgs are shifted to the future to ensure they arrive at the + # controller before their timestamp + FUTURE_SHIFT = 10 / 1000 + def __init__(self, topics_to_republish: List[str], subscribed_subtopic: str, published_subtopic: str): self._mutex = Lock() self._topics_first_msg_timestamp = None # Timestamp of first msg received amongst all topics to republish @@ -62,7 +67,7 @@ def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): new_traj.header = data.header new_traj.header.stamp = data.header.stamp - self._topics_first_msg_timestamp + self._first_msg_current_time - new_traj.header.stamp += rospy.Duration.from_sec(10 / 1000) # Shift timestamp to 10ms in the future + new_traj.header.stamp += rospy.Duration.from_sec(self.FUTURE_SHIFT) # Shift timestamp to the future new_traj.points = data.points new_traj.joint_names = data.joint_names From 064a5aebd3b387872f07964bdd8c294e2d0ad96b Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Wed, 14 Jan 2026 11:00:49 +0000 Subject: [PATCH 05/10] fix typos --- .../scripts/sr_utilities_common/republish_trajectory_v2.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index e4c77c19..e248ecbf 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -16,7 +16,7 @@ ''' WHEN TO USE: -Use this script when you have a rosbag and wand to play the trajectory data to a hand or hand and arm system. +Use this script when you have a rosbag and want to play the trajectory data on a hand or hand and arm system. This tool is intended to work on all hand and arm variations (unimanual, bimanual, hand only, etc). HOW TO USE: From df45580c106ba61e9a816260000a1e5a158e0606 Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Wed, 14 Jan 2026 12:02:56 +0000 Subject: [PATCH 06/10] Abraham Lintoln --- .../sr_utilities_common/republish_trajectory_v2.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index e248ecbf..caaf0e05 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -44,12 +44,12 @@ class RePubTrajectory: # controller before their timestamp FUTURE_SHIFT = 10 / 1000 - def __init__(self, topics_to_republish: List[str], subscribed_subtopic: str, published_subtopic: str): + def __init__(self, topics: List[str], subscribed_subtopic: str, published_subtopic: str): self._mutex = Lock() self._topics_first_msg_timestamp = None # Timestamp of first msg received amongst all topics to republish self._current_time_of_first_msg = None # Current time when first msg was received - for topic in topics_to_republish: + for topic in topics: self._traj_pub = rospy.Publisher(topic + published_subtopic, JointTrajectory, queue_size=10) @@ -61,12 +61,12 @@ def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): if self._topics_first_msg_timestamp is None: with self._mutex: self._topics_first_msg_timestamp = data.header.stamp - self._first_msg_current_time = rospy.Time.now() + self._current_time_of_first_msg = rospy.Time.now() new_traj = JointTrajectory() new_traj.header = data.header - new_traj.header.stamp = data.header.stamp - self._topics_first_msg_timestamp + self._first_msg_current_time + new_traj.header.stamp = data.header.stamp - self._topics_first_msg_timestamp + self._current_time_of_first_msg new_traj.header.stamp += rospy.Duration.from_sec(self.FUTURE_SHIFT) # Shift timestamp to the future new_traj.points = data.points From 3f9c4128c6ac54ef63ef5b49b9a86aa5def52f0d Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Wed, 14 Jan 2026 17:33:18 +0000 Subject: [PATCH 07/10] addressing comments --- .../republish_trajectory_v2.py | 29 +++++++++---------- 1 file changed, 14 insertions(+), 15 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index caaf0e05..59620434 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -49,13 +49,17 @@ def __init__(self, topics: List[str], subscribed_subtopic: str, published_subtop self._topics_first_msg_timestamp = None # Timestamp of first msg received amongst all topics to republish self._current_time_of_first_msg = None # Current time when first msg was received + self._traj_pubs = [] + self._bag_tf_subs = [] + for topic in topics: - self._traj_pub = rospy.Publisher(topic + published_subtopic, - JointTrajectory, - queue_size=10) - self._bag_tf_sub = rospy.Subscriber(topic + subscribed_subtopic, - JointTrajectory, - partial(self._bag_traj_cb, self._traj_pub)) + traj_pub = rospy.Publisher(topic + published_subtopic, JointTrajectory, queue_size=10) + bag_tf_sub = rospy.Subscriber(topic + subscribed_subtopic, JointTrajectory, + partial(self._bag_traj_cb, traj_pub)) + # Using a partial here to reuse the same callback method with multiple publishers + + self._traj_pubs.append(traj_pub) + self._bag_tf_subs.append(bag_tf_sub) def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): if self._topics_first_msg_timestamp is None: @@ -63,16 +67,11 @@ def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): self._topics_first_msg_timestamp = data.header.stamp self._current_time_of_first_msg = rospy.Time.now() - new_traj = JointTrajectory() - - new_traj.header = data.header - new_traj.header.stamp = data.header.stamp - self._topics_first_msg_timestamp + self._current_time_of_first_msg - new_traj.header.stamp += rospy.Duration.from_sec(self.FUTURE_SHIFT) # Shift timestamp to the future - - new_traj.points = data.points - new_traj.joint_names = data.joint_names + data.header.stamp -= self._topics_first_msg_timestamp + data.header.stamp += self._current_time_of_first_msg + data.header.stamp += rospy.Duration.from_sec(self.FUTURE_SHIFT) # Shift timestamp to the future - republisher.publish(new_traj) + republisher.publish(data) if __name__ == "__main__": From c0a5299e948c71f2e3354978abe565fc06016d27 Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Mon, 19 Jan 2026 14:42:51 +0000 Subject: [PATCH 08/10] Added watchdog to reset node if no bag topics arrive after 3 seconds --- .../republish_trajectory_v2.py | 24 ++++++++++++++++--- 1 file changed, 21 insertions(+), 3 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index 59620434..776d5c57 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -44,10 +44,15 @@ class RePubTrajectory: # controller before their timestamp FUTURE_SHIFT = 10 / 1000 + # Time in seconds after which the node will reset if not topics are received. Node reset means the + # "topics_first_msg_timestamp" variable will be reset to None + NODE_RESET_TIME = 3 + def __init__(self, topics: List[str], subscribed_subtopic: str, published_subtopic: str): self._mutex = Lock() self._topics_first_msg_timestamp = None # Timestamp of first msg received amongst all topics to republish self._current_time_of_first_msg = None # Current time when first msg was received + self._watchdog_current_time = rospy.get_time() self._traj_pubs = [] self._bag_tf_subs = [] @@ -62,8 +67,10 @@ def __init__(self, topics: List[str], subscribed_subtopic: str, published_subtop self._bag_tf_subs.append(bag_tf_sub) def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): - if self._topics_first_msg_timestamp is None: - with self._mutex: + with self._mutex: + self._watchdog_current_time = rospy.get_time() + + if self._topics_first_msg_timestamp is None: self._topics_first_msg_timestamp = data.header.stamp self._current_time_of_first_msg = rospy.Time.now() @@ -73,6 +80,17 @@ def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): republisher.publish(data) + def spin(self): + while not rospy.is_shutdown(): + if self._topics_first_msg_timestamp is None: + continue + + if rospy.get_time() - self._watchdog_current_time > self.NODE_RESET_TIME: + rospy.logwarn(f"Resetting node. No bag topics received in the last {self.NODE_RESET_TIME} seconds") + + with self._mutex: + self._topics_first_msg_timestamp = None + if __name__ == "__main__": rospy.init_node("republish_trajectory") @@ -86,4 +104,4 @@ def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): republishers = RePubTrajectory(topics_to_republish, "/command_remapped", "/command") - rospy.spin() + republishers.spin() From f6d7e3a44a0a6d75a31295b7b3bfae1b228ad074 Mon Sep 17 00:00:00 2001 From: niko-holmes Date: Mon, 19 Jan 2026 16:03:08 +0000 Subject: [PATCH 09/10] sleep the loop --- .../scripts/sr_utilities_common/republish_trajectory_v2.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index 776d5c57..dcf2c29d 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -91,6 +91,8 @@ def spin(self): with self._mutex: self._topics_first_msg_timestamp = None + rospy.sleep(1.0) + if __name__ == "__main__": rospy.init_node("republish_trajectory") From 20ae3b915d78fdf942c75fc814c02ac970ade71b Mon Sep 17 00:00:00 2001 From: Nikolaus Holmes <143398822+niko-holmes@users.noreply.github.com> Date: Wed, 28 Jan 2026 14:50:20 +0000 Subject: [PATCH 10/10] Update republish_trajectory_v2.py --- .../scripts/sr_utilities_common/republish_trajectory_v2.py | 1 + 1 file changed, 1 insertion(+) diff --git a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py index dcf2c29d..e084477b 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py +++ b/sr_utilities_common/scripts/sr_utilities_common/republish_trajectory_v2.py @@ -83,6 +83,7 @@ def _bag_traj_cb(self, republisher: rospy.Publisher, data: JointTrajectory): def spin(self): while not rospy.is_shutdown(): if self._topics_first_msg_timestamp is None: + rospy.sleep(1.0) continue if rospy.get_time() - self._watchdog_current_time > self.NODE_RESET_TIME: