00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039 PKG = 'test_rospy'
00040 NAME = 'test_embed_msg'
00041 import roslib; roslib.load_manifest(PKG)
00042
00043 import sys
00044 import time
00045 import unittest
00046
00047 import rospy
00048 import rostest
00049 from std_msgs.msg import String, Int32
00050 from test_rospy.msg import EmbedTest, Val, ArrayVal
00051
00052 PUBTOPIC = "chatter"
00053 LPNODE = 'listenerpublisher'
00054 LPTOPIC = 'listenerpublisher'
00055
00056 MSG = EmbedTest
00057
00058 TIMEOUT = 10.0
00059
00060 class TestEmbedMsg(unittest.TestCase):
00061
00062 def setUp(self):
00063 self.callback_data = None
00064
00065 def _test_embed_msg_callback(self, data):
00066 self.callback_data = data
00067
00068 def test_embed_msg(self):
00069 self.assert_(self.callback_data is None, "invalid test fixture")
00070
00071
00072 timeout_t = time.time() + 5.0
00073 while not rostest.is_subscriber(
00074 rospy.resolve_name(PUBTOPIC),
00075 rospy.resolve_name(LPNODE)) and time.time() < timeout_t:
00076 time.sleep(0.1)
00077
00078 self.assert_(rostest.is_subscriber(
00079 rospy.resolve_name(PUBTOPIC),
00080 rospy.resolve_name(LPNODE)), "%s is not up"%LPNODE)
00081
00082 print "Publishing to ", PUBTOPIC
00083 pub = rospy.Publisher(PUBTOPIC, MSG)
00084 rospy.Subscriber(LPTOPIC, MSG, self._test_embed_msg_callback)
00085
00086
00087 import random
00088 val = random.randint(0, 109812312)
00089 msg = "hi [%s]"%val
00090 for i in xrange(0, 10):
00091
00092
00093
00094
00095 pub.publish(
00096 MSG(String(msg), Int32(val),
00097 [Int32(val+1), Int32(val+2), Int32(val+3)],
00098 Val(msg+msg),
00099 [Val(msg), Val("two")],
00100 [ArrayVal([Val("av1"), Val("av2")]),
00101 ArrayVal([])
00102 ]
00103 ))
00104 time.sleep(0.1)
00105
00106
00107
00108 self.assert_(self.callback_data is not None, "no callback data from listenerpublisher")
00109 print "Got ", self.callback_data.str1.data, self.callback_data.int1.data
00110 errorstr = "callback msg field [%s] from listenerpublisher does not match"
00111 self.assertEquals(msg, self.callback_data.str1.data,
00112 errorstr%"str1.data")
00113 self.assertEquals(val, self.callback_data.int1.data,
00114 errorstr%"int1.data")
00115 for i in xrange(1, 4):
00116 self.assertEquals(val+i, self.callback_data.ints[i-1].data,
00117 errorstr%"ints[i-1].data")
00118 self.assertEquals(msg+msg, self.callback_data.val.val,
00119 errorstr%"val.val")
00120 self.assertEquals(msg, self.callback_data.vals[0].val,
00121 errorstr%"vals[0].val")
00122 self.assertEquals("two", self.callback_data.vals[1].val,
00123 errorstr%"vals[1].val")
00124
00125 self.assertEquals(2, len(self.callback_data.arrayval),
00126 errorstr%"len arrayval")
00127 self.assertEquals(2, len(self.callback_data.arrayval[0].vals),
00128 errorstr%"len arrayval[0].vals")
00129 self.assertEquals("av1", self.callback_data.arrayval[0].vals[0].val,
00130 errorstr%"arrayval[0].vals[0].val")
00131 self.assertEquals("av2", self.callback_data.arrayval[0].vals[1].val,
00132 errorstr%"arrayval[0].vals[1].val")
00133 self.assertEquals(0, len(self.callback_data.arrayval[1].vals),
00134 errorstr%"len arrayval[1].vals")
00135
00136
00137 if __name__ == '__main__':
00138 rospy.init_node(NAME)
00139 rostest.run(PKG, NAME, TestEmbedMsg, sys.argv)