12
12
// See the License for the specific language governing permissions and
13
13
// limitations under the License.
14
14
15
- #include < gtest/gtest.h>
16
-
17
15
#include " lidar_centerpoint/ros_utils.hpp"
18
16
17
+ #include < gtest/gtest.h>
18
+
19
19
TEST (TestSuite, box3DToDetectedObject)
20
20
{
21
- std::vector<std::string> class_names = {" CAR" , " TRUCK" , " BUS" , " TRAILER" , " BICYCLE" , " MOTORBIKE" , " PEDESTRIAN" };
22
-
21
+ std::vector<std::string> class_names = {" CAR" , " TRUCK" , " BUS" , " TRAILER" ,
22
+ " BICYCLE" , " MOTORBIKE" , " PEDESTRIAN" };
23
+
23
24
// Test case 1: Test with valid label, has_twist=true, has_variance=true
24
25
{
25
26
centerpoint::Box3D box3d;
26
27
box3d.score = 0 .8f ;
27
- box3d.label = 0 ; // CAR
28
+ box3d.label = 0 ; // CAR
28
29
box3d.x = 1.0 ;
29
30
box3d.y = 2.0 ;
30
31
box3d.z = 3.0 ;
@@ -45,7 +46,8 @@ TEST(TestSuite, box3DToDetectedObject)
45
46
centerpoint::box3DToDetectedObject (box3d, class_names, true , true , obj);
46
47
47
48
EXPECT_FLOAT_EQ (obj.existence_probability , 0 .8f );
48
- EXPECT_EQ (obj.classification [0 ].label , autoware_auto_perception_msgs::msg::ObjectClassification::CAR);
49
+ EXPECT_EQ (
50
+ obj.classification [0 ].label , autoware_auto_perception_msgs::msg::ObjectClassification::CAR);
49
51
EXPECT_FLOAT_EQ (obj.kinematics .pose_with_covariance .pose .position .x , 1.0 );
50
52
EXPECT_FLOAT_EQ (obj.kinematics .pose_with_covariance .pose .position .y , 2.0 );
51
53
EXPECT_FLOAT_EQ (obj.kinematics .pose_with_covariance .pose .position .z , 3.0 );
@@ -61,13 +63,15 @@ TEST(TestSuite, box3DToDetectedObject)
61
63
{
62
64
centerpoint::Box3D box3d;
63
65
box3d.score = 0 .5f ;
64
- box3d.label = 10 ; // Invalid
66
+ box3d.label = 10 ; // Invalid
65
67
66
68
autoware_auto_perception_msgs::msg::DetectedObject obj;
67
69
centerpoint::box3DToDetectedObject (box3d, class_names, false , false , obj);
68
70
69
71
EXPECT_FLOAT_EQ (obj.existence_probability , 0 .5f );
70
- EXPECT_EQ (obj.classification [0 ].label , autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN);
72
+ EXPECT_EQ (
73
+ obj.classification [0 ].label ,
74
+ autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN);
71
75
EXPECT_FALSE (obj.kinematics .has_position_covariance );
72
76
EXPECT_FALSE (obj.kinematics .has_twist );
73
77
EXPECT_FALSE (obj.kinematics .has_twist_covariance );
@@ -76,14 +80,30 @@ TEST(TestSuite, box3DToDetectedObject)
76
80
77
81
TEST (TestSuite, getSemanticType)
78
82
{
79
- EXPECT_EQ (centerpoint::getSemanticType (" CAR" ), autoware_auto_perception_msgs::msg::ObjectClassification::CAR);
80
- EXPECT_EQ (centerpoint::getSemanticType (" TRUCK" ), autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK);
81
- EXPECT_EQ (centerpoint::getSemanticType (" BUS" ), autoware_auto_perception_msgs::msg::ObjectClassification::BUS);
82
- EXPECT_EQ (centerpoint::getSemanticType (" TRAILER" ), autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER);
83
- EXPECT_EQ (centerpoint::getSemanticType (" BICYCLE" ), autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE);
84
- EXPECT_EQ (centerpoint::getSemanticType (" MOTORBIKE" ), autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE);
85
- EXPECT_EQ (centerpoint::getSemanticType (" PEDESTRIAN" ), autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN);
86
- EXPECT_EQ (centerpoint::getSemanticType (" UNKNOWN" ), autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN);
83
+ EXPECT_EQ (
84
+ centerpoint::getSemanticType (" CAR" ),
85
+ autoware_auto_perception_msgs::msg::ObjectClassification::CAR);
86
+ EXPECT_EQ (
87
+ centerpoint::getSemanticType (" TRUCK" ),
88
+ autoware_auto_perception_msgs::msg::ObjectClassification::TRUCK);
89
+ EXPECT_EQ (
90
+ centerpoint::getSemanticType (" BUS" ),
91
+ autoware_auto_perception_msgs::msg::ObjectClassification::BUS);
92
+ EXPECT_EQ (
93
+ centerpoint::getSemanticType (" TRAILER" ),
94
+ autoware_auto_perception_msgs::msg::ObjectClassification::TRAILER);
95
+ EXPECT_EQ (
96
+ centerpoint::getSemanticType (" BICYCLE" ),
97
+ autoware_auto_perception_msgs::msg::ObjectClassification::BICYCLE);
98
+ EXPECT_EQ (
99
+ centerpoint::getSemanticType (" MOTORBIKE" ),
100
+ autoware_auto_perception_msgs::msg::ObjectClassification::MOTORCYCLE);
101
+ EXPECT_EQ (
102
+ centerpoint::getSemanticType (" PEDESTRIAN" ),
103
+ autoware_auto_perception_msgs::msg::ObjectClassification::PEDESTRIAN);
104
+ EXPECT_EQ (
105
+ centerpoint::getSemanticType (" UNKNOWN" ),
106
+ autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN);
87
107
}
88
108
89
109
TEST (TestSuite, convertPoseCovarianceMatrix)
@@ -114,8 +134,8 @@ TEST(TestSuite, convertTwistCovarianceMatrix)
114
134
EXPECT_FLOAT_EQ (twist_covariance[7 ], 0.2 );
115
135
}
116
136
117
- int main (int argc, char **argv)
137
+ int main (int argc, char ** argv)
118
138
{
119
139
testing::InitGoogleTest (&argc, argv);
120
140
return RUN_ALL_TESTS ();
121
- }
141
+ }
0 commit comments