|
| 1 | +// Copyright 2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include "lidar_centerpoint/ros_utils.hpp" |
| 16 | + |
| 17 | +#include <gtest/gtest.h> |
| 18 | + |
| 19 | +TEST(TestSuite, box3DToDetectedObject) |
| 20 | +{ |
| 21 | + std::vector<std::string> class_names = {"CAR", "TRUCK", "BUS", "TRAILER", |
| 22 | + "BICYCLE", "MOTORBIKE", "PEDESTRIAN"}; |
| 23 | + |
| 24 | + // Test case 1: Test with valid label, has_twist=true, has_variance=true |
| 25 | + { |
| 26 | + centerpoint::Box3D box3d; |
| 27 | + box3d.score = 0.8f; |
| 28 | + box3d.label = 0; // CAR |
| 29 | + box3d.x = 1.0; |
| 30 | + box3d.y = 2.0; |
| 31 | + box3d.z = 3.0; |
| 32 | + box3d.yaw = 0.5; |
| 33 | + box3d.length = 4.0; |
| 34 | + box3d.width = 2.0; |
| 35 | + box3d.height = 1.5; |
| 36 | + box3d.vel_x = 1.0; |
| 37 | + box3d.vel_y = 0.5; |
| 38 | + box3d.x_variance = 0.1; |
| 39 | + box3d.y_variance = 0.2; |
| 40 | + box3d.z_variance = 0.3; |
| 41 | + box3d.yaw_variance = 0.4; |
| 42 | + box3d.vel_x_variance = 0.5; |
| 43 | + box3d.vel_y_variance = 0.6; |
| 44 | + |
| 45 | + autoware_auto_perception_msgs::msg::DetectedObject obj; |
| 46 | + centerpoint::box3DToDetectedObject(box3d, class_names, true, true, obj); |
| 47 | + |
| 48 | + EXPECT_FLOAT_EQ(obj.existence_probability, 0.8f); |
| 49 | + EXPECT_EQ( |
| 50 | + obj.classification[0].label, autoware_auto_perception_msgs::msg::ObjectClassification::CAR); |
| 51 | + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.x, 1.0); |
| 52 | + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.y, 2.0); |
| 53 | + EXPECT_FLOAT_EQ(obj.kinematics.pose_with_covariance.pose.position.z, 3.0); |
| 54 | + EXPECT_FLOAT_EQ(obj.shape.dimensions.x, 4.0); |
| 55 | + EXPECT_FLOAT_EQ(obj.shape.dimensions.y, 2.0); |
| 56 | + EXPECT_FLOAT_EQ(obj.shape.dimensions.z, 1.5); |
| 57 | + EXPECT_TRUE(obj.kinematics.has_position_covariance); |
| 58 | + EXPECT_TRUE(obj.kinematics.has_twist); |
| 59 | + EXPECT_TRUE(obj.kinematics.has_twist_covariance); |
| 60 | + } |
| 61 | + |
| 62 | + // Test case 2: Test with invalid label, has_twist=false, has_variance=false |
| 63 | + { |
| 64 | + centerpoint::Box3D box3d; |
| 65 | + box3d.score = 0.5f; |
| 66 | + box3d.label = 10; // Invalid |
| 67 | + |
| 68 | + autoware_auto_perception_msgs::msg::DetectedObject obj; |
| 69 | + centerpoint::box3DToDetectedObject(box3d, class_names, false, false, obj); |
| 70 | + |
| 71 | + EXPECT_FLOAT_EQ(obj.existence_probability, 0.5f); |
| 72 | + EXPECT_EQ( |
| 73 | + obj.classification[0].label, |
| 74 | + autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN); |
| 75 | + EXPECT_FALSE(obj.kinematics.has_position_covariance); |
| 76 | + EXPECT_FALSE(obj.kinematics.has_twist); |
| 77 | + EXPECT_FALSE(obj.kinematics.has_twist_covariance); |
| 78 | + } |
| 79 | +} |
| 80 | + |
| 81 | +TEST(TestSuite, getSemanticType) |
| 82 | +{ |
| 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); |
| 107 | +} |
| 108 | + |
| 109 | +TEST(TestSuite, convertPoseCovarianceMatrix) |
| 110 | +{ |
| 111 | + centerpoint::Box3D box3d; |
| 112 | + box3d.x_variance = 0.1; |
| 113 | + box3d.y_variance = 0.2; |
| 114 | + box3d.z_variance = 0.3; |
| 115 | + box3d.yaw_variance = 0.4; |
| 116 | + |
| 117 | + std::array<double, 36> pose_covariance = centerpoint::convertPoseCovarianceMatrix(box3d); |
| 118 | + |
| 119 | + EXPECT_FLOAT_EQ(pose_covariance[0], 0.1); |
| 120 | + EXPECT_FLOAT_EQ(pose_covariance[7], 0.2); |
| 121 | + EXPECT_FLOAT_EQ(pose_covariance[14], 0.3); |
| 122 | + EXPECT_FLOAT_EQ(pose_covariance[35], 0.4); |
| 123 | +} |
| 124 | + |
| 125 | +TEST(TestSuite, convertTwistCovarianceMatrix) |
| 126 | +{ |
| 127 | + centerpoint::Box3D box3d; |
| 128 | + box3d.vel_x_variance = 0.1; |
| 129 | + box3d.vel_y_variance = 0.2; |
| 130 | + |
| 131 | + std::array<double, 36> twist_covariance = centerpoint::convertTwistCovarianceMatrix(box3d); |
| 132 | + |
| 133 | + EXPECT_FLOAT_EQ(twist_covariance[0], 0.1); |
| 134 | + EXPECT_FLOAT_EQ(twist_covariance[7], 0.2); |
| 135 | +} |
| 136 | + |
| 137 | +int main(int argc, char ** argv) |
| 138 | +{ |
| 139 | + testing::InitGoogleTest(&argc, argv); |
| 140 | + return RUN_ALL_TESTS(); |
| 141 | +} |
0 commit comments