From 5af569d0c1c4d0cfb461269befe3a12dd35ba584 Mon Sep 17 00:00:00 2001 From: Build Tools Date: Tue, 11 Jun 2024 14:02:53 +0000 Subject: [PATCH 1/6] live tf republisher for rosbag record --- .../config/frames_to_remap.yaml | 24 ++++++ .../launch/live_tf_republisher.launch | 20 +++++ .../live_tf_republisher.py | 79 +++++++++++++++++++ 3 files changed, 123 insertions(+) create mode 100644 sr_utilities_common/config/frames_to_remap.yaml create mode 100644 sr_utilities_common/launch/live_tf_republisher.launch create mode 100755 sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py diff --git a/sr_utilities_common/config/frames_to_remap.yaml b/sr_utilities_common/config/frames_to_remap.yaml new file mode 100644 index 00000000..8120bedb --- /dev/null +++ b/sr_utilities_common/config/frames_to_remap.yaml @@ -0,0 +1,24 @@ +# Copyright 2024 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 . + +--- +sr_tf_live_republisher: + output_topic: '/tf_filtered_record' + frequency: 100 + parent_to_child_frames: + rh_palm: rh_fftip + rh_palm: rh_mftip + rh_palm: rh_rftip + rh_palm: rh_lftip + rh_palm: rh_thtip \ No newline at end of file diff --git a/sr_utilities_common/launch/live_tf_republisher.launch b/sr_utilities_common/launch/live_tf_republisher.launch new file mode 100644 index 00000000..51f276c2 --- /dev/null +++ b/sr_utilities_common/launch/live_tf_republisher.launch @@ -0,0 +1,20 @@ + + + + + + + + + \ No newline at end of file diff --git a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py new file mode 100755 index 00000000..5c94cb48 --- /dev/null +++ b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py @@ -0,0 +1,79 @@ +#!/usr/bin/env python3 + +# Copyright 2024 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 . + +import os +import argparse +import yaml +import rospy +import tf2_ros +import tf2_msgs + + +class SrLiveTfRepublisher: + def __init__(self, config_file): + loaded_config = self._load_and_validate_config(config_file) + self._parent_child_frames_dict = {} + for parent, child in loaded_config['parent_to_child_frames'].items(): + self._parent_child_frames_dict[parent] = child + output_topic = loaded_config['output_topic'] + frequency = loaded_config['frequency'] + self._tf_buffer = tf2_ros.Buffer() + self._tf_listener = tf2_ros.TransformListener(self._tf_buffer) + self._broadcaster = tf2_ros.TransformBroadcaster() + self._broadcaster.pub_tf.name = output_topic + self._broadcaster.pub_tf.__init__(self._broadcaster.pub_tf.name, # pylint: disable=W0233 + tf2_msgs.msg.TFMessage, + queue_size=100) + self._timer = rospy.Timer(rospy.Duration(1/frequency), self._timer_callback) + + @staticmethod + def _load_and_validate_config(config_file): + if not os.path.exists(config_file): + raise FileNotFoundError(f'File {config_file} not found') + with open(config_file, 'r', encoding='utf-8') as opened_file: + loaded_config = yaml.safe_load(opened_file) + if 'sr_tf_live_republisher' not in loaded_config: + raise ValueError("Config file must have 'sr_tf_live_republisher' section at the top level") + if 'parent_to_child_frames' not in loaded_config['sr_tf_live_republisher']: + raise ValueError('Config file must contain parent_to_child_frames under sr_tf_live_republisher') + if len(loaded_config['sr_tf_live_republisher']['parent_to_child_frames']) == 0: + raise ValueError('parent_to_child_frames section must contain at least one parent to child frame mapping') + if 'output_topic' not in loaded_config['sr_tf_live_republisher']: + raise ValueError('Config file must contain output_topic section under sr_tf_live_republisher') + if 'frequency' not in loaded_config['sr_tf_live_republisher']: + raise ValueError('Config file must contain frequency section under sr_tf_live_republisher') + return loaded_config['sr_tf_live_republisher'] + + def _timer_callback(self, _): + try: + transforms = [self._tf_buffer.lookup_transform(parent, child, rospy.Time()) + for parent, child in self._parent_child_frames_dict.items()] + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): + pass + else: + for trans in transforms: + self._broadcaster.sendTransform(trans) + + +if __name__ == '__main__': + # parser = argparse.ArgumentParser() + # parser.add_argument('--config_file', help='Path to the yaml config file', required=True) + # args = parser.parse_args() + # input_config_file = args.config_file + rospy.init_node('tf_live_filter') + input_config_file = rospy.get_param('~config_file') + republisher = SrLiveTfRepublisher(input_config_file) + rospy.spin() From b35e4aeb88940981d0cbdb29ca867e9748654c8d Mon Sep 17 00:00:00 2001 From: Build Tools Date: Tue, 11 Jun 2024 14:08:32 +0000 Subject: [PATCH 2/6] tidy, anon=true --- .../scripts/sr_utilities_common/live_tf_republisher.py | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py index 5c94cb48..65f87542 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py +++ b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py @@ -15,7 +15,6 @@ # with this program. If not, see . import os -import argparse import yaml import rospy import tf2_ros @@ -69,11 +68,7 @@ def _timer_callback(self, _): if __name__ == '__main__': - # parser = argparse.ArgumentParser() - # parser.add_argument('--config_file', help='Path to the yaml config file', required=True) - # args = parser.parse_args() - # input_config_file = args.config_file - rospy.init_node('tf_live_filter') + rospy.init_node('tf_live_filter', anonymous=True) input_config_file = rospy.get_param('~config_file') republisher = SrLiveTfRepublisher(input_config_file) rospy.spin() From 003eb1040a967a64afe8d8a90b2f28e50ba1cf34 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 13 Jun 2024 09:18:15 +0000 Subject: [PATCH 3/6] support multiple config 'keys' with the same name --- sr_utilities_common/config/frames_to_remap.yaml | 10 +++++----- .../sr_utilities_common/live_tf_republisher.py | 15 +++++++++------ 2 files changed, 14 insertions(+), 11 deletions(-) diff --git a/sr_utilities_common/config/frames_to_remap.yaml b/sr_utilities_common/config/frames_to_remap.yaml index 8120bedb..33058a0f 100644 --- a/sr_utilities_common/config/frames_to_remap.yaml +++ b/sr_utilities_common/config/frames_to_remap.yaml @@ -17,8 +17,8 @@ sr_tf_live_republisher: output_topic: '/tf_filtered_record' frequency: 100 parent_to_child_frames: - rh_palm: rh_fftip - rh_palm: rh_mftip - rh_palm: rh_rftip - rh_palm: rh_lftip - rh_palm: rh_thtip \ No newline at end of file + - rh_palm: rh_fftip + - rh_palm: rh_mftip + - rh_palm: rh_rftip + - rh_palm: rh_lftip + - rh_palm: rh_thtip diff --git a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py index 65f87542..ba401752 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py +++ b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py @@ -24,9 +24,11 @@ class SrLiveTfRepublisher: def __init__(self, config_file): loaded_config = self._load_and_validate_config(config_file) - self._parent_child_frames_dict = {} - for parent, child in loaded_config['parent_to_child_frames'].items(): - self._parent_child_frames_dict[parent] = child + self._parent_child_frames = [] + for parent_child_pair in loaded_config['parent_to_child_frames']: + for parent, child in parent_child_pair.items(): + self._parent_child_frames.append((parent, child)) + output_topic = loaded_config['output_topic'] frequency = loaded_config['frequency'] self._tf_buffer = tf2_ros.Buffer() @@ -58,8 +60,8 @@ def _load_and_validate_config(config_file): def _timer_callback(self, _): try: - transforms = [self._tf_buffer.lookup_transform(parent, child, rospy.Time()) - for parent, child in self._parent_child_frames_dict.items()] + transforms = [self._tf_buffer.lookup_transform(parent_child_pair[0], parent_child_pair[1], rospy.Time()) + for parent_child_pair in self._parent_child_frames] except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): pass else: @@ -69,6 +71,7 @@ def _timer_callback(self, _): if __name__ == '__main__': rospy.init_node('tf_live_filter', anonymous=True) - input_config_file = rospy.get_param('~config_file') + # input_config_file = rospy.get_param('~config_file') + input_config_file = '/home/user/projects/shadow_robot/base/src/common_resources/sr_utilities_common/config/frames_to_remap.yaml' republisher = SrLiveTfRepublisher(input_config_file) rospy.spin() From ea44c0e1c14eb8644061c49655668adb0845a224 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 13 Jun 2024 09:22:56 +0000 Subject: [PATCH 4/6] oops --- .../scripts/sr_utilities_common/live_tf_republisher.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py index ba401752..0ac5b744 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py +++ b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py @@ -61,7 +61,7 @@ def _load_and_validate_config(config_file): def _timer_callback(self, _): try: transforms = [self._tf_buffer.lookup_transform(parent_child_pair[0], parent_child_pair[1], rospy.Time()) - for parent_child_pair in self._parent_child_frames] + for parent_child_pair in self._parent_child_frames] except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): pass else: @@ -71,7 +71,6 @@ def _timer_callback(self, _): if __name__ == '__main__': rospy.init_node('tf_live_filter', anonymous=True) - # input_config_file = rospy.get_param('~config_file') - input_config_file = '/home/user/projects/shadow_robot/base/src/common_resources/sr_utilities_common/config/frames_to_remap.yaml' + input_config_file = rospy.get_param('~config_file') republisher = SrLiveTfRepublisher(input_config_file) rospy.spin() From e898be1c349ab2513ea83b998e06facd19b34656 Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 13 Jun 2024 10:54:28 +0000 Subject: [PATCH 5/6] treat exceptions individually --- .../sr_utilities_common/live_tf_republisher.py | 16 +++++++--------- 1 file changed, 7 insertions(+), 9 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py index 0ac5b744..511fa821 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py +++ b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py @@ -59,15 +59,13 @@ def _load_and_validate_config(config_file): return loaded_config['sr_tf_live_republisher'] def _timer_callback(self, _): - try: - transforms = [self._tf_buffer.lookup_transform(parent_child_pair[0], parent_child_pair[1], rospy.Time()) - for parent_child_pair in self._parent_child_frames] - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException): - pass - else: - for trans in transforms: - self._broadcaster.sendTransform(trans) - + for parent_child_pair in self._parent_child_frames: + try: + transform = self._tf_buffer.lookup_transform(parent_child_pair[0], parent_child_pair[1], rospy.Time()) + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: + rospy.logwarn_throttle(1.0, f'{e}') + else: + self._broadcaster.sendTransform(transform) if __name__ == '__main__': rospy.init_node('tf_live_filter', anonymous=True) From c3d7be30f7751bc577edcf1b3b8933d54c2b2caa Mon Sep 17 00:00:00 2001 From: carebare47 Date: Thu, 20 Jun 2024 15:16:02 +0000 Subject: [PATCH 6/6] lint --- .../scripts/sr_utilities_common/live_tf_republisher.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py index 511fa821..0f6acbe6 100755 --- a/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py +++ b/sr_utilities_common/scripts/sr_utilities_common/live_tf_republisher.py @@ -62,11 +62,12 @@ def _timer_callback(self, _): for parent_child_pair in self._parent_child_frames: try: transform = self._tf_buffer.lookup_transform(parent_child_pair[0], parent_child_pair[1], rospy.Time()) - except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as e: - rospy.logwarn_throttle(1.0, f'{e}') + except (tf2_ros.LookupException, tf2_ros.ConnectivityException, tf2_ros.ExtrapolationException) as error: + rospy.logwarn_throttle(1.0, f'{error}') else: self._broadcaster.sendTransform(transform) + if __name__ == '__main__': rospy.init_node('tf_live_filter', anonymous=True) input_config_file = rospy.get_param('~config_file')