Go to the documentation of this file.00001
00002
00003
00004 import math
00005 import sys
00006 import unittest
00007
00008 from nose.tools import assert_false
00009 from nose.tools import assert_true
00010
00011 from jsk_recognition_msgs.msg import BoundingBoxArray
00012 import rospy
00013 import rostest
00014
00015
00016 PKG = 'jsk_pcl_ros'
00017 NAME = 'test_cluster_point_indices_decomposer_box'
00018
00019
00020 class TestClusterPointIndicesDecomposerBbox(unittest.TestCase):
00021 def __init__(self, *args):
00022 super(TestClusterPointIndicesDecomposerBbox, self).__init__(*args)
00023 rospy.init_node(NAME)
00024 self.check_times = rospy.get_param('~check_times', 1)
00025
00026 def test_bbox(self):
00027 is_boxes_empty = True
00028 for i in range(self.check_times):
00029 msg = rospy.wait_for_message("~boxes", BoundingBoxArray)
00030 for box in msg.boxes:
00031 is_boxes_empty = False
00032 assert_false(math.isnan(box.pose.position.x),
00033 'pose.position.x is nan')
00034 assert_false(math.isnan(box.pose.position.y),
00035 'pose.position.y is nan')
00036 assert_false(math.isnan(box.pose.position.z),
00037 'pose.position.z is nan')
00038 assert_false(math.isnan(box.pose.orientation.x),
00039 'pose.orientation.x is nan')
00040 assert_false(math.isnan(box.pose.orientation.y),
00041 'pose.orientation.y is nan')
00042 assert_false(math.isnan(box.pose.orientation.z),
00043 'pose.orientation.z is nan')
00044 assert_false(math.isnan(box.pose.orientation.w),
00045 'pose.orientation.w is nan')
00046 assert_false(math.isnan(box.dimensions.x),
00047 'dimensions.x is nan')
00048 assert_false(math.isnan(box.dimensions.y),
00049 'dimensions.y is nan')
00050 assert_false(math.isnan(box.dimensions.z),
00051 'dimensions.z is nan')
00052 assert_true(box.dimensions.x != 0, 'dimensions.x is 0')
00053 assert_true(box.dimensions.y != 0, 'dimensions.y is 0')
00054 assert_true(box.dimensions.z != 0, 'dimensions.z is 0')
00055 assert_true(
00056 not is_boxes_empty,
00057 'Bboxes array is always empty in %d trials' % self.check_times
00058 )
00059
00060
00061 if __name__ == '__main__':
00062 rostest.run(PKG, NAME, TestClusterPointIndicesDecomposerBbox, sys.argv)