feat(radar_threshold_filter): add unit test (#5817)

* update cmakelists Signed-off-by: N-Eiki <[email protected]> * add z filter param Signed-off-by: N-Eiki <[email protected]> * isWithin func private -> public Signed-off-by: N-Eiki <[email protected]> * add test code Signed-off-by: N-Eiki <[email protected]> * style(pre-commit): autofix --------- Signed-off-by: N-Eiki <[email protected]> Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Satoshi Tanaka <[email protected]>
autowarefoundation · Dec 11, 2023 · 84b658b · 84b658b
1 parent 8cd0628
commit 84b658b
Show file tree

Hide file tree

Showing 5 changed files with 192 additions and 0 deletions.
diff --git a/sensing/radar_threshold_filter/CMakeLists.txt b/sensing/radar_threshold_filter/CMakeLists.txt
@@ -17,10 +17,18 @@ rclcpp_components_register_node(radar_threshold_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_threshold_filter ${test_files})
+
+  target_link_libraries(radar_threshold_filter
+    radar_threshold_filter_node_component
+  )
 endif()
 
 ## Package

diff --git a/...ing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp b/...ing/radar_threshold_filter/include/radar_threshold_filter/radar_threshold_filter_node.hpp
@@ -68,6 +68,7 @@ class RadarThresholdFilterNode : public rclcpp::Node
   // Parameter
   NodeParam node_param_{};
 
+public:
   // Function
   bool isWithinThreshold(const RadarReturn & radar_return);
 };

diff --git a/...ng/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp b/...ng/radar_threshold_filter/src/radar_threshold_filter_node/radar_threshold_filter_node.cpp
@@ -74,6 +74,9 @@ RadarThresholdFilterNode::RadarThresholdFilterNode(const rclcpp::NodeOptions & n
   node_param_.is_azimuth_filter = declare_parameter<bool>("node_params.is_azimuth_filter", false);
   node_param_.azimuth_min = declare_parameter<double>("node_params.azimuth_min", 0.0);
   node_param_.azimuth_max = declare_parameter<double>("node_params.azimuth_max", 0.0);
+  node_param_.is_z_filter = declare_parameter<bool>("node_params.is_z_filter", false);
+  node_param_.z_min = declare_parameter<double>("node_params.z_min", 0.0);
+  node_param_.z_max = declare_parameter<double>("node_params.z_max", 0.0);
 
   // Subscriber
   sub_radar_ = create_subscription<RadarScan>(
@@ -99,6 +102,9 @@ rcl_interfaces::msg::SetParametersResult RadarThresholdFilterNode::onSetParam(
       update_param(params, "node_params.is_azimuth_filter", p.is_azimuth_filter);
       update_param(params, "node_params.azimuth_min", p.azimuth_min);
       update_param(params, "node_params.azimuth_max", p.azimuth_max);
+      update_param(params, "node_params.is_z_filter", p.is_z_filter);
+      update_param(params, "node_params.z_min", p.z_min);
+      update_param(params, "node_params.z_max", p.z_max);
     }
   } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) {
     result.successful = false;

diff --git a/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp b/sensing/radar_threshold_filter/test/radar_threshold_filter/test_radar_threshold_filter.cpp
@@ -0,0 +1,151 @@
+// 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_threshold_filter/radar_threshold_filter_node.hpp"
+
+#include <radar_msgs/msg/radar_scan.hpp>
+
+#include <gtest/gtest.h>
+
+TEST(RadarThresholdFilter, isWithinThreshold)
+{
+  rclcpp::init(0, nullptr);
+  using radar_msgs::msg::RadarReturn;
+  using radar_threshold_filter::RadarThresholdFilterNode;
+
+  const double amplitude_min = -10.0;
+  const double amplitude_max = 100.0;
+  const double range_min = 20.0;
+  const double range_max = 300.0;
+  const double azimuth_min = -1.2;
+  const double azimuth_max = 1.2;
+  const double z_min = -2.0;
+  const double z_max = 5.0;
+  // amplitude filter
+  {
+    rclcpp::NodeOptions node_options;
+    node_options.parameter_overrides({
+      {"node_params.is_amplitude_filter", true},
+      {"node_params.amplitude_min", amplitude_min},
+      {"node_params.amplitude_max", amplitude_max},
+      {"node_params.is_range_filter", false},
+      {"node_params.range_min", range_min},
+      {"node_params.range_max", range_max},
+      {"node_params.is_azimuth_filter", false},
+      {"node_params.azimuth_min", azimuth_min},
+      {"node_params.azimuth_max", azimuth_max},
+      {"node_params.is_z_filter", false},
+      {"node_params.z_min", z_min},
+      {"node_params.z_max", z_max},
+    });
+
+    RadarThresholdFilterNode node(node_options);
+    RadarReturn radar_return;
+
+    // Now you can use radar_return and set its fields as needed
+    radar_return.amplitude = -100.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+    radar_return.amplitude = 0.0;
+    EXPECT_TRUE(node.isWithinThreshold(radar_return));
+    radar_return.amplitude = 1000.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+  }
+
+  // range filter
+  {
+    rclcpp::NodeOptions node_options;
+    node_options.parameter_overrides({
+      {"node_params.is_amplitude_filter", false},
+      {"node_params.amplitude_min", amplitude_min},
+      {"node_params.amplitude_max", amplitude_max},
+      {"node_params.is_range_filter", true},
+      {"node_params.range_min", range_min},
+      {"node_params.range_max", range_max},
+      {"node_params.is_azimuth_filter", false},
+      {"node_params.azimuth_min", azimuth_min},
+      {"node_params.azimuth_max", azimuth_max},
+      {"node_params.is_z_filter", false},
+      {"node_params.z_min", z_min},
+      {"node_params.z_max", z_max},
+    });
+
+    RadarThresholdFilterNode node(node_options);
+    RadarReturn radar_return;
+
+    radar_return.range = 0.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+    radar_return.range = 100.0;
+    EXPECT_TRUE(node.isWithinThreshold(radar_return));
+    radar_return.range = 1000.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+  }
+
+  // azimuth filter
+  {
+    rclcpp::NodeOptions node_options;
+    node_options.parameter_overrides({
+      {"node_params.is_amplitude_filter", false},
+      {"node_params.amplitude_min", amplitude_min},
+      {"node_params.amplitude_max", amplitude_max},
+      {"node_params.is_range_filter", false},
+      {"node_params.range_min", range_min},
+      {"node_params.range_max", range_max},
+      {"node_params.is_azimuth_filter", true},
+      {"node_params.azimuth_min", azimuth_min},
+      {"node_params.azimuth_max", azimuth_max},
+      {"node_params.is_z_filter", false},
+      {"node_params.z_min", z_min},
+      {"node_params.z_max", z_max},
+    });
+
+    RadarThresholdFilterNode node(node_options);
+    RadarReturn radar_return;
+    radar_return.azimuth = -10.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+    radar_return.azimuth = 0.0;
+    EXPECT_TRUE(node.isWithinThreshold(radar_return));
+    radar_return.azimuth = 10.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+  }
+
+  // z
+  {
+    rclcpp::NodeOptions node_options;
+    node_options.parameter_overrides({
+      {"node_params.is_amplitude_filter", false},
+      {"node_params.amplitude_min", amplitude_min},
+      {"node_params.amplitude_max", amplitude_max},
+      {"node_params.is_range_filter", false},
+      {"node_params.range_min", range_min},
+      {"node_params.range_max", range_max},
+      {"node_params.is_azimuth_filter", false},
+      {"node_params.azimuth_min", azimuth_min},
+      {"node_params.azimuth_max", azimuth_max},
+      {"node_params.is_z_filter", true},
+      {"node_params.z_min", z_min},
+      {"node_params.z_max", z_max},
+    });
+
+    RadarThresholdFilterNode node(node_options);
+    RadarReturn radar_return;
+    radar_return.elevation = M_PI / 2;
+    // range =  z / std::sin(radar_return.elevation)
+    radar_return.range = -10.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+    radar_return.range = 0.0;
+    EXPECT_TRUE(node.isWithinThreshold(radar_return));
+    radar_return.range = 10.0;
+    EXPECT_FALSE(node.isWithinThreshold(radar_return));
+  }
+}
diff --git a/sensing/radar_threshold_filter/test/test_threshold_filter.cpp b/sensing/radar_threshold_filter/test/test_threshold_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 <rclcpp/rclcpp.hpp>
+
+#include <gtest/gtest.h>
+
+int main(int argc, char * argv[])
+{
+  testing::InitGoogleTest(&argc, argv);
+  rclcpp::init(argc, argv);
+  bool result = RUN_ALL_TESTS();
+  rclcpp::shutdown();
+  return result;
+}