Skip to content

Instantly share code, notes, and snippets.

@eacousineau
Last active December 25, 2015 13:49
Show Gist options
  • Save eacousineau/6986950 to your computer and use it in GitHub Desktop.
Save eacousineau/6986950 to your computer and use it in GitHub Desktop.
#!/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()
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment