From e5e72cfeb26f8a28fe1eaaf4c4bf9c91d05bccdb Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Mon, 20 Mar 2017 00:26:39 +0900 Subject: [PATCH 1/2] Use different value for duration and rate in tf_to_transform.py Rate can be 50 - 100 for example, but duration should be ~1 [s] even so. In previous implementation, the duration will be 1/100 - 1/50 [s] and it is too small to resolve tf. --- jsk_topic_tools/scripts/tf_to_transform.py | 37 ++++++++++------------ 1 file changed, 16 insertions(+), 21 deletions(-) diff --git a/jsk_topic_tools/scripts/tf_to_transform.py b/jsk_topic_tools/scripts/tf_to_transform.py index a55ee3c0e..cd212abd0 100755 --- a/jsk_topic_tools/scripts/tf_to_transform.py +++ b/jsk_topic_tools/scripts/tf_to_transform.py @@ -11,22 +11,14 @@ class TFToTransform(object): - def __init__(self, parent_frame_id, child_frame_id, duration): - if duration is None: - self.duration = rospy.get_param('~duration', default=1.0) - else: - self.duration = duration - if parent_frame_id is None: - self.parent_frame_id = rospy.get_param('~parent_frame_id') - else: - self.parent_frame_id = parent_frame_id - if child_frame_id is None: - self.child_frame_id = rospy.get_param('~child_frame_id') - else: - self.child_frame_id = child_frame_id + def __init__(self, parent_frame_id, child_frame_id, duration, rate): + self.duration = rospy.get_param('~duration', duration) + self.parent_frame_id = rospy.get_param('~parent_frame_id', parent_frame_id) + self.child_frame_id = rospy.get_param('~child_frame_id', child_frame_id) + rate = rospy.get_param('~rate', rate) self.pub = rospy.Publisher('~output', TransformStamped, queue_size=1) self.listener = tf.TransformListener() - rospy.Timer(rospy.Duration(self.duration), self._publish_transform) + rospy.Timer(rospy.Duration(1. / rate), self._publish_transform) def _publish_transform(self, event): try: @@ -37,10 +29,10 @@ def _publish_transform(self, event): now, timeout=rospy.Duration(self.duration) ) - except (tf.LookupException, - tf.ConnectivityException, - tf.ExtrapolationException): - rospy.logerr('cannot get transform') + except Exception: + rospy.logerr( + "cannot get transform from '%s' to '%s' in '%.2f' [s]" % + (self.parent_frame_id, self.child_frame_id, self.duration)) return trans, rot = self.listener.lookupTransform( @@ -77,13 +69,16 @@ def _publish_transform(self, event): help='parent frame id', default=None) parser.add_argument('child_frame_id', nargs='?', help='child frame id', default=None) - parser.add_argument('--duration', '-d', type=float, - help='Duration [s]: default=1.0', default=None) + parser.add_argument('--duration', '-d', type=float, default=1, + help='Duration to resolve tf. default: 1 [s]') + parser.add_argument('--rate', '-r', type=float, default=1, + help='Rate of publication. default: 1 [hz]') args = parser.parse_args(rospy.myargv()[1:]) rospy.init_node('tf_to_transform') tf_pub = TFToTransform( args.parent_frame_id, args.child_frame_id, - args.duration + args.duration, + args.rate, ) rospy.spin() From 934113f257d1084988ad8fbd49c3d9e753eecd93 Mon Sep 17 00:00:00 2001 From: Kentaro Wada Date: Mon, 20 Mar 2017 00:28:54 +0900 Subject: [PATCH 2/2] Fix for flake8 --- jsk_topic_tools/scripts/tf_to_transform.py | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/jsk_topic_tools/scripts/tf_to_transform.py b/jsk_topic_tools/scripts/tf_to_transform.py index cd212abd0..0995a3a51 100755 --- a/jsk_topic_tools/scripts/tf_to_transform.py +++ b/jsk_topic_tools/scripts/tf_to_transform.py @@ -13,8 +13,10 @@ class TFToTransform(object): def __init__(self, parent_frame_id, child_frame_id, duration, rate): self.duration = rospy.get_param('~duration', duration) - self.parent_frame_id = rospy.get_param('~parent_frame_id', parent_frame_id) - self.child_frame_id = rospy.get_param('~child_frame_id', child_frame_id) + self.parent_frame_id = rospy.get_param('~parent_frame_id', + parent_frame_id) + self.child_frame_id = rospy.get_param('~child_frame_id', + child_frame_id) rate = rospy.get_param('~rate', rate) self.pub = rospy.Publisher('~output', TransformStamped, queue_size=1) self.listener = tf.TransformListener() @@ -76,9 +78,9 @@ def _publish_transform(self, event): args = parser.parse_args(rospy.myargv()[1:]) rospy.init_node('tf_to_transform') tf_pub = TFToTransform( - args.parent_frame_id, - args.child_frame_id, - args.duration, - args.rate, - ) + args.parent_frame_id, + args.child_frame_id, + args.duration, + args.rate, + ) rospy.spin()