00001 #!/usr/bin/env python 00002 # Software License Agreement (BSD License) 00003 # 00004 # Copyright (c) 2008, Willow Garage, Inc. 00005 # All rights reserved. 00006 # 00007 # Redistribution and use in source and binary forms, with or without 00008 # modification, are permitted provided that the following conditions 00009 # are met: 00010 # 00011 # * Redistributions of source code must retain the above copyright 00012 # notice, this list of conditions and the following disclaimer. 00013 # * Redistributions in binary form must reproduce the above 00014 # copyright notice, this list of conditions and the following 00015 # disclaimer in the documentation and/or other materials provided 00016 # with the distribution. 00017 # * Neither the name of Willow Garage, Inc. nor the names of its 00018 # contributors may be used to endorse or promote products derived 00019 # from this software without specific prior written permission. 00020 # 00021 # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00022 # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00023 # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00024 # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00025 # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00026 # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00027 # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00028 # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00029 # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00030 # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00031 # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00032 # POSSIBILITY OF SUCH DAMAGE. 00033 00034 ## Simple talker demo that publishes std_msg/Strings to the 'chatter' topic 00035 00036 PKG = 'test_rospy' 00037 NAME = 'test_node' 00038 00039 import sys 00040 00041 import rospy 00042 import test_rosmaster.msg 00043 00044 # Copied from test_ros.test_node 00045 #_required_publications = 'test_string_out', 'test_primitives_out', 'test_arrays_out', 'test_header_out' 00046 #_required_subscriptions = 'test_string_in', 'test_primitives_in', 'test_arrays_in', 'test_header_in', 'probe_topic' 00047 00048 ## pass-through callback that republishes message on \a pub. 00049 ## @param pub TopicPub: topic to republish incoming messages on 00050 ## @return fn: callback fn for TopicSub 00051 def chain_callback(pub): 00052 def chained_callback(data): 00053 # special logic for handling TestHeader 00054 if isinstance(data, test_rosmaster.msg.TestHeader): 00055 if data.auto == 0: 00056 new_data = test_rosmaster.msg.TestHeader() 00057 # when auto is 0, must send 1234, 5678 as time 00058 new_data.header.stamp = Time(1234, 5678) 00059 # frame_id not really important 00060 new_data.header.frame_id = 1234 00061 else: # force auto-header timestamp 00062 new_data = test_rosmaster.msg.TestHeader(None, rospy.caller_id(), data.auto) 00063 data = new_data 00064 data.caller_id = rospy.caller_id() 00065 pub.publish(data) 00066 00067 def test_node(): 00068 # required publications 00069 string_out = rospy.Publisher("test_string_out", test_rosmaster.msg.TestString) 00070 primitives_out = rospy.Publisher("test_primitives_out", test_rosmaster.msg.TestPrimitives) 00071 arrays_out = rospy.Publisher("test_arrays_out", test_rosmaster.msg.TestArrays) 00072 header_out = rospy.Publisher("test_header_out", test_rosmaster.msg.TestHeader) 00073 00074 #required subs 00075 rospy.Subscriber("test_string_in", test_rosmaster.msg.TestString, chain_callback(string_out)) 00076 rospy.Subscriber("test_primitives_in", test_rosmaster.msg.TestPrimitives, chain_callback(primitives_out)) 00077 rospy.Subscriber("test_arrays_in", test_rosmaster.msg.TestArrays, chain_callback(arrays_out)) 00078 rospy.Subscriber("test_header_in", test_rosmaster.msg.TestHeader, chain_callback(header_out)) 00079 00080 # subscription with no publisher 00081 probe_in = rospy.Subscriber("probe_topic", test_rosmaster.msg.TestString) 00082 00083 rospy.init_node(NAME) 00084 rospy.spin() 00085 00086 if __name__ == '__main__': 00087 test_node() 00088 00089