feat(radar_crossing_noise_filter): add unit test (#6257)

* update node Signed-off-by: N-Eiki <[email protected]> * add test Signed-off-by: N-Eiki <[email protected]> * style(pre-commit): autofix * update year Signed-off-by: N-Eiki <[email protected]> * use tier4_autoware_utils 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]> Signed-off-by: Kotaro Yoshimoto <[email protected]>
autowarefoundation · Mar 12, 2024 · 539461f · 539461f
1 parent 3773fef
commit 539461f
Show file tree

Hide file tree

Showing 4 changed files with 210 additions and 0 deletions.
diff --git 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/.../include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp b/.../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
@@ -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 <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;
+}
diff --git a/.../test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp b/.../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 <geometry_msgs/msg/point32.hpp>
+
+#include <gtest/gtest.h>
+
+std::shared_ptr<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode> 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<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode>(
+      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));
+    }
+  }
+}
-Original file line number
+Diff line change
@@ Expand Up @@
       // Parameter
       NodeParam node_param_{};
+    public:
       // Core
       bool isNoise(const DetectedObject & object);
     };
@@ Expand Down @@