From 588ecf1a7ee491723f59f8ed0a976db680556df5 Mon Sep 17 00:00:00 2001 From: N-Eiki Date: Wed, 31 Jan 2024 20:05:48 +0900 Subject: [PATCH 1/6] update node Signed-off-by: N-Eiki --- .../radar_crossing_objects_noise_filter/CMakeLists.txt | 8 ++++++++ .../radar_crossing_objects_noise_filter_node.hpp | 1 + 2 files changed, 9 insertions(+) diff --git a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt index 81b1daabaa7ef..026a59fde7287 100644 --- a/perception/radar_crossing_objects_noise_filter/CMakeLists.txt +++ b/perception/radar_crossing_objects_noise_filter/CMakeLists.txt @@ -28,9 +28,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); }; From fb93d2ccd5dfa200d5a93071b81a1c1c4585ab20 Mon Sep 17 00:00:00 2001 From: N-Eiki Date: Wed, 31 Jan 2024 20:06:02 +0900 Subject: [PATCH 2/6] add test Signed-off-by: N-Eiki --- .../test_radar_crossing_objects_filter.cpp | 26 +++ ...radar_crossing_objects_filter_is_noise.cpp | 208 ++++++++++++++++++ 2 files changed, 234 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/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)); + } + } +} + From c22a15d565c148696be928312786eef108f9d8c8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Wed, 31 Jan 2024 11:15:49 +0000 Subject: [PATCH 3/6] style(pre-commit): autofix --- ...radar_crossing_objects_filter_is_noise.cpp | 70 +++++++++---------- 1 file changed, 35 insertions(+), 35 deletions(-) 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 index 908a28583eab1..e0808d76494d4 100644 --- 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 @@ -13,6 +13,7 @@ // limitations under the License. #include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" + #include std::shared_ptr get_node( @@ -21,26 +22,18 @@ std::shared_ptr(node_options); + 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 - ) + 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(); + 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); @@ -67,28 +60,30 @@ TEST(RadarCrossingObjectsFilter, IsNoise) 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); + 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; + 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; + 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; + 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; + float angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_TRUE(node->isNoise(*object)); } @@ -104,28 +99,30 @@ TEST(RadarCrossingObjectsFilter, IsNoise) 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); + 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; + 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; + 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; + 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; + float angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_FALSE(node->isNoise(*object)); } @@ -141,28 +138,30 @@ TEST(RadarCrossingObjectsFilter, IsNoise) 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); + 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; + 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; + 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; + 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; + float angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_TRUE(node->isNoise(*object)); } @@ -178,31 +177,32 @@ TEST(RadarCrossingObjectsFilter, IsNoise) 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); + 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; + 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; + 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; + 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; + float angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_FALSE(node->isNoise(*object)); } } } - From b663de1478db63ee3fdb95f6ff047874f46d1791 Mon Sep 17 00:00:00 2001 From: N-Eiki Date: Thu, 22 Feb 2024 15:51:08 +0900 Subject: [PATCH 4/6] update year Signed-off-by: N-Eiki --- .../test/test_radar_crossing_objects_filter.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 55032a7b62e79..43f030ce8493a 100644 --- 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 @@ -1,4 +1,4 @@ -// Copyright 2023 TierIV +// 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. From db09e0687881070077cd2af78822ebf7719bccc8 Mon Sep 17 00:00:00 2001 From: N-Eiki Date: Thu, 22 Feb 2024 15:51:51 +0900 Subject: [PATCH 5/6] use tier4_autoware_utils Signed-off-by: N-Eiki --- ...radar_crossing_objects_filter_is_noise.cpp | 191 ++++++++---------- 1 file changed, 80 insertions(+), 111 deletions(-) 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 index 908a28583eab1..ff9829b2ef978 100644 --- 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 @@ -1,4 +1,4 @@ -// Copyright 2023 Tier IV, Inc. +// 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. @@ -12,11 +12,16 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "tier4_autoware_utils/geometry/geometry.hpp" +#include "tier4_autoware_utils/math/unit_conversion.hpp" + +#include + #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) + double angle_threshold, double velocity_threshold) { rclcpp::NodeOptions node_options; @@ -31,177 +36,141 @@ 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; +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); - { - 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); + 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); { - float velocity_threshold = 40.0; - float angle_threshold = 1.0472; + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_TRUE(node->isNoise(*object)); + EXPECT_TRUE(node->isNoise(object)); } { - float velocity_threshold = 40.0; - float angle_threshold = -1.0472; + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_TRUE(node->isNoise(*object)); + EXPECT_TRUE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = 1.0472; + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_TRUE(node->isNoise(*object)); + EXPECT_TRUE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = -1.0472; + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_TRUE(node->isNoise(*object)); + 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); + 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); { - float velocity_threshold = 40.0; - float angle_threshold = 1.0472; + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = 40.0; - float angle_threshold = -1.0472; + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = 1.0472; + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = -1.0472; + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + 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); + 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); { - float velocity_threshold = 40.0; - float angle_threshold = 1.0472; + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = 40.0; - float angle_threshold = -1.0472; + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = 1.0472; + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_TRUE(node->isNoise(*object)); + EXPECT_TRUE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = -1.0472; + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_TRUE(node->isNoise(*object)); + 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); + 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); { - float velocity_threshold = 40.0; - float angle_threshold = 1.0472; + double velocity_threshold = 40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = 40.0; - float angle_threshold = -1.0472; + double velocity_threshold = 40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = 1.0472; + double velocity_threshold = -40.0; + double angle_threshold = 1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } { - float velocity_threshold = -40.0; - float angle_threshold = -1.0472; + double velocity_threshold = -40.0; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); - EXPECT_FALSE(node->isNoise(*object)); + EXPECT_FALSE(node->isNoise(object)); } } } From 6fa23a4b8037dbe2492ca1d94033435bd63c4d97 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 22 Feb 2024 07:05:47 +0000 Subject: [PATCH 6/6] style(pre-commit): autofix --- ...radar_crossing_objects_filter_is_noise.cpp | 51 +++++++++---------- 1 file changed, 25 insertions(+), 26 deletions(-) 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 index 76bc470eb9311..16d3cad8c3314 100644 --- 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 @@ -12,13 +12,12 @@ // 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 "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp" - #include std::shared_ptr get_node( @@ -35,16 +34,16 @@ std::shared_ptrisNoise(object)); } { double velocity_threshold = 40.0; - double angle_threshold = -1.0472; + 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; + 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; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_TRUE(node->isNoise(object)); } @@ -88,25 +87,25 @@ TEST(RadarCrossingObjectsFilter, IsNoise) auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; - double angle_threshold = 1.0472; + 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; + 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; + 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; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_FALSE(node->isNoise(object)); } @@ -119,25 +118,25 @@ TEST(RadarCrossingObjectsFilter, IsNoise) auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; - double angle_threshold = 1.0472; + 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; + 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; + 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; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_TRUE(node->isNoise(object)); } @@ -150,25 +149,25 @@ TEST(RadarCrossingObjectsFilter, IsNoise) auto object = get_object(velocity, position, orientation); { double velocity_threshold = 40.0; - double angle_threshold = 1.0472; + 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; + 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; + 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; + double angle_threshold = -1.0472; auto node = get_node(angle_threshold, velocity_threshold); EXPECT_FALSE(node->isNoise(object)); }