subtopic_forwarder.py
Go to the documentation of this file.00001 #! /usr/bin/python 00002 #*********************************************************** 00003 #* Software License Agreement (BSD License) 00004 #* 00005 #* Copyright (c) 2009, Willow Garage, Inc. 00006 #* All rights reserved. 00007 #* 00008 #* Redistribution and use in source and binary forms, with or without 00009 #* modification, are permitted provided that the following conditions 00010 #* are met: 00011 #* 00012 #* * Redistributions of source code must retain the above copyright 00013 #* notice, this list of conditions and the following disclaimer. 00014 #* * Redistributions in binary form must reproduce the above 00015 #* copyright notice, this list of conditions and the following 00016 #* disclaimer in the documentation and/or other materials provided 00017 #* with the distribution. 00018 #* * Neither the name of Willow Garage, Inc. nor the names of its 00019 #* contributors may be used to endorse or promote products derived 00020 #* from this software without specific prior written permission. 00021 #* 00022 #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00023 #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00024 #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00025 #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00026 #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00027 #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00028 #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00029 #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00030 #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00031 #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00032 #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00033 #* POSSIBILITY OF SUCH DAMAGE. 00034 #* 00035 #* Author: Eitan Marder-Eppstein 00036 #*********************************************************** 00037 PKG = 'move_base' 00038 NAME = 'subtopic_forwarder' 00039 00040 import roslib; roslib.load_manifest(PKG) 00041 00042 import rospy 00043 import sys 00044 import rostopic 00045 from std_msgs.msg import * 00046 from roslib.message import Message 00047 00048 class SubtopicForwarder(object): 00049 def __init__(self, topic, remapped_topic): 00050 topic_class, real_topic, msg_eval = rostopic.get_topic_class(topic) 00051 try: 00052 rospy.Subscriber(real_topic, topic_class, self.callback, (remapped_topic, msg_eval)) 00053 except ValueError: pass 00054 00055 def callback(self, data, args): 00056 remapped_topic, msg_eval = args 00057 00058 #make sure to check if we have a subtopic or just a topic 00059 remapped_msg = msg_eval != None and msg_eval(data) or data 00060 00061 if not issubclass(type(remapped_msg), Message): 00062 rospy.logerr("Sorry, forwarding primitive types, in this case a %s, is not supported by this tool :(", type(remapped_msg)) 00063 return 00064 00065 remap_pub = rospy.Publisher(remapped_topic, type(remapped_msg)) 00066 remap_pub.publish(remapped_msg) 00067 00068 if __name__ == '__main__': 00069 if len(sys.argv) < 3: 00070 print "You are uinsg this tool wrong, usage is:" 00071 print "subtopic_forwarder [source_topic] [destination_topic]" 00072 sys.exit(-1) 00073 00074 rospy.init_node(NAME, anonymous=True) 00075 00076 topic = rospy.resolve_name(sys.argv[1]) 00077 remapped_topic = rospy.resolve_name(sys.argv[2]) 00078 sf = SubtopicForwarder(topic, remapped_topic) 00079 00080 rospy.spin()