topic_aggreator.py

#!/usr/bin/python
import fnmatch
import argparse
import rosgraph.masterapi
import roslib
import rospy
import uuid
import sys

parser = argparse.ArgumentParser()
parser.add_argument('-l', '--latch', action='store_true')
parser.add_argument('-t', '--type', type=str, default=None, help="Option type of published topic. If not specified, will infer from first matching topic")
parser.add_argument('output', type=str, help="Topic to publish to")
parser.add_argument('inputs', type=str, nargs='+', help="List of topic names or globbing patterns (see fnmatch) to subscribe to")
args = parser.parse_args()

def get_message_class(name):
    # Copied from ROSTopicOutput
    # genpy.message.get_message_class doesn't seem to cut it for non-standard types
    pkg, msg = name.split('/')
    roslib.load_manifest(pkg)
    cls = __import__(pkg + '.msg')
    return getattr(cls.msg, msg)

rospy.init_node('topic_aggregator_{}'.format(str(uuid.uuid1()).replace('-', '_')))
master = rosgraph.masterapi.Master('/rostopic')
topicTuples = master.getPublishedTopics('/')

# Check string as patterns? Allow it to subscribe to unpublished topics
matches = []
for (topic, msgType) in topicTuples:
    for pattern in args.inputs:
        if fnmatch.fnmatch(topic, pattern):
            matches.append((topic, msgType))
            break

msgType = args.type
if not msgType:
    if len(matches) == 0:
        raise Exception("No topics currently shown, cannot infer message type")
    msgType = matches[0][1]

print "Publishing: {} ({})".format(args.output, msgType)
msgClass = get_message_class(msgType)

pub = rospy.Publisher(args.output, msgClass, latch=args.latch)

subs = []

def publish(msg):
    if not rospy.is_shutdown():
        pub.publish(msg)

# Check to ensure data types are the same. If so, subscriber
for (topic, curType) in matches:
    if curType == msgType:
        print "Subscribing: {}".format(topic)
        subs.append(rospy.Subscriber(topic, msgClass, publish))
    else:
        sys.stderr.write("Topic '{}' type '{}' does not jive with desired type '{}'".format(topic, curType, msgType))

print "Spinning..."

# Get an error when shutting down about publishing to closed topic. Meh.
rospy.spin()