diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp new file mode 100644 index 0000000000000..55032a7b62e79 --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp @@ -0,0 +1,26 @@ +// Copyright 2023 TierIV +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp new file mode 100644 index 0000000000000..908a28583eab1 --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp @@ -0,0 +1,208 @@ +// Copyright 2023 Tier IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" +#include + +std::shared_ptr get_node( + float angle_threshold, float velocity_threshold) +{ + rclcpp::NodeOptions node_options; + + node_options.parameter_overrides( + { + {"angle_threshold", angle_threshold}, + {"velocity_threshold", velocity_threshold} + } +); + auto node = + std::make_shared(node_options); + return node; +} + + +std::shared_ptr get_object( + float twist_x, float twist_y, float twist_z, + float orientation_x, float orientation_y, float orientation_z, float orientation_w, + float pose_x, float pose_y + ) +{ + + auto ret = + std::make_shared(); + ret->kinematics.twist_with_covariance.twist.linear.x = static_cast(twist_x); + ret->kinematics.twist_with_covariance.twist.linear.y = static_cast(twist_y); + ret->kinematics.twist_with_covariance.twist.linear.z = static_cast(twist_z); + ret->kinematics.pose_with_covariance.pose.position.x = static_cast(pose_x); + ret->kinematics.pose_with_covariance.pose.position.y = static_cast(pose_y); + ret->kinematics.pose_with_covariance.pose.orientation.x = static_cast(orientation_x); + ret->kinematics.pose_with_covariance.pose.orientation.y = static_cast(orientation_y); + ret->kinematics.pose_with_covariance.pose.orientation.z = static_cast(orientation_z); + ret->kinematics.pose_with_covariance.pose.orientation.w = static_cast(orientation_w); + return ret; +} + +TEST(RadarCrossingObjectsFilter, IsNoise) +{ + rclcpp::init(0, nullptr); + + { + float twist_x = 40.0; + float twist_y = 30.0; + float twist_z = 0.0; + float orientation_x = 1.0; + float orientation_y = 1.0; + float orientation_z = 1.0; + float orientation_w = 0.0; + float pose_x = 1.0; + float pose_y = 0.0; + auto object = get_object(twist_x, twist_y, twist_z, orientation_x, orientation_y, orientation_z, orientation_w, pose_x, pose_y); + { + float velocity_threshold = 40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(*object)); + } + { + float velocity_threshold = 40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(*object)); + } + } + + { + float twist_x = 40.0; + float twist_y = 30.0; + float twist_z = 0.0; + float orientation_x = 1.0; + float orientation_y = 1.0; + float orientation_z = 1.0; + float orientation_w = 0.0; + float pose_x = 1.0; + float pose_y = 2.0; + auto object = get_object(twist_x, twist_y, twist_z, orientation_x, orientation_y, orientation_z, orientation_w, pose_x, pose_y); + { + float velocity_threshold = 40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = 40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + } + + { + float twist_x = 24.0; + float twist_y = 18.0; + float twist_z = 0.0; + float orientation_x = 1.0; + float orientation_y = 1.0; + float orientation_z = 1.0; + float orientation_w = 0.0; + float pose_x = 1.0; + float pose_y = 0.0; + auto object = get_object(twist_x, twist_y, twist_z, orientation_x, orientation_y, orientation_z, orientation_w, pose_x, pose_y); + { + float velocity_threshold = 40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = 40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(*object)); + } + } + + { + float twist_x = 24.0; + float twist_y = 18.0; + float twist_z = 0.0; + float orientation_x = 1.0; + float orientation_y = 1.0; + float orientation_z = 1.0; + float orientation_w = 0.0; + float pose_x = 1.0; + float pose_y = 2.0; + auto object = get_object(twist_x, twist_y, twist_z, orientation_x, orientation_y, orientation_z, orientation_w, pose_x, pose_y); + { + float velocity_threshold = 40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = 40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + { + float velocity_threshold = -40.0; + float angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(*object)); + } + } +} +