From f95c450e3f429744ffa7508137e25bfa682019c2 Mon Sep 17 00:00:00 2001 From: eiki <53928021+N-Eiki@users.noreply.github.com> Date: Tue, 27 Feb 2024 07:58:30 +0900 Subject: [PATCH] feat(radar_crossing_noise_filter): add unit test (#6257) * update node Signed-off-by: N-Eiki * add test Signed-off-by: N-Eiki * style(pre-commit): autofix * update year Signed-off-by: N-Eiki * use tier4_autoware_utils Signed-off-by: N-Eiki * style(pre-commit): autofix --------- Signed-off-by: N-Eiki Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Satoshi Tanaka <16330533+scepter914@users.noreply.github.com> --- .../CMakeLists.txt | 8 + ...dar_crossing_objects_noise_filter_node.hpp | 1 + .../test_radar_crossing_objects_filter.cpp | 26 +++ ...radar_crossing_objects_filter_is_noise.cpp | 175 ++++++++++++++++++ 4 files changed, 210 insertions(+) create mode 100644 perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp create mode 100644 perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index dd179e99829a3..bf4a46cda7ae6 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -17,9 +17,17 @@ rclcpp_components_register_node(radar_crossing_objects_noise_filter_node_compone # Tests if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(radar_crossing_objects_noise_filter ${test_files}) + + target_link_libraries(radar_crossing_objects_noise_filter + radar_crossing_objects_noise_filter_node_component + ) endif() # Package diff --git a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp b/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp index 3b4674072a742..7ef369840c791 100644 --- a/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp +++ b/perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp @@ -58,6 +58,7 @@ class RadarCrossingObjectsNoiseFilterNode : public rclcpp::Node // Parameter NodeParam node_param_{}; +public: // Core bool isNoise(const DetectedObject & object); }; 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..43f030ce8493a --- /dev/null +++ b/perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp @@ -0,0 +1,26 @@ +// Copyright 2024 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..16d3cad8c3314 --- /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,175 @@ +// Copyright 2024 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 "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" + +#include + +#include + +std::shared_ptr get_node( + double angle_threshold, double 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; +} + +autoware_auto_perception_msgs::msg::DetectedObject get_object( + geometry_msgs::msg::Vector3 velocity, geometry_msgs::msg::Point position, + geometry_msgs::msg::Quaternion orientation) +{ + autoware_auto_perception_msgs::msg::DetectedObject object; + object.kinematics.twist_with_covariance.twist.linear = velocity; + object.kinematics.pose_with_covariance.pose.position = position; + object.kinematics.pose_with_covariance.pose.orientation = orientation; + return object; +} + +TEST(RadarCrossingObjectsFilter, IsNoise) +{ + rclcpp::init(0, nullptr); + { + auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + } + + { + auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + } + + { + auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_TRUE(node->isNoise(object)); + } + } + + { + auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0); + auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0); + auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0); + auto object = get_object(velocity, position, orientation); + { + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + { + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; + auto node = get_node(angle_threshold, velocity_threshold); + EXPECT_FALSE(node->isNoise(object)); + } + } +}