Go to the documentation of this file.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
00040
00041
00042
00043 PKG='applanix_test'
00044
00045 import roslib; roslib.load_manifest(PKG)
00046 import rospy
00047
00048 from applanix_msgs.msg import NavigationSolution
00049 from applanix_generated_msgs.msg import AllMsgs
00050 from diagnostic_msgs.msg import DiagnosticArray
00051 from sensor_msgs.msg import NavSatFix, Imu
00052
00053 import rostest
00054 import unittest
00055
00056
00057 class MessageReceiver(object):
00058 def __init__(self, topic_name, topic_type):
00059 self.msgs = []
00060 rospy.Subscriber(topic_name, topic_type, self.cb)
00061
00062 def cb(self, msg):
00063 self.msgs.append(msg)
00064
00065
00066 class TestSmoke(unittest.TestCase):
00067 receivers = {}
00068
00069 def test_diagnostics(self):
00070 self.assertTrue(len(self.receivers['diag'].msgs) > 70)
00071 fields = {}
00072 for msg in self.receivers['diag'].msgs:
00073 for field in msg.status[0].values:
00074 fields[field.key] = field.value
00075 self.assertEqual(fields['IMU_STATUS'], '0')
00076 self.assertEqual(fields['PRIMARY_GNSS_IN_CA_MODE'], '1')
00077 self.assertEqual(fields['INERTIAL_NAVIGATOR_INITIALIZED'], '1')
00078 self.assertEqual(fields['FULL_NAVIGATION_SOLUTION'], '0')
00079
00080 def test_navigation(self):
00081 self.assertTrue(len(self.receivers['nav'].msgs) > 70)
00082 msg = self.receivers['nav'].msgs[-1]
00083 self.assertAlmostEqual(msg.latitude, 44.2449408681)
00084 self.assertAlmostEqual(msg.longitude, -76.5096210157)
00085 self.assertAlmostEqual(msg.roll, 2.04550977266)
00086 self.assertAlmostEqual(msg.pitch, 1.3181307736)
00087 self.assertAlmostEqual(msg.heading, 20.812475085)
00088
00089 def test_fix(self):
00090 self.assertTrue(len(self.receivers['fix'].msgs) > 70)
00091 msg = self.receivers['fix'].msgs[-1]
00092 self.assertAlmostEqual(msg.latitude, 44.2449408681)
00093 self.assertAlmostEqual(msg.longitude, -76.5096210157)
00094
00095 def test_imu(self):
00096 self.assertTrue(len(self.receivers['imu'].msgs) > 70)
00097 msg = self.receivers['imu'].msgs[-1]
00098 self.assertAlmostEqual(msg.angular_velocity.x, -0.000960592777497)
00099 self.assertAlmostEqual(msg.linear_acceleration.y, 0.0397075638175)
00100
00101 def test_config(self):
00102 """ Exists primarily to test the correct parsing of complex messages/groups, including
00103 nested arrays, etc. """
00104 msg = self.receivers['cfg'].msgs[-1]
00105 self.assertEqual(len(msg.primary_data_port.groups), 3)
00106 self.assertEqual(msg.primary_data_port.groups[1].group, 10)
00107 self.assertEqual(msg.com_port_setup.ports[0].baud, 7)
00108 self.assertEqual(msg.com_port_setup.ports[0].parity, 0)
00109 self.assertEqual(msg.com_port_setup.ports[0].data_stop, 2)
00110
00111
00112 if __name__ == '__main__':
00113 rospy.init_node('applanix_test', anonymous=True)
00114 TestSmoke.receivers['diag'] = MessageReceiver('/diagnostics', DiagnosticArray)
00115 TestSmoke.receivers['nav'] = MessageReceiver('nav', NavigationSolution)
00116 TestSmoke.receivers['fix'] = MessageReceiver('gps_fix', NavSatFix)
00117 TestSmoke.receivers['imu'] = MessageReceiver('imu_data', Imu)
00118 TestSmoke.receivers['cfg'] = MessageReceiver('config', AllMsgs)
00119 rospy.sleep(3.0)
00120
00121 rostest.rosrun(PKG, 'test_smoke', TestSmoke)
00122