diff --git a/jsk_topic_tools/scripts/filtered_relay.py b/jsk_topic_tools/scripts/filtered_relay.py new file mode 100755 index 000000000..97bab6dec --- /dev/null +++ b/jsk_topic_tools/scripts/filtered_relay.py @@ -0,0 +1,93 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import sys +from importlib import import_module + +import roslib.message +import rospy +import std_msgs.msg + + +def expr_eval(expr, modules): + def eval_fn(topic, m, t): + return eval(expr, modules, {'m': m}) + return eval_fn + + +class FilteredRelay(object): + + def __init__(self): + import_list = rospy.get_param('~import', []) + self.modules = {} + for module in import_list: + try: + mod = import_module(module) + except ImportError: + rospy.logerr('Failed to import module: %s' % module, file=sys.stderr) + else: + self.modules[module] = mod + + self.filter_expression = rospy.get_param('~filter', None) + if self.filter_expression is not None: + self.filter_expression = expr_eval(self.filter_expression, self.modules) + + self.output_type = rospy.get_param('~output_type') + self.output_class = roslib.message.get_message_class(self.output_type) + + self.transform_expression = rospy.get_param('~transform', 'm') + self.pub_transform = rospy.Publisher( + '~output', + self.output_class, queue_size=1) + + self.topic_name = rospy.resolve_name('~input') + self.sub = rospy.Subscriber( + self.topic_name, + rospy.AnyMsg, + callback=lambda msg, tn=self.topic_name: self.callback(tn, msg), + queue_size=1) + + def publish(self, m): + if self.output_type == 'std_msgs/Empty': + self.pub_transform.publish() + return + if self.transform_expression is not None: + try: + res = eval(self.transform_expression, self.modules, {'m': m}) + except NameError as e: + rospy.logerr("Expression using variables other than 'm': %s" + % e.message, file=sys.stderr) + except UnboundLocalError as e: + rospy.logerr('Wrong expression:%s' % e.message, + file=sys.stderr) + except Exception as e: + rospy.logerr(str(e)) + else: + if not isinstance(res, (list, tuple)): + res = [res] + self.pub_transform.publish(*res) + else: + self.pub_transform.publish(m) + + def callback(self, topic_name, msg): + if isinstance(msg, rospy.msg.AnyMsg): + package, msg_type = msg._connection_header['type'].split('/') + ros_pkg = package + '.msg' + msg_class = getattr(import_module(ros_pkg), msg_type) + self.sub.unregister() + deserialized_sub = rospy.Subscriber( + topic_name, msg_class, + lambda msg, tn=topic_name: self.callback(tn, msg)) + self.sub = deserialized_sub + msg = msg_class().deserialize(msg._buff) + if self.filter_expression is not None: + if self.filter_expression(topic_name, msg, rospy.Time.now()): + self.publish(msg) + else: + self.publish(msg) + + +if __name__ == '__main__': + rospy.init_node('filtered_relay') + node = FilteredRelay() + rospy.spin()