test_polygon_array_unwrapper.py
Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 # -*- coding: utf-8 -*-
00003 # Author: Yuki Furuta <furushchev@jsk.imi.i.u-tokyo.ac.jp>
00004 
00005 
00006 import rospy
00007 from threading import Event
00008 from unittest import TestCase
00009 
00010 import dynamic_reconfigure.client
00011 from jsk_recognition_msgs.msg import PolygonArray, ModelCoefficientsArray
00012 from geometry_msgs.msg import PolygonStamped
00013 from pcl_msgs.msg import ModelCoefficients
00014 
00015 TEST_NODE = "/polygon_array_unwrapper"
00016 
00017 class TestPolygonArrayUnwrapper(TestCase):
00018     def setUp(self):
00019         self.client = dynamic_reconfigure.client.Client(TEST_NODE, timeout=30)
00020         self.pub_polygon = rospy.Publisher(TEST_NODE + "/input_polygons",
00021                                            PolygonArray, queue_size=1)
00022         self.pub_coefficients = rospy.Publisher(TEST_NODE + "/input_coefficients",
00023                                                 ModelCoefficientsArray, queue_size=1)
00024         self.sub_polygon = None
00025         self.sub_coefficients = None
00026         self.msg_wait_timeout = 10
00027         self.polygon_msg = None
00028         self.polygon_msg_callback_event = Event()
00029         self.coeff_msg = None
00030         self.coeff_msg_callback_event = Event()
00031         self.timer = rospy.Timer(rospy.Duration(0.1), self.publishPolygons)
00032     def tearDown(self):
00033         self.unsubscribe()
00034         self.timer.shutdown()
00035         self.polygon_msg_callback_event.clear()
00036         self.coeff_msg_callback_event.clear()
00037     def subscribe(self):
00038         self.sub_polygon = rospy.Subscriber(TEST_NODE + "/output_polygon",
00039                                             PolygonStamped,
00040                                             self.polygonMsgCallback)
00041         self.sub_coefficients = rospy.Subscriber(TEST_NODE + "/output_coefficients",
00042                                                  ModelCoefficients,
00043                                                  self.coeffMsgCallback)
00044     def unsubscribe(self):
00045         self.sub_polygon.unregister()
00046         self.sub_coefficients.unregister()
00047     def polygonMsgCallback(self, msg):
00048         self.polygon_msg = msg
00049         self.polygon_msg_callback_event.set()
00050     def coeffMsgCallback(self, msg):
00051         self.coeff_msg = msg
00052         self.coeff_msg_callback_event.set()
00053     def publishPolygons(self, event=None):
00054         pmsg = PolygonArray()
00055         pmsg.header.stamp = rospy.Time.now()
00056         cmsg = ModelCoefficientsArray()
00057         cmsg.header.stamp = pmsg.header.stamp
00058         for i in range(10):
00059             pmsg.polygons.append(PolygonStamped())
00060             pmsg.polygons[i].header.stamp = pmsg.header.stamp
00061             pmsg.polygons[i].header.frame_id = str(i)
00062             cmsg.coefficients.append(ModelCoefficients())
00063             cmsg.coefficients[i].header.stamp = cmsg.header.stamp
00064             cmsg.coefficients[i].header.frame_id = str(i)
00065         pmsg.likelihood = [1.0,2.0,3.0,4.0,5.0,4.0,3.0,2.0,1.0,0.0]
00066         self.pub_polygon.publish(pmsg)
00067         self.pub_coefficients.publish(cmsg)
00068     def waitMsgs(self):
00069         self.polygon_msg_callback_event.wait(self.msg_wait_timeout)
00070         self.coeff_msg_callback_event.wait(self.msg_wait_timeout)
00071     def testUnwrap(self):
00072         self.client.update_configuration({
00073             "plane_index": 0,
00074             "use_likelihood": False
00075         })
00076         self.subscribe()
00077         self.waitMsgs()
00078 
00079         self.assertTrue(self.polygon_msg is not None)
00080         self.assertTrue(self.coeff_msg is not None)
00081         self.assertEqual(self.polygon_msg.header.frame_id, str(0))
00082         self.assertEqual(self.coeff_msg.header.frame_id, str(0))
00083     def testUnwrapIndex(self):
00084         self.client.update_configuration({
00085             "plane_index": 7,
00086             "use_likelihood": False
00087         })
00088         self.subscribe()
00089         self.waitMsgs()
00090 
00091         self.assertTrue(self.polygon_msg is not None)
00092         self.assertTrue(self.coeff_msg is not None)
00093         self.assertEqual(self.polygon_msg.header.frame_id, str(7))
00094         self.assertEqual(self.coeff_msg.header.frame_id, str(7))
00095     def testUnwrapLikelihood(self):
00096         self.client.update_configuration({
00097             "plane_index": 2,
00098             "use_likelihood": True
00099         })
00100         self.subscribe()
00101         self.waitMsgs()
00102 
00103         self.assertTrue(self.polygon_msg is not None)
00104         self.assertTrue(self.coeff_msg is not None)
00105         self.assertEqual(self.polygon_msg.header.frame_id, str(4))
00106         self.assertEqual(self.coeff_msg.header.frame_id, str(4))
00107 
00108 if __name__ == '__main__':
00109     import rostest
00110     rospy.init_node("polygon_array_unwrapper_test")
00111     rostest.rosrun("jsk_pcl_ros_utils", "polygon_array_unwrapper_test", TestPolygonArrayUnwrapper)


jsk_pcl_ros_utils
Author(s): Yohei Kakiuchi
autogenerated on Tue Jul 2 2019 19:40:52