Skip to content

Instantly share code, notes, and snippets.

Last active December 25, 2015 13:49
Show Gist options
  • Star 0 You must be signed in to star a gist
  • Fork 0 You must be signed in to fork a gist
  • Save eacousineau/6986950 to your computer and use it in GitHub Desktop.
Save eacousineau/6986950 to your computer and use it in GitHub Desktop.
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('/')
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))
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():
# 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))
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.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment