12
12
// See the License for the specific language governing permissions and
13
13
// limitations under the License.
14
14
15
+ #include " radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp"
15
16
#include " tier4_autoware_utils/geometry/geometry.hpp"
16
17
#include " tier4_autoware_utils/math/unit_conversion.hpp"
17
18
18
19
#include < geometry_msgs/msg/point32.hpp>
19
20
20
- #include " radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp"
21
-
22
21
#include < gtest/gtest.h>
23
22
24
23
std::shared_ptr<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode> get_node (
@@ -35,16 +34,16 @@ std::shared_ptr<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFi
35
34
}
36
35
37
36
autoware_auto_perception_msgs::msg::DetectedObject get_object (
38
- geometry_msgs::msg::Vector3 velocity, geometry_msgs::msg::Point position, geometry_msgs::msg::Quaternion orientation
39
- ){
40
- autoware_auto_perception_msgs::msg::DetectedObject object;
41
- object.kinematics .twist_with_covariance .twist .linear = velocity;
42
- object.kinematics .pose_with_covariance .pose .position = position;
43
- object.kinematics .pose_with_covariance .pose .orientation = orientation;
44
- return object;
37
+ geometry_msgs::msg::Vector3 velocity, geometry_msgs::msg::Point position,
38
+ geometry_msgs::msg::Quaternion orientation)
39
+ {
40
+ autoware_auto_perception_msgs::msg::DetectedObject object;
41
+ object.kinematics .twist_with_covariance .twist .linear = velocity;
42
+ object.kinematics .pose_with_covariance .pose .position = position;
43
+ object.kinematics .pose_with_covariance .pose .orientation = orientation;
44
+ return object;
45
45
}
46
46
47
-
48
47
TEST (RadarCrossingObjectsFilter, IsNoise)
49
48
{
50
49
rclcpp::init (0 , nullptr );
@@ -55,27 +54,27 @@ TEST(RadarCrossingObjectsFilter, IsNoise)
55
54
auto object = get_object (velocity, position, orientation);
56
55
{
57
56
double velocity_threshold = 40.0 ;
58
- double angle_threshold = 1.0472 ;
57
+ double angle_threshold = 1.0472 ;
59
58
auto node = get_node (angle_threshold, velocity_threshold);
60
59
EXPECT_TRUE (node->isNoise (object));
61
60
}
62
61
{
63
62
double velocity_threshold = 40.0 ;
64
- double angle_threshold = -1.0472 ;
63
+ double angle_threshold = -1.0472 ;
65
64
66
65
auto node = get_node (angle_threshold, velocity_threshold);
67
66
EXPECT_TRUE (node->isNoise (object));
68
67
}
69
68
{
70
69
double velocity_threshold = -40.0 ;
71
- double angle_threshold = 1.0472 ;
70
+ double angle_threshold = 1.0472 ;
72
71
73
72
auto node = get_node (angle_threshold, velocity_threshold);
74
73
EXPECT_TRUE (node->isNoise (object));
75
74
}
76
75
{
77
76
double velocity_threshold = -40.0 ;
78
- double angle_threshold = -1.0472 ;
77
+ double angle_threshold = -1.0472 ;
79
78
auto node = get_node (angle_threshold, velocity_threshold);
80
79
EXPECT_TRUE (node->isNoise (object));
81
80
}
@@ -88,25 +87,25 @@ TEST(RadarCrossingObjectsFilter, IsNoise)
88
87
auto object = get_object (velocity, position, orientation);
89
88
{
90
89
double velocity_threshold = 40.0 ;
91
- double angle_threshold = 1.0472 ;
90
+ double angle_threshold = 1.0472 ;
92
91
auto node = get_node (angle_threshold, velocity_threshold);
93
92
EXPECT_FALSE (node->isNoise (object));
94
93
}
95
94
{
96
95
double velocity_threshold = 40.0 ;
97
- double angle_threshold = -1.0472 ;
96
+ double angle_threshold = -1.0472 ;
98
97
auto node = get_node (angle_threshold, velocity_threshold);
99
98
EXPECT_FALSE (node->isNoise (object));
100
99
}
101
100
{
102
101
double velocity_threshold = -40.0 ;
103
- double angle_threshold = 1.0472 ;
102
+ double angle_threshold = 1.0472 ;
104
103
auto node = get_node (angle_threshold, velocity_threshold);
105
104
EXPECT_FALSE (node->isNoise (object));
106
105
}
107
106
{
108
107
double velocity_threshold = -40.0 ;
109
- double angle_threshold = -1.0472 ;
108
+ double angle_threshold = -1.0472 ;
110
109
auto node = get_node (angle_threshold, velocity_threshold);
111
110
EXPECT_FALSE (node->isNoise (object));
112
111
}
@@ -119,25 +118,25 @@ TEST(RadarCrossingObjectsFilter, IsNoise)
119
118
auto object = get_object (velocity, position, orientation);
120
119
{
121
120
double velocity_threshold = 40.0 ;
122
- double angle_threshold = 1.0472 ;
121
+ double angle_threshold = 1.0472 ;
123
122
auto node = get_node (angle_threshold, velocity_threshold);
124
123
EXPECT_FALSE (node->isNoise (object));
125
124
}
126
125
{
127
126
double velocity_threshold = 40.0 ;
128
- double angle_threshold = -1.0472 ;
127
+ double angle_threshold = -1.0472 ;
129
128
auto node = get_node (angle_threshold, velocity_threshold);
130
129
EXPECT_FALSE (node->isNoise (object));
131
130
}
132
131
{
133
132
double velocity_threshold = -40.0 ;
134
- double angle_threshold = 1.0472 ;
133
+ double angle_threshold = 1.0472 ;
135
134
auto node = get_node (angle_threshold, velocity_threshold);
136
135
EXPECT_TRUE (node->isNoise (object));
137
136
}
138
137
{
139
138
double velocity_threshold = -40.0 ;
140
- double angle_threshold = -1.0472 ;
139
+ double angle_threshold = -1.0472 ;
141
140
auto node = get_node (angle_threshold, velocity_threshold);
142
141
EXPECT_TRUE (node->isNoise (object));
143
142
}
@@ -150,25 +149,25 @@ TEST(RadarCrossingObjectsFilter, IsNoise)
150
149
auto object = get_object (velocity, position, orientation);
151
150
{
152
151
double velocity_threshold = 40.0 ;
153
- double angle_threshold = 1.0472 ;
152
+ double angle_threshold = 1.0472 ;
154
153
auto node = get_node (angle_threshold, velocity_threshold);
155
154
EXPECT_FALSE (node->isNoise (object));
156
155
}
157
156
{
158
157
double velocity_threshold = 40.0 ;
159
- double angle_threshold = -1.0472 ;
158
+ double angle_threshold = -1.0472 ;
160
159
auto node = get_node (angle_threshold, velocity_threshold);
161
160
EXPECT_FALSE (node->isNoise (object));
162
161
}
163
162
{
164
163
double velocity_threshold = -40.0 ;
165
- double angle_threshold = 1.0472 ;
164
+ double angle_threshold = 1.0472 ;
166
165
auto node = get_node (angle_threshold, velocity_threshold);
167
166
EXPECT_FALSE (node->isNoise (object));
168
167
}
169
168
{
170
169
double velocity_threshold = -40.0 ;
171
- double angle_threshold = -1.0472 ;
170
+ double angle_threshold = -1.0472 ;
172
171
auto node = get_node (angle_threshold, velocity_threshold);
173
172
EXPECT_FALSE (node->isNoise (object));
174
173
}
0 commit comments