diff --git a/perception/simple_object_merger/CMakeLists.txt b/perception/simple_object_merger/CMakeLists.txt index 10bd0efa7b8fc..71f8d7c668093 100644 --- a/perception/simple_object_merger/CMakeLists.txt +++ b/perception/simple_object_merger/CMakeLists.txt @@ -17,9 +17,13 @@ rclcpp_components_register_node(simple_object_merger_node_component # Tests if(BUILD_TESTING) - 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(test_simple_object_merger_node ${test_files}) + + target_link_libraries(test_simple_object_merger_node + simple_object_merger_node_component + ) endif() # Package diff --git a/perception/simple_object_merger/package.xml b/perception/simple_object_merger/package.xml index 487155bffd102..1f65c3afdc5d3 100644 --- a/perception/simple_object_merger/package.xml +++ b/perception/simple_object_merger/package.xml @@ -12,7 +12,9 @@ ament_cmake_auto + ament_cmake_ros autoware_perception_msgs + autoware_test_utils autoware_universe_utils geometry_msgs rclcpp diff --git a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp index faa531560ba09..56ca0fa57a571 100644 --- a/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp +++ b/perception/simple_object_merger/src/simple_object_merger_node/simple_object_merger_node.cpp @@ -15,6 +15,7 @@ #include "simple_object_merger/simple_object_merger_node.hpp" #include +#include #include #include diff --git a/perception/simple_object_merger/test/test_simple_object_merger_node.cpp b/perception/simple_object_merger/test/test_simple_object_merger_node.cpp new file mode 100644 index 0000000000000..d6346b03e625a --- /dev/null +++ b/perception/simple_object_merger/test/test_simple_object_merger_node.cpp @@ -0,0 +1,150 @@ +// 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 "simple_object_merger/simple_object_merger_node.hpp" + +#include +#include + +#include + +#include + +using autoware_perception_msgs::msg::DetectedObject; +using autoware_perception_msgs::msg::DetectedObjects; +using autoware_perception_msgs::msg::ObjectClassification; +using simple_object_merger::SimpleObjectMergerNode; + +std::shared_ptr generateTestManager() +{ + auto test_manager = std::make_shared(); + return test_manager; +} + +std::shared_ptr generateNode() +{ + auto node_options = rclcpp::NodeOptions{}; + node_options.append_parameter_override("use_sim_time", true); + node_options.append_parameter_override("update_rate_hz", 20.0); + node_options.append_parameter_override("new_frame_id", "base_link"); + node_options.append_parameter_override("timeout_threshold", 1.0); + node_options.append_parameter_override( + "input_topics", std::vector{"/input/objects1", "/input/objects2"}); + + return std::make_shared(node_options); +} + +TEST(SimpleObjectMergerNodeTest, testSimpleObjectMerge) +{ + rclcpp::init(0, nullptr); + const std::string input_topic_1 = "/input/objects1"; + const std::string input_topic_2 = "/input/objects2"; + const std::string output_topic = "/simple_object_merger/output/objects"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + // Create subscriber + size_t latest_msg_objects_size = 0; + auto callback = [&latest_msg_objects_size](const DetectedObjects::ConstSharedPtr msg) { + latest_msg_objects_size = msg->objects.size(); + }; + test_manager->set_subscriber(output_topic, callback); + + // Create a DetectedObjects message with one object + DetectedObjects msg; + msg.header.frame_id = "base_link"; + { + DetectedObject object; + object.kinematics.pose_with_covariance.pose.position.x = 10.0; + object.kinematics.pose_with_covariance.pose.position.y = 5.0; + object.shape.dimensions.x = 1.0; + object.shape.dimensions.y = 1.0; + object.shape.dimensions.z = 1.0; + object.classification.resize(1); + object.classification[0].label = ObjectClassification::UNKNOWN; + msg.objects.push_back(object); + } + + // Set start time + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::Time current_time(0, 0, clock->get_clock_type()); + + // Publish the message to topic1 + test_manager->jump_clock(current_time); + msg.header.stamp = current_time; + test_manager->test_pub_msg(test_target_node, input_topic_1, msg); + + // Increment time for 0.1 sec + current_time = current_time + rclcpp::Duration::from_seconds(0.1); // 0.1 sec + + // Publish the message to topic2 + test_manager->jump_clock(current_time); + msg.header.stamp = current_time; + test_manager->test_pub_msg(test_target_node, input_topic_2, msg); + + EXPECT_EQ(latest_msg_objects_size, 2u); + rclcpp::shutdown(); +} + +TEST(SimpleObjectMergerNodeTest, testSimpleObjectMergeTimeOut) +{ + rclcpp::init(0, nullptr); + const std::string input_topic_1 = "/input/objects1"; + const std::string input_topic_2 = "/input/objects2"; + const std::string output_topic = "/simple_object_merger/output/objects"; + auto test_manager = generateTestManager(); + auto test_target_node = generateNode(); + + // Create subscriber + size_t latest_msg_objects_size = 0; + auto callback = [&latest_msg_objects_size](const DetectedObjects::ConstSharedPtr msg) { + latest_msg_objects_size = msg->objects.size(); + }; + test_manager->set_subscriber(output_topic, callback); + + // Create a DetectedObjects message with one object + DetectedObjects msg; + msg.header.frame_id = "base_link"; + { + DetectedObject object; + object.kinematics.pose_with_covariance.pose.position.x = 10.0; + object.kinematics.pose_with_covariance.pose.position.y = 5.0; + object.shape.dimensions.x = 1.0; + object.shape.dimensions.y = 1.0; + object.shape.dimensions.z = 1.0; + object.classification.resize(1); + object.classification[0].label = ObjectClassification::UNKNOWN; + msg.objects.push_back(object); + } + + // Set start time + auto clock = std::make_shared(RCL_ROS_TIME); + rclcpp::Time current_time(0, 0, clock->get_clock_type()); + + // Publish the message to topic1 + test_manager->jump_clock(current_time); + msg.header.stamp = current_time; + test_manager->test_pub_msg(test_target_node, input_topic_1, msg); + + // Increment time for 2.0 sec + current_time = current_time + rclcpp::Duration::from_seconds(2.0); // 2.0 sec + + // Publish the message to topic2 + test_manager->jump_clock(current_time); + msg.header.stamp = current_time; + test_manager->test_pub_msg(test_target_node, input_topic_2, msg); + + EXPECT_EQ(latest_msg_objects_size, 1u); + rclcpp::shutdown(); +}