Go to the documentation of this file.00001
00002
00003 import unittest
00004
00005 import numpy as np
00006
00007 from jsk_recognition_msgs.msg import ClusterPointIndices
00008 from pcl_msgs.msg import PointIndices
00009 import dynamic_reconfigure.client
00010 import rospy
00011 import rostest
00012
00013
00014 class TestClusterPointIndicesToPointIndices(unittest.TestCase):
00015
00016 def setUp(self):
00017 self.msg = None
00018 self.cluster_indices = ((0, 10, 20, 30), (5, 15, 25, 35))
00019 self.dynparam = dynamic_reconfigure.client.Client(
00020 'cluster_point_indices_to_point_indices')
00021 self.pub = rospy.Publisher('~output', ClusterPointIndices,
00022 queue_size=1)
00023 self.sub = rospy.Subscriber('~input', PointIndices, self.cb)
00024
00025 def cb(self, msg):
00026 self.msg = msg
00027
00028 def test_conversion(self):
00029 self.dynparam.update_configuration({'index': 1})
00030
00031 cluster_indices_msg = ClusterPointIndices()
00032 while self.msg is None:
00033 cluster_indices_msg.header.stamp = rospy.Time.now()
00034 for indices in self.cluster_indices:
00035 cluster_indices_msg.cluster_indices.append(
00036 PointIndices(
00037 header=cluster_indices_msg.header,
00038 indices=indices,
00039 )
00040 )
00041 self.pub.publish(cluster_indices_msg)
00042 rospy.sleep(0.1)
00043
00044 self.assertTupleEqual(self.msg.indices, self.cluster_indices[1])
00045
00046
00047 if __name__ == '__main__':
00048 PKG = 'jsk_pcl_ros_utils'
00049 ID = 'test_cluster_point_indices_to_point_indices'
00050 rospy.init_node(ID)
00051 rostest.rosrun(PKG, ID, TestClusterPointIndicesToPointIndices)