00001
00002
00003
00004 from math import pi
00005 import unittest
00006
00007 import rospy
00008 import roslib.message
00009 from sensor_msgs.msg import LaserScan
00010 from geometry_msgs.msg import Pose
00011 from nav_msgs.msg import Odometry
00012 from geometry_msgs.msg import Polygon
00013 from geometry_msgs.msg import Point32
00014
00015 from lama_interfaces.srv import GetDoubleRequest
00016 from lama_interfaces.srv import GetVectorDoubleRequest
00017 from lama_interfaces.srv import GetVectorLaserScanRequest
00018 from lama_interfaces.srv import GetVectorPoseRequest
00019 from lama_interfaces.srv import GetVectorOdometryRequest
00020 from lama_interfaces.srv import GetPolygonRequest
00021
00022
00023 class RosTestCase(unittest.TestCase):
00024 def assertMsgEqual(self, msg0, msg1):
00025 """Fail if two ROS messages are not equal"""
00026 self.assertIsInstance(msg0, roslib.message.Message,
00027 msg='Argument 1 is not a Message')
00028 self.assertIsInstance(msg1, roslib.message.Message,
00029 msg='Argument 2 is not a Message')
00030 slots0 = msg0.__slots__
00031 slots1 = msg1.__slots__
00032 self.assertEquals(slots0, slots1,
00033 msg=('Messages do not have the same arguments\n' +
00034 'Msg1 args: {}\n'.format(slots0) +
00035 'Msg2 args: {}\n'.format(slots1)))
00036 for slot in slots0:
00037 value0 = getattr(msg0, slot)
00038 value1 = getattr(msg1, slot)
00039 if isinstance(value0, roslib.message.Message):
00040 self.assertMsgEqual(value0, value1)
00041 elif isinstance(value0, (list, tuple)):
00042 self.assertAlmostEquals(list(value0), list(value1), places=6,
00043 msg='Argument {} differ: {} != {}'.
00044 format(slot, value0, value1))
00045 else:
00046 self.assertAlmostEqual(value0, value1, places=6,
00047 msg='Argument {} differ: {} != {}'.
00048 format(slot, value0, value1))
00049
00050
00051 class DbMessagePassingBase(object):
00052 """test setting and getting several descriptors"""
00053 def __init__(self, *args, **kwargs):
00054 rospy.init_node('test_lama_core', anonymous=True)
00055 super(DbMessagePassingBase, self).__init__(*args, **kwargs)
00056
00057 def test_double(self):
00058 """Test for a message as a list or tuple"""
00059 interface_name = 'double_' + self.interface_type
00060 getter_service = 'lama_interfaces/GetDouble'
00061 setter_service = 'lama_interfaces/SetDouble'
00062
00063
00064 iface = self.interface_factory(interface_name,
00065 getter_service,
00066 setter_service)
00067 get_srv = rospy.ServiceProxy(iface.getter_service_name,
00068 iface.getter_service_class)
00069 set_srv = rospy.ServiceProxy(iface.setter_service_name,
00070 iface.setter_service_class)
00071
00072 msg = 5.678
00073
00074 descriptor_from_setter = set_srv(msg)
00075
00076
00077 descriptor_to_getter = GetDoubleRequest()
00078 descriptor_to_getter.id = descriptor_from_setter.id
00079 response = get_srv(descriptor_to_getter)
00080
00081 self.assertIsNot(msg, response.descriptor)
00082 self.assertAlmostEqual(msg, response.descriptor, places=6)
00083
00084 def test_vector_double(self):
00085 """Test for a message as a list or tuple"""
00086 interface_name = 'vector_double_descriptor_' + self.interface_type
00087 getter_service = 'lama_interfaces/GetVectorDouble'
00088 setter_service = 'lama_interfaces/SetVectorDouble'
00089
00090
00091 iface = self.interface_factory(interface_name,
00092 getter_service,
00093 setter_service)
00094 get_srv = rospy.ServiceProxy(iface.getter_service_name,
00095 iface.getter_service_class)
00096 set_srv = rospy.ServiceProxy(iface.setter_service_name,
00097 iface.setter_service_class)
00098
00099 msg = (1.45, 5.9)
00100
00101 descriptor_from_setter = set_srv(msg)
00102
00103
00104 descriptor_to_getter = GetVectorDoubleRequest()
00105 descriptor_to_getter.id = descriptor_from_setter.id
00106 response = get_srv(descriptor_to_getter)
00107
00108 self.assertIsNot(msg, response.descriptor)
00109 self.assertAlmostEquals(msg, response.descriptor, places=6)
00110
00111 msg = [54.29, -9458.2]
00112
00113 descriptor_from_setter = set_srv(msg)
00114
00115
00116 descriptor_to_getter = GetVectorDoubleRequest()
00117 descriptor_to_getter.id = descriptor_from_setter.id
00118 response = get_srv(descriptor_to_getter)
00119
00120 self.assertIsNot(msg, response.descriptor)
00121 self.assertAlmostEquals(msg, list(response.descriptor), places=6)
00122
00123 def test_vector_laser_scan(self):
00124 """Test passing and getting a LaserScan[] message"""
00125 interface_name = 'laser_descriptor_' + self.interface_type
00126 getter_service = 'lama_interfaces/GetVectorLaserScan'
00127 setter_service = 'lama_interfaces/SetVectorLaserScan'
00128
00129
00130 iface = self.interface_factory(interface_name,
00131 getter_service,
00132 setter_service)
00133 get_srv = rospy.ServiceProxy(iface.getter_service_name,
00134 iface.getter_service_class)
00135 set_srv = rospy.ServiceProxy(iface.setter_service_name,
00136 iface.setter_service_class)
00137
00138 scan0 = LaserScan()
00139 scan0.header.seq = 1
00140 scan0.header.stamp = rospy.Time.now()
00141 scan0.header.frame_id = 'frame0'
00142 scan0.angle_min = -pi
00143 scan0.angle_max = pi
00144 scan0.range_max = 10.
00145 scan0.ranges = [0., 1.]
00146 scan1 = LaserScan()
00147 scan1.header.seq = 2
00148 scan1.header.stamp = rospy.Time.now()
00149 scan1.header.frame_id = 'frame1'
00150 scan1.angle_min = -pi / 2
00151 scan1.angle_max = pi / 2
00152 scan1.range_max = 9.
00153 scan1.ranges = [2., 3.]
00154 out_scans = [scan0, scan1]
00155
00156 descriptor_from_setter = set_srv(out_scans)
00157
00158
00159 descriptor_to_getter = GetVectorLaserScanRequest()
00160 descriptor_to_getter.id = descriptor_from_setter.id
00161 response = get_srv(descriptor_to_getter)
00162
00163 self.assertEqual(len(out_scans), len(response.descriptor))
00164 self.assertIsNot(scan0, response.descriptor[0])
00165 self.assertIsNot(scan0, response.descriptor[1])
00166 self.assertIsNot(scan1, response.descriptor[0])
00167 self.assertIsNot(scan1, response.descriptor[1])
00168 for scan_out, scan_in in zip(out_scans, response.descriptor):
00169 self.assertMsgEqual(scan_out, scan_in)
00170
00171 def test_pose(self):
00172 """Test passing and getting a Pose[] message"""
00173 interface_name = 'pose_descriptor_' + self.interface_type
00174 getter_service = 'lama_interfaces/GetVectorPose'
00175 setter_service = 'lama_interfaces/SetVectorPose'
00176
00177
00178 iface = self.interface_factory(interface_name,
00179 getter_service,
00180 setter_service)
00181 get_srv = rospy.ServiceProxy(iface.getter_service_name,
00182 iface.getter_service_class)
00183 set_srv = rospy.ServiceProxy(iface.setter_service_name,
00184 iface.setter_service_class)
00185
00186 pose0 = Pose()
00187 pose0.position.x = 34.5
00188 pose0.position.y = 5424.9
00189 pose0.position.z = -1238.5
00190 pose0.orientation.x = 0.5215973586107343
00191 pose0.orientation.y = 0.5145501061145893
00192 pose0.orientation.z = 0.20795107752790498
00193 pose0.orientation.w = (1 - (
00194 pose0.orientation.x ** 2 +
00195 pose0.orientation.y ** 2 +
00196 pose0.orientation.z ** 2)) ** 0.5
00197 poses = [pose0, pose0]
00198
00199 descriptor_from_setter = set_srv(poses)
00200
00201
00202 descriptor_to_getter = GetVectorPoseRequest()
00203 descriptor_to_getter.id = descriptor_from_setter.id
00204 response = get_srv(descriptor_to_getter)
00205
00206 self.assertEqual(len(poses), len(response.descriptor))
00207 self.assertIsNot(poses[0], response.descriptor[0])
00208 self.assertIsNot(poses[1], response.descriptor[1])
00209 for pose_out, pose_in in zip(poses, response.descriptor):
00210 self.assertMsgEqual(pose_out, pose_in)
00211
00212 def test_odometry(self):
00213 """Test passing and getting a Odometry[] message"""
00214 interface_name = 'odometry_descriptor_' + self.interface_type
00215 getter_service = 'lama_interfaces/GetVectorOdometry'
00216 setter_service = 'lama_interfaces/SetVectorOdometry'
00217
00218
00219 iface = self.interface_factory(interface_name,
00220 getter_service,
00221 setter_service)
00222 get_srv = rospy.ServiceProxy(iface.getter_service_name,
00223 iface.getter_service_class)
00224 set_srv = rospy.ServiceProxy(iface.setter_service_name,
00225 iface.setter_service_class)
00226
00227 odom = Odometry()
00228 odom.header.seq = 1
00229 odom.header.stamp = rospy.Time.now()
00230 odom.header.frame_id = 'frame'
00231 odom.pose.pose.position.x = 34.5
00232 odom.pose.pose.position.y = 5424.9
00233 odom.pose.pose.position.z = -1238.5
00234 odom.pose.pose.orientation.x = 0.5215973586107343
00235 odom.pose.pose.orientation.y = 0.5145501061145893
00236 odom.pose.pose.orientation.z = 0.20795107752790498
00237 odom.pose.pose.orientation.w = (1 - (
00238 odom.pose.pose.orientation.x ** 2 +
00239 odom.pose.pose.orientation.y ** 2 +
00240 odom.pose.pose.orientation.z ** 2)) ** 0.5
00241 odom.twist.twist.linear.x = 43587.2
00242 odom.twist.twist.linear.y = 23.0
00243 odom.twist.twist.linear.z = -34.1
00244 odom.twist.twist.angular.x = 0.9
00245 odom.twist.twist.angular.y = 1.459
00246 odom.twist.twist.angular.z = 6.8
00247 odom.pose.covariance = [0.66661561, 0.66656431, 0.98260750,
00248 0.32037977, 0.37148378, 0.39368085,
00249 0.79881689, 0.62314512, 0.69425357,
00250 0.93326911, 0.68453431, 0.17272260,
00251 0.78877475, 0.47810612, 0.81750745,
00252 0.07891373, 0.43773912, 0.87397257,
00253 0.07260977, 0.28417538, 0.64133374,
00254 0.14946342, 0.21064081, 0.64976447,
00255 0.04895663, 0.59137640, 0.91207933,
00256 0.13848898, 0.68960110, 0.05312844,
00257 0.67371487, 0.73003252, 0.81269526,
00258 0.73006462, 0.31009889, 0.21264097]
00259
00260 odom.twist.covariance = [0.74771120, 0.67419758, 0.94429162,
00261 0.50301016, 0.44133241, 0.67792401,
00262 0.69450461, 0.01300356, 0.96009616,
00263 0.20118201, 0.36520037, 0.53622379,
00264 0.40432863, 0.96075259, 0.63216066,
00265 0.73726525, 0.77320984, 0.38823612,
00266 0.24819383, 0.41003267, 0.14771474,
00267 0.27845271, 0.44038160, 0.11593736,
00268 0.19048534, 0.15305760, 0.29514903,
00269 0.87055584, 0.24909447, 0.57612981,
00270 0.50363839, 0.65519450, 0.05121275,
00271 0.01989313, 0.06818292, 0.46997481]
00272 odoms = [odom]
00273
00274 descriptor_from_setter = set_srv(odoms)
00275
00276
00277 descriptor_to_getter = GetVectorOdometryRequest()
00278 descriptor_to_getter.id = descriptor_from_setter.id
00279 response = get_srv(descriptor_to_getter)
00280
00281 self.assertEqual(len(odoms), len(response.descriptor))
00282 self.assertIsNot(odoms[0], response.descriptor[0])
00283 for odom_out, odom_in in zip(odoms, response.descriptor):
00284 self.assertMsgEqual(odom_out, odom_in)
00285
00286 def test_polygon(self):
00287 """Test passing and getting a Polygon message"""
00288 interface_name = 'polygon_' + self.interface_type
00289 getter_service = 'lama_interfaces/GetPolygon'
00290 setter_service = 'lama_interfaces/SetPolygon'
00291
00292
00293 iface = self.interface_factory(interface_name,
00294 getter_service,
00295 setter_service)
00296 get_srv = rospy.ServiceProxy(iface.getter_service_name,
00297 iface.getter_service_class)
00298 set_srv = rospy.ServiceProxy(iface.setter_service_name,
00299 iface.setter_service_class)
00300
00301 polygon = Polygon()
00302
00303 descriptor_from_setter = set_srv(polygon)
00304
00305
00306 descriptor_to_getter = GetPolygonRequest()
00307 descriptor_to_getter.id = descriptor_from_setter.id
00308 response = get_srv(descriptor_to_getter)
00309
00310 self.assertIsNot(polygon, response.descriptor)
00311 for point_out, point_in in zip(polygon.points,
00312 response.descriptor.points):
00313 self.assertAlmostEqual(point_out.x, point_in.x, places=5)
00314 self.assertAlmostEqual(point_out.y, point_in.y, places=5)
00315 self.assertAlmostEqual(point_out.z, point_in.z, places=5)
00316
00317 polygon = Polygon()
00318 polygon.points.append(Point32())
00319 polygon.points[0].x = 45.6
00320 polygon.points[0].y = -34.0
00321 polygon.points[0].z = -26.4
00322 polygon.points.append(Point32())
00323 polygon.points[1].x = -5.6
00324 polygon.points[1].y = -3.0
00325 polygon.points[1].z = 6.4
00326
00327 descriptor_from_setter = set_srv(polygon)
00328
00329
00330 descriptor_to_getter = GetPolygonRequest()
00331 descriptor_to_getter.id = descriptor_from_setter.id
00332 response = get_srv(descriptor_to_getter)
00333
00334 self.assertIsNot(polygon, response.descriptor)
00335 for point_out, point_in in zip(polygon.points,
00336 response.descriptor.points):
00337 self.assertAlmostEqual(point_out.x, point_in.x, places=5)
00338 self.assertAlmostEqual(point_out.y, point_in.y, places=5)
00339 self.assertAlmostEqual(point_out.z, point_in.z, places=5)