diff --git a/sensing/radar_tracks_noise_filter/CMakeLists.txt b/sensing/radar_tracks_noise_filter/CMakeLists.txt index 517a3f1df72f1..133a057cf1a12 100644 --- a/sensing/radar_tracks_noise_filter/CMakeLists.txt +++ b/sensing/radar_tracks_noise_filter/CMakeLists.txt @@ -28,9 +28,16 @@ rclcpp_components_register_node(radar_tracks_noise_filter_node_component # 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_tracks_noise_filter ${test_files}) + + target_link_libraries(radar_tracks_noise_filter + radar_tracks_noise_filter_node_component + ) endif() # Package diff --git a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp index 9b329da3e5579..2ff4025cc64bc 100644 --- a/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp +++ b/sensing/radar_tracks_noise_filter/include/radar_tracks_noise_filter/radar_tracks_noise_filter_node.hpp @@ -57,6 +57,7 @@ class RadarTrackCrossingNoiseFilterNode : public rclcpp::Node // Parameter NodeParam node_param_{}; +public: // Core bool isNoise(const RadarTrack & radar_track); }; diff --git a/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp new file mode 100644 index 0000000000000..aa08260dee2e7 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/radar_tracks_noise_filter/test_radar_tracks_noise_filter_is_noise.cpp @@ -0,0 +1,95 @@ +// 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_tracks_noise_filter/radar_tracks_noise_filter_node.hpp" + +#include + +#include + +std::shared_ptr get_node( + float velocity_y_threshold) +{ + rclcpp::NodeOptions node_options; + node_options.parameter_overrides({ + {"node_params.is_amplitude_filter", true}, + {"velocity_y_threshold", velocity_y_threshold}, + }); + + auto node = + std::make_shared(node_options); + return node; +} + +radar_msgs::msg::RadarTrack getRadarTrack(float velocity_x, float velocity_y) +{ + radar_msgs::msg::RadarTrack radar_track; + geometry_msgs::msg::Vector3 vector_msg; + vector_msg.x = velocity_x; + vector_msg.y = velocity_y; + vector_msg.z = 0.0; + radar_track.velocity = vector_msg; + return radar_track; +} + +TEST(RadarTracksNoiseFilter, isNoise) +{ + using radar_msgs::msg::RadarTrack; + using radar_tracks_noise_filter::RadarTrackCrossingNoiseFilterNode; + rclcpp::init(0, nullptr); + { + float velocity_node_threshold = 0.0; + float y_velocity = 0.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = -1.0; + float y_velocity = 3.0; + float x_velocity = 100.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_TRUE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = 10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } + + { + float velocity_node_threshold = 3.0; + float y_velocity = 1.0; + float x_velocity = -10.0; + std::shared_ptr node = get_node(velocity_node_threshold); + RadarTrack radar_track = getRadarTrack(x_velocity, y_velocity); + EXPECT_FALSE(node->isNoise(radar_track)); + } +} diff --git a/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_filter.cpp new file mode 100644 index 0000000000000..55032a7b62e79 --- /dev/null +++ b/sensing/radar_tracks_noise_filter/test/test_radar_tracks_noise_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; +}