From 108062d150f0e3aa5993a610add3f1020893c0a2 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Thu, 20 Jun 2024 11:36:16 +0200 Subject: [PATCH 1/6] test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Added unit tests to DetectionSensor --- .../detection_sensor/CMakeLists.txt | 3 + .../test_detection_sensor.cpp | 316 ++++++++++++++++++ .../test_detection_sensor.hpp | 178 ++++++++++ 3 files changed, 497 insertions(+) create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt index 846e00dd899..64d07ddb504 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt @@ -1,2 +1,5 @@ ament_add_gtest(test_detection_sensor_base test_detection_sensor_base.cpp) target_link_libraries(test_detection_sensor_base simple_sensor_simulator_component ${Protobuf_LIBRARIES}) + +ament_add_gtest(test_detection_sensor test_detection_sensor.cpp) +target_link_libraries(test_detection_sensor simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp new file mode 100644 index 00000000000..22383a5f379 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp @@ -0,0 +1,316 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// 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 "test_detection_sensor.hpp" + +#include + +#include "../expect_eq_macros.hpp" + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and UNKNOWN + * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test + * the standard detection functionality when an entity is detected because it is close to Ego. + */ +TEST_F(DetectionSensorTest, update_unknownSubtype) +{ + createEntity("unknown", EntityType::VEHICLE, EntitySubtype::UNKNOWN); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::UNKNOWN); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and CAR + * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test + * the standard detection functionality when an entity is detected because it is close to Ego. + */ +TEST_F(DetectionSensorTest, update_carSubtype) +{ + createEntity("car", EntityType::VEHICLE, EntitySubtype::CAR); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::CAR); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and TRUCK + * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test + * the standard detection functionality when an entity is detected because it is close to Ego. + */ +TEST_F(DetectionSensorTest, update_truckSubtype) +{ + createEntity("truck", EntityType::VEHICLE, EntitySubtype::TRUCK); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::TRUCK); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and BUS + * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test + * the standard detection functionality when an entity is detected because it is close to Ego. + */ +TEST_F(DetectionSensorTest, update_busSubtype) +{ + createEntity("bus", EntityType::VEHICLE, EntitySubtype::BUS); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::BUS); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and TRAILER + * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test + * the standard detection functionality when an entity is detected because it is close to Ego. + */ +TEST_F(DetectionSensorTest, update_trailerSubtype) +{ + createEntity("trailer", EntityType::VEHICLE, EntitySubtype::TRAILER); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::TRAILER); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and + * MOTORCYCLE entity positioned closer to Ego than the range parameter (in configuration) - the goal + * is to test the standard detection functionality when an entity is detected because it is close to + * Ego. + */ +TEST_F(DetectionSensorTest, update_motorcycleSubtype) +{ + createEntity("motorcycle", EntityType::VEHICLE, EntitySubtype::MOTORCYCLE); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::MOTORCYCLE); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and BICYCLE + * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test + * the standard detection functionality when an entity is detected because it is close to Ego + */ +TEST_F(DetectionSensorTest, update_bicycleSubtype) +{ + createEntity("bicycle", EntityType::VEHICLE, EntitySubtype::BICYCLE); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::BICYCLE); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration), no probability of lost (configuration) and + * PEDESTRIAN entity positioned closer to Ego than the range parameter (in configuration) - the goal + * is to test the standard detection functionality when an entity is detected because it is close to + * Ego + */ +TEST_F(DetectionSensorTest, update_pedestrianSubtype) +{ + createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, a + * substantial positive delay and filter_by_range = true (in configuration) no probability of lost + * (configuration) and some entity positioned closer to Ego than the range parameter (in + * configuration) - the goal is to test the simulated detection delay correctness + */ +TEST_F(DetectionSensorTest, update_detectionDelay) +{ + config_.set_object_recognition_delay(0.5); + detection_sensor_ = std::make_unique>( + 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); + + createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + + // Initial update: before the delay period, should not detect the object + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + // Check that no message has been received yet + EXPECT_EQ(received_msg_, nullptr); + + // Advance the simulation time to after the delay period + current_simulation_time_ += 0.6; // 0.6 seconds, greater than the 0.5-second delay + current_ros_time_ = rclcpp::Time(current_simulation_time_, RCL_ROS_TIME); + + // Update again: after the delay period, should detect the object + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = true (in configuration) 100% probability of lost (configuration) and some + * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test + * the simulated malfunction when the message is not being delivered + */ +TEST_F(DetectionSensorTest, update_looseAllData) +{ + config_.set_probability_of_lost(1.0); // 100% probability of lost + + detection_sensor_ = std::make_unique>( + 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); + + createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + EXPECT_EQ(received_msg_->objects.size(), 0); +} + +/** + * @note Test basic functionality. Test update process correctness with no position noise, no delay + * and filter_by_range = false (in configuration) no probability of lost (configuration) and some + * entity in lidar_detected_entity vector - the goal is to test detection based on lidar + * functionality + */ +TEST_F(DetectionSensorTest, update_lidarBasedDetection) +{ + config_.set_detect_all_objects_in_range(false); + + detection_sensor_ = std::make_unique>( + 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); + + createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + + detection_sensor_->update( + current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); + + // Spin the node to process callbacks + rclcpp::spin_some(node_); + + ASSERT_TRUE(received_msg_ != nullptr); + ASSERT_EQ(received_msg_->objects.size(), 1); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN); + EXPECT_POSITION_NEAR( + received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, + std::numeric_limits::epsilon()); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp new file mode 100644 index 00000000000..6c85a2201af --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp @@ -0,0 +1,178 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// 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. + +#ifndef SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace simple_sensor_simulator; + +using DetectedObjectsMsg = autoware_auto_perception_msgs::msg::DetectedObjects; +using TrackedObjectsMsg = autoware_auto_perception_msgs::msg::TrackedObjects; +using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; +using EntityType = traffic_simulator_msgs::msg::EntityType; +using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; +using EntityStatus = traffic_simulator_msgs::EntityStatus; + +class DetectionSensorTest : public ::testing::Test +{ +protected: + DetectionSensorTest() + { + configureDetectionSensor(); + + entity_pose_ = createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); + entity_dimensions_ = createDimensions(4.5, 2.0, 1.5); + + createEgo(); + } + + void SetUp() override + { + rclcpp::init(0, nullptr); + node_ = std::make_shared("detection_sensor_test_node"); + + createRosInterface(); + + detection_sensor_ = std::make_unique>( + 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); + } + + void TearDown() override { rclcpp::shutdown(); } + + rclcpp::Node::SharedPtr node_; + rclcpp::Publisher::SharedPtr detected_objects_publisher_; + rclcpp::Publisher::SharedPtr ground_truth_objects_publisher_; + rclcpp::Subscription::SharedPtr detected_objects_subscriber_; + + EntityStatus ego_status_; + std::vector status_; + std::vector lidar_detected_entities_; + + std::unique_ptr detection_sensor_; + simulation_api_schema::DetectionSensorConfiguration config_; + DetectedObjectsMsg::SharedPtr received_msg_; + + double current_simulation_time_{1.0}; + rclcpp::Time current_ros_time_{1}; + geometry_msgs::msg::Pose entity_pose_; + geometry_msgs::msg::Vector3 entity_dimensions_; + + auto createEgo() -> void + { + initializeEntity( + ego_status_, "ego", EntityType::EGO, EntitySubtype::CAR, + createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), entity_dimensions_); + status_.push_back(ego_status_); + } + + auto createEntity(const std::string & name, const uint8_t type, const uint8_t subtype) -> void + { + EntityStatus status; + initializeEntity(status, name, type, subtype, entity_pose_, entity_dimensions_); + status_.push_back(status); + lidar_detected_entities_.push_back(name); + } + +private: + auto configureDetectionSensor() -> void + { + config_.set_entity("ego"); + config_.set_architecture_type("awf/universe"); + config_.set_update_duration(0.1); + config_.set_object_recognition_delay(0.0); + config_.set_object_recognition_ground_truth_delay(0.0); + config_.set_pos_noise_stddev(0.0); + config_.set_probability_of_lost(0.0); + config_.set_detect_all_objects_in_range(true); + config_.set_range(100.0); + config_.set_random_seed(12345); + } + + auto createRosInterface() -> void + { + detected_objects_publisher_ = + node_->create_publisher("detected_objects_output", 10); + ground_truth_objects_publisher_ = + node_->create_publisher("tracked_objects_output", 10); + + detected_objects_subscriber_ = node_->create_subscription( + "detected_objects_output", 10, + [this](const DetectedObjectsMsg::SharedPtr msg) { received_msg_ = msg; }); + } + + auto initializeEntity( + EntityStatus & status, const std::string & name, const uint8_t type, const uint8_t subtype, + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions) -> void + { + status.set_name(name); + + EntityType entity_type; + entity_type.type = static_cast(type); + EntitySubtype entity_subtype; + entity_subtype.value = static_cast(subtype); + + simulation_interface::toProto(entity_type, *status.mutable_type()); + simulation_interface::toProto(entity_subtype, *status.mutable_subtype()); + + auto new_pose = status.mutable_pose(); + auto new_position = new_pose->mutable_position(); + new_position->set_x(pose.position.x); + new_position->set_y(pose.position.y); + new_position->set_z(pose.position.z); + + auto new_orientation = new_pose->mutable_orientation(); + new_orientation->set_x(pose.orientation.x); + new_orientation->set_y(pose.orientation.y); + new_orientation->set_z(pose.orientation.z); + new_orientation->set_w(pose.orientation.w); + + auto new_bounding_box = status.mutable_bounding_box(); + auto new_dimensions = new_bounding_box->mutable_dimensions(); + new_dimensions->set_x(dimensions.x); + new_dimensions->set_y(dimensions.y); + new_dimensions->set_z(dimensions.z); + } + + auto createPose(double px, double py, double pz, double ox, double oy, double oz, double ow) + -> geometry_msgs::msg::Pose + { + return geometry_msgs::build() + .position(geometry_msgs::build().x(px).y(py).z(pz)) + .orientation(geometry_msgs::build().x(ox).y(oy).z(oz).w(ow)); + } + + auto createDimensions(double x, double y, double z) -> geometry_msgs::msg::Vector3 + { + return geometry_msgs::build().x(x).y(y).z(z); + } +}; + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_ \ No newline at end of file From 0655cf4206ecadfdcf9fff6ed0c60fc1c4568224 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Thu, 20 Jun 2024 13:02:48 +0200 Subject: [PATCH 2/6] test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Added new line at the end of test_detection_sensor.hpp --- simulation/simple_sensor_simulator/test/CMakeLists.txt | 1 - .../detection_sensor/test_detection_sensor.hpp | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index 4dacf372e2b..e195c8ccb51 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -5,4 +5,3 @@ add_subdirectory(src/sensor_simulation/lidar) add_subdirectory(src/sensor_simulation/primitives) add_subdirectory(src/sensor_simulation/occupancy_grid) add_subdirectory(src/sensor_simulation/detection_sensor) - diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp index 6c85a2201af..1db0a807b7c 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp @@ -175,4 +175,4 @@ class DetectionSensorTest : public ::testing::Test } }; -#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_ \ No newline at end of file +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_ From 8f47d212db69010208486115489781b7d10aebd5 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Fri, 21 Jun 2024 16:37:55 +0200 Subject: [PATCH 3/6] Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Refactored DetectionSensor tests - Refactored LidarSensor tests --- .../detection_sensor/CMakeLists.txt | 3 - .../test_detection_sensor.cpp | 68 +++++----- .../test_detection_sensor.hpp | 117 ++++-------------- .../lidar/test_lidar_sensor.cpp | 4 +- .../lidar/test_lidar_sensor.hpp | 51 ++------ .../primitives/CMakeLists.txt | 2 +- .../test/src/utils/helper_functions.hpp | 81 ++++++++++++ 7 files changed, 146 insertions(+), 180 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt index 64d07ddb504..9af7c058821 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/CMakeLists.txt @@ -1,5 +1,2 @@ -ament_add_gtest(test_detection_sensor_base test_detection_sensor_base.cpp) -target_link_libraries(test_detection_sensor_base simple_sensor_simulator_component ${Protobuf_LIBRARIES}) - ament_add_gtest(test_detection_sensor test_detection_sensor.cpp) target_link_libraries(test_detection_sensor simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp index 22383a5f379..5a1b74efb9e 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp @@ -16,7 +16,7 @@ #include -#include "../expect_eq_macros.hpp" +#include "../../utils/expect_eq_macros.hpp" /** * @note Test basic functionality. Test update process correctness with no position noise, no delay @@ -26,7 +26,7 @@ */ TEST_F(DetectionSensorTest, update_unknownSubtype) { - createEntity("unknown", EntityType::VEHICLE, EntitySubtype::UNKNOWN); + initializeEntityStatus("unknown", EntityType::VEHICLE, EntitySubtype::UNKNOWN); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -34,8 +34,8 @@ TEST_F(DetectionSensorTest, update_unknownSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::UNKNOWN); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -50,7 +50,7 @@ TEST_F(DetectionSensorTest, update_unknownSubtype) */ TEST_F(DetectionSensorTest, update_carSubtype) { - createEntity("car", EntityType::VEHICLE, EntitySubtype::CAR); + initializeEntityStatus("car", EntityType::VEHICLE, EntitySubtype::CAR); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -58,8 +58,8 @@ TEST_F(DetectionSensorTest, update_carSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::CAR); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -74,7 +74,7 @@ TEST_F(DetectionSensorTest, update_carSubtype) */ TEST_F(DetectionSensorTest, update_truckSubtype) { - createEntity("truck", EntityType::VEHICLE, EntitySubtype::TRUCK); + initializeEntityStatus("truck", EntityType::VEHICLE, EntitySubtype::TRUCK); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -82,8 +82,8 @@ TEST_F(DetectionSensorTest, update_truckSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::TRUCK); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -98,7 +98,7 @@ TEST_F(DetectionSensorTest, update_truckSubtype) */ TEST_F(DetectionSensorTest, update_busSubtype) { - createEntity("bus", EntityType::VEHICLE, EntitySubtype::BUS); + initializeEntityStatus("bus", EntityType::VEHICLE, EntitySubtype::BUS); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -106,8 +106,8 @@ TEST_F(DetectionSensorTest, update_busSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::BUS); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -122,7 +122,7 @@ TEST_F(DetectionSensorTest, update_busSubtype) */ TEST_F(DetectionSensorTest, update_trailerSubtype) { - createEntity("trailer", EntityType::VEHICLE, EntitySubtype::TRAILER); + initializeEntityStatus("trailer", EntityType::VEHICLE, EntitySubtype::TRAILER); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -130,8 +130,8 @@ TEST_F(DetectionSensorTest, update_trailerSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::TRAILER); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -147,7 +147,7 @@ TEST_F(DetectionSensorTest, update_trailerSubtype) */ TEST_F(DetectionSensorTest, update_motorcycleSubtype) { - createEntity("motorcycle", EntityType::VEHICLE, EntitySubtype::MOTORCYCLE); + initializeEntityStatus("motorcycle", EntityType::VEHICLE, EntitySubtype::MOTORCYCLE); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -155,8 +155,8 @@ TEST_F(DetectionSensorTest, update_motorcycleSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::MOTORCYCLE); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -171,7 +171,7 @@ TEST_F(DetectionSensorTest, update_motorcycleSubtype) */ TEST_F(DetectionSensorTest, update_bicycleSubtype) { - createEntity("bicycle", EntityType::VEHICLE, EntitySubtype::BICYCLE); + initializeEntityStatus("bicycle", EntityType::VEHICLE, EntitySubtype::BICYCLE); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -179,8 +179,8 @@ TEST_F(DetectionSensorTest, update_bicycleSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::BICYCLE); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -196,7 +196,7 @@ TEST_F(DetectionSensorTest, update_bicycleSubtype) */ TEST_F(DetectionSensorTest, update_pedestrianSubtype) { - createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -204,8 +204,8 @@ TEST_F(DetectionSensorTest, update_pedestrianSubtype) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -224,7 +224,7 @@ TEST_F(DetectionSensorTest, update_detectionDelay) detection_sensor_ = std::make_unique>( 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); - createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); // Initial update: before the delay period, should not detect the object detection_sensor_->update( @@ -247,8 +247,8 @@ TEST_F(DetectionSensorTest, update_detectionDelay) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, @@ -268,7 +268,7 @@ TEST_F(DetectionSensorTest, update_looseAllData) detection_sensor_ = std::make_unique>( 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); - createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -276,8 +276,8 @@ TEST_F(DetectionSensorTest, update_looseAllData) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - EXPECT_EQ(received_msg_->objects.size(), 0); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(0)); } /** @@ -293,7 +293,7 @@ TEST_F(DetectionSensorTest, update_lidarBasedDetection) detection_sensor_ = std::make_unique>( 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); - createEntity("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); + initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -301,8 +301,8 @@ TEST_F(DetectionSensorTest, update_lidarBasedDetection) // Spin the node to process callbacks rclcpp::spin_some(node_); - ASSERT_TRUE(received_msg_ != nullptr); - ASSERT_EQ(received_msg_->objects.size(), 1); + ASSERT_NE(received_msg_, nullptr); + ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp index 1db0a807b7c..1ec6ee79348 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp @@ -19,65 +19,48 @@ #include #include -#include -#include -#include -#include #include #include -#include #include -#include -#include -#include #include +#include "../../utils/helper_functions.hpp" + using namespace simple_sensor_simulator; using DetectedObjectsMsg = autoware_auto_perception_msgs::msg::DetectedObjects; using TrackedObjectsMsg = autoware_auto_perception_msgs::msg::TrackedObjects; using ObjectClassification = autoware_auto_perception_msgs::msg::ObjectClassification; -using EntityType = traffic_simulator_msgs::msg::EntityType; -using EntitySubtype = traffic_simulator_msgs::msg::EntitySubtype; -using EntityStatus = traffic_simulator_msgs::EntityStatus; class DetectionSensorTest : public ::testing::Test { protected: DetectionSensorTest() - { - configureDetectionSensor(); - - entity_pose_ = createPose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0); - entity_dimensions_ = createDimensions(4.5, 2.0, 1.5); - - createEgo(); - } - - void SetUp() override + : config_(utils::constructDetectionSensorConfiguration("ego", "awf/universe", 0.1, 100, true)), + entity_pose_(utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0)), + entity_dimensions_(utils::makeDimensions(4.5, 2.0, 1.5)) { rclcpp::init(0, nullptr); node_ = std::make_shared("detection_sensor_test_node"); - createRosInterface(); + makeRosInterface(); + makeEgo(); detection_sensor_ = std::make_unique>( 0.0, config_, detected_objects_publisher_, ground_truth_objects_publisher_); } - - void TearDown() override { rclcpp::shutdown(); } + ~DetectionSensorTest() { rclcpp::shutdown(); } rclcpp::Node::SharedPtr node_; rclcpp::Publisher::SharedPtr detected_objects_publisher_; rclcpp::Publisher::SharedPtr ground_truth_objects_publisher_; rclcpp::Subscription::SharedPtr detected_objects_subscriber_; - EntityStatus ego_status_; std::vector status_; std::vector lidar_detected_entities_; - std::unique_ptr detection_sensor_; simulation_api_schema::DetectionSensorConfiguration config_; + std::unique_ptr detection_sensor_; DetectedObjectsMsg::SharedPtr received_msg_; double current_simulation_time_{1.0}; @@ -85,38 +68,26 @@ class DetectionSensorTest : public ::testing::Test geometry_msgs::msg::Pose entity_pose_; geometry_msgs::msg::Vector3 entity_dimensions_; - auto createEgo() -> void + auto initializeEntityStatus( + const std::string & name, const EntityType::Enum type, const EntitySubtype::Enum subtype) + -> void { - initializeEntity( - ego_status_, "ego", EntityType::EGO, EntitySubtype::CAR, - createPose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), entity_dimensions_); - status_.push_back(ego_status_); - } - - auto createEntity(const std::string & name, const uint8_t type, const uint8_t subtype) -> void - { - EntityStatus status; - initializeEntity(status, name, type, subtype, entity_pose_, entity_dimensions_); - status_.push_back(status); + const auto entity_status = + utils::makeEntity(name, type, subtype, entity_pose_, entity_dimensions_); + status_.push_back(entity_status); lidar_detected_entities_.push_back(name); } private: - auto configureDetectionSensor() -> void + auto makeEgo() -> void { - config_.set_entity("ego"); - config_.set_architecture_type("awf/universe"); - config_.set_update_duration(0.1); - config_.set_object_recognition_delay(0.0); - config_.set_object_recognition_ground_truth_delay(0.0); - config_.set_pos_noise_stddev(0.0); - config_.set_probability_of_lost(0.0); - config_.set_detect_all_objects_in_range(true); - config_.set_range(100.0); - config_.set_random_seed(12345); + const auto ego_status = utils::makeEntity( + "ego", EntityType::EGO, EntitySubtype::CAR, + utils::makePose(0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), entity_dimensions_); + status_.push_back(ego_status); } - auto createRosInterface() -> void + auto makeRosInterface() -> void { detected_objects_publisher_ = node_->create_publisher("detected_objects_output", 10); @@ -127,52 +98,6 @@ class DetectionSensorTest : public ::testing::Test "detected_objects_output", 10, [this](const DetectedObjectsMsg::SharedPtr msg) { received_msg_ = msg; }); } - - auto initializeEntity( - EntityStatus & status, const std::string & name, const uint8_t type, const uint8_t subtype, - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions) -> void - { - status.set_name(name); - - EntityType entity_type; - entity_type.type = static_cast(type); - EntitySubtype entity_subtype; - entity_subtype.value = static_cast(subtype); - - simulation_interface::toProto(entity_type, *status.mutable_type()); - simulation_interface::toProto(entity_subtype, *status.mutable_subtype()); - - auto new_pose = status.mutable_pose(); - auto new_position = new_pose->mutable_position(); - new_position->set_x(pose.position.x); - new_position->set_y(pose.position.y); - new_position->set_z(pose.position.z); - - auto new_orientation = new_pose->mutable_orientation(); - new_orientation->set_x(pose.orientation.x); - new_orientation->set_y(pose.orientation.y); - new_orientation->set_z(pose.orientation.z); - new_orientation->set_w(pose.orientation.w); - - auto new_bounding_box = status.mutable_bounding_box(); - auto new_dimensions = new_bounding_box->mutable_dimensions(); - new_dimensions->set_x(dimensions.x); - new_dimensions->set_y(dimensions.y); - new_dimensions->set_z(dimensions.z); - } - - auto createPose(double px, double py, double pz, double ox, double oy, double oz, double ow) - -> geometry_msgs::msg::Pose - { - return geometry_msgs::build() - .position(geometry_msgs::build().x(px).y(py).z(pz)) - .orientation(geometry_msgs::build().x(ox).y(oy).z(oz).w(ow)); - } - - auto createDimensions(double x, double y, double z) -> geometry_msgs::msg::Vector3 - { - return geometry_msgs::build().x(x).y(y).z(z); - } }; #endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_ diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp index c7aba68ff9f..20dd82e78d4 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.cpp @@ -67,7 +67,7 @@ TEST_F(LidarSensorTest, update_goBackInTime) */ TEST_F(LidarSensorTest, getDetectedObjects) { - const std::set expected_objects = {other1_status_.name(), other2_status_.name()}; + const std::set expected_objects = {status_[1].name(), status_[2].name()}; lidar_->update(current_simulation_time_, status_, current_ros_time_); @@ -77,7 +77,7 @@ TEST_F(LidarSensorTest, getDetectedObjects) const auto & detected_objects = lidar_->getDetectedObjects(); // LidarSensor returns duplicates. To avoid them, a set is used. - std::set unique_objects(detected_objects.begin(), detected_objects.end()); + const std::set unique_objects(detected_objects.begin(), detected_objects.end()); ASSERT_FALSE(unique_objects.empty()); EXPECT_EQ(unique_objects, expected_objects); diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp index 7aef40e53b4..54bf61a03b0 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/lidar/test_lidar_sensor.hpp @@ -22,16 +22,12 @@ #include #include #include -#include #include #include "../../utils/helper_functions.hpp" using namespace simple_sensor_simulator; -using EntityType = traffic_simulator_msgs::EntityType; -using EntityStatus = traffic_simulator_msgs::EntityStatus; - class LidarSensorTest : public ::testing::Test { protected: @@ -39,7 +35,7 @@ class LidarSensorTest : public ::testing::Test { rclcpp::init(0, nullptr); node_ = std::make_shared("lidar_sensor_test_node"); - createRosInterface(); + makeRosInterface(); initializeEntityStatuses(); lidar_ = std::make_unique>(0.0, config_, publisher_); @@ -51,9 +47,6 @@ class LidarSensorTest : public ::testing::Test rclcpp::Publisher::SharedPtr publisher_; rclcpp::Subscription::SharedPtr subscription_; - EntityStatus ego_status_; - EntityStatus other1_status_; - EntityStatus other2_status_; std::vector status_; std::unique_ptr lidar_; @@ -66,51 +59,21 @@ class LidarSensorTest : public ::testing::Test private: auto initializeEntityStatuses() -> void { - auto dimensions = utils::makeDimensions(4.5, 2.0, 1.5); + const auto dimensions = utils::makeDimensions(4.5, 2.0, 1.5); - ego_status_ = makeEntity( + const auto ego_status = utils::makeEntity( "ego", EntityType::EGO, utils::makePose(5.0, 5.0, 0.0, 0.0, 0.0, 0.0, 1.0), dimensions); - other1_status_ = makeEntity( + const auto other1_status = utils::makeEntity( "other1", EntityType::VEHICLE, utils::makePose(-3.0, -3.0, 0.0, 0.0, 0.0, 0.0, 1.0), dimensions); - other2_status_ = makeEntity( + const auto other2_status = utils::makeEntity( "other2", EntityType::VEHICLE, utils::makePose(5.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0), dimensions); - status_ = {ego_status_, other1_status_, other2_status_}; - } - - auto makeEntity( - const std::string & name, const EntityType::Enum type, const geometry_msgs::msg::Pose & pose, - const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus - { - EntityStatus status; - - status.set_name(name); - status.mutable_type()->set_type(type); - - auto new_pose = status.mutable_pose(); - auto new_position = new_pose->mutable_position(); - new_position->set_x(pose.position.x); - new_position->set_y(pose.position.y); - new_position->set_z(pose.position.z); - - auto new_orientation = new_pose->mutable_orientation(); - new_orientation->set_x(pose.orientation.x); - new_orientation->set_y(pose.orientation.y); - new_orientation->set_z(pose.orientation.z); - new_orientation->set_w(pose.orientation.w); - - auto new_bounding_box = status.mutable_bounding_box(); - auto new_dimensions = new_bounding_box->mutable_dimensions(); - new_dimensions->set_x(dimensions.x); - new_dimensions->set_y(dimensions.y); - new_dimensions->set_z(dimensions.z); - - return status; + status_ = {ego_status, other1_status, other2_status}; } - auto createRosInterface() -> void + auto makeRosInterface() -> void { publisher_ = node_->create_publisher("lidar_output", 10); subscription_ = node_->create_subscription( diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt index 8a5af2ad924..b5ae088e7a7 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/primitives/CMakeLists.txt @@ -5,4 +5,4 @@ ament_add_gtest(test_vertex test_vertex.cpp) target_link_libraries(test_vertex simple_sensor_simulator_component) ament_add_gtest(test_primitive test_primitive.cpp) -target_link_libraries(test_primitive simple_sensor_simulator_component) +target_link_libraries(test_primitive simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp index 4f8d47cc1f5..881249afd5a 100644 --- a/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp +++ b/simulation/simple_sensor_simulator/test/src/utils/helper_functions.hpp @@ -17,10 +17,19 @@ #include +#include #include #include #include #include +#include +#include +#include +#include + +using EntitySubtype = traffic_simulator_msgs::EntitySubtype; +using EntityType = traffic_simulator_msgs::EntityType; +using EntityStatus = traffic_simulator_msgs::EntityStatus; namespace utils { @@ -48,6 +57,29 @@ inline auto makeDimensions(const double x, const double y, const double z) return geometry_msgs::build().x(x).y(y).z(z); } +auto constructDetectionSensorConfiguration( + const std::string & entity, const std::string & architecture_type, const double update_duration, + const double range = 300.0, const bool detect_all_objects_in_range = false, + const double pos_noise_stddev = 0.0, const double probability_of_lost = 0.0, + const double object_recognition_delay = 0.0, + const double object_recognition_ground_truth_delay = 0.0) + -> const simulation_api_schema::DetectionSensorConfiguration +{ + simulation_api_schema::DetectionSensorConfiguration configuration; + configuration.set_entity(entity); + configuration.set_architecture_type(architecture_type); + configuration.set_update_duration(update_duration); + configuration.set_object_recognition_delay(object_recognition_delay); + configuration.set_object_recognition_ground_truth_delay(object_recognition_ground_truth_delay); + configuration.set_pos_noise_stddev(pos_noise_stddev); + configuration.set_probability_of_lost(probability_of_lost); + configuration.set_detect_all_objects_in_range(detect_all_objects_in_range); + configuration.set_range(range); + configuration.set_random_seed(1234); + + return configuration; +} + inline auto constructLidarConfiguration( const std::string & entity, const std::string & architecture_type, const double lidar_sensor_delay, const double horizontal_resolution) @@ -79,6 +111,55 @@ inline auto constructLidarConfiguration( return configuration; } +inline auto createEntityStatus( + const std::string & name, const EntityType::Enum type, + const std::optional & subtype, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus +{ + EntityStatus status; + status.set_name(name); + status.mutable_type()->set_type(type); + + if (subtype) { + status.mutable_subtype()->set_value(*subtype); + } + + auto new_pose = status.mutable_pose(); + auto new_position = new_pose->mutable_position(); + new_position->set_x(pose.position.x); + new_position->set_y(pose.position.y); + new_position->set_z(pose.position.z); + + auto new_orientation = new_pose->mutable_orientation(); + new_orientation->set_x(pose.orientation.x); + new_orientation->set_y(pose.orientation.y); + new_orientation->set_z(pose.orientation.z); + new_orientation->set_w(pose.orientation.w); + + auto new_bounding_box = status.mutable_bounding_box(); + auto new_dimensions = new_bounding_box->mutable_dimensions(); + new_dimensions->set_x(dimensions.x); + new_dimensions->set_y(dimensions.y); + new_dimensions->set_z(dimensions.z); + + return status; +} + +inline auto makeEntity( + const std::string & name, const EntityType::Enum type, const EntitySubtype::Enum subtype, + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Vector3 & dimensions) + -> EntityStatus +{ + return createEntityStatus(name, type, subtype, pose, dimensions); +} + +inline auto makeEntity( + const std::string & name, const EntityType::Enum type, const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Vector3 & dimensions) -> EntityStatus +{ + return createEntityStatus(name, type, std::nullopt, pose, dimensions); +} + } // namespace utils #endif // SIMPLE_SENSOR_SIMULATOR__TEST__UTILS__HELPER_FUNCTIONS_HPP_ From d2a63f2cd26c1a1b388db454d60e5a2bc08d8f2b Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Wed, 26 Jun 2024 10:06:47 +0200 Subject: [PATCH 4/6] Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Refactored DetectionSensor tests --- .../test_detection_sensor.cpp | 198 +++--------------- .../test_detection_sensor.hpp | 13 ++ 2 files changed, 38 insertions(+), 173 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp index 5a1b74efb9e..0aefad21dc2 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.cpp @@ -20,134 +20,15 @@ /** * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and UNKNOWN - * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test - * the standard detection functionality when an entity is detected because it is close to Ego. - */ -TEST_F(DetectionSensorTest, update_unknownSubtype) -{ - initializeEntityStatus("unknown", EntityType::VEHICLE, EntitySubtype::UNKNOWN); - - detection_sensor_->update( - current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); - - // Spin the node to process callbacks - rclcpp::spin_some(node_); - - ASSERT_NE(received_msg_, nullptr); - ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::UNKNOWN); - EXPECT_POSITION_NEAR( - received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, - std::numeric_limits::epsilon()); -} - -/** - * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and CAR - * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test - * the standard detection functionality when an entity is detected because it is close to Ego. - */ -TEST_F(DetectionSensorTest, update_carSubtype) -{ - initializeEntityStatus("car", EntityType::VEHICLE, EntitySubtype::CAR); - - detection_sensor_->update( - current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); - - // Spin the node to process callbacks - rclcpp::spin_some(node_); - - ASSERT_NE(received_msg_, nullptr); - ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::CAR); - EXPECT_POSITION_NEAR( - received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, - std::numeric_limits::epsilon()); -} - -/** - * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and TRUCK - * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test - * the standard detection functionality when an entity is detected because it is close to Ego. - */ -TEST_F(DetectionSensorTest, update_truckSubtype) -{ - initializeEntityStatus("truck", EntityType::VEHICLE, EntitySubtype::TRUCK); - - detection_sensor_->update( - current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); - - // Spin the node to process callbacks - rclcpp::spin_some(node_); - - ASSERT_NE(received_msg_, nullptr); - ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::TRUCK); - EXPECT_POSITION_NEAR( - received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, - std::numeric_limits::epsilon()); -} - -/** - * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and BUS - * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test - * the standard detection functionality when an entity is detected because it is close to Ego. - */ -TEST_F(DetectionSensorTest, update_busSubtype) -{ - initializeEntityStatus("bus", EntityType::VEHICLE, EntitySubtype::BUS); - - detection_sensor_->update( - current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); - - // Spin the node to process callbacks - rclcpp::spin_some(node_); - - ASSERT_NE(received_msg_, nullptr); - ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::BUS); - EXPECT_POSITION_NEAR( - received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, - std::numeric_limits::epsilon()); -} - -/** - * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and TRAILER - * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test - * the standard detection functionality when an entity is detected because it is close to Ego. - */ -TEST_F(DetectionSensorTest, update_trailerSubtype) -{ - initializeEntityStatus("trailer", EntityType::VEHICLE, EntitySubtype::TRAILER); - - detection_sensor_->update( - current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); - - // Spin the node to process callbacks - rclcpp::spin_some(node_); - - ASSERT_NE(received_msg_, nullptr); - ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::TRAILER); - EXPECT_POSITION_NEAR( - received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, - std::numeric_limits::epsilon()); -} - -/** - * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and - * MOTORCYCLE entity positioned closer to Ego than the range parameter (in configuration) - the goal + * and filter_by_range = true (in configuration), no probability of lost (configuration) and a + * specific entity positioned closer to Ego than the range parameter (in configuration) - the goal * is to test the standard detection functionality when an entity is detected because it is close to * Ego. */ -TEST_F(DetectionSensorTest, update_motorcycleSubtype) +TEST_P(DetectionSensorTestParameterized, update) { - initializeEntityStatus("motorcycle", EntityType::VEHICLE, EntitySubtype::MOTORCYCLE); + const auto param = GetParam(); + initializeEntityStatus(param.entity_name_, param.entity_type_, param.entity_subtype_); detection_sensor_->update( current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); @@ -157,60 +38,31 @@ TEST_F(DetectionSensorTest, update_motorcycleSubtype) ASSERT_NE(received_msg_, nullptr); ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::MOTORCYCLE); + EXPECT_EQ(received_msg_->objects[0].classification[0].label, param.expected_label_); EXPECT_POSITION_NEAR( received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, std::numeric_limits::epsilon()); } -/** - * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and BICYCLE - * entity positioned closer to Ego than the range parameter (in configuration) - the goal is to test - * the standard detection functionality when an entity is detected because it is close to Ego - */ -TEST_F(DetectionSensorTest, update_bicycleSubtype) -{ - initializeEntityStatus("bicycle", EntityType::VEHICLE, EntitySubtype::BICYCLE); - - detection_sensor_->update( - current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); - - // Spin the node to process callbacks - rclcpp::spin_some(node_); - - ASSERT_NE(received_msg_, nullptr); - ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::BICYCLE); - EXPECT_POSITION_NEAR( - received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, - std::numeric_limits::epsilon()); -} - -/** - * @note Test basic functionality. Test update process correctness with no position noise, no delay - * and filter_by_range = true (in configuration), no probability of lost (configuration) and - * PEDESTRIAN entity positioned closer to Ego than the range parameter (in configuration) - the goal - * is to test the standard detection functionality when an entity is detected because it is close to - * Ego - */ -TEST_F(DetectionSensorTest, update_pedestrianSubtype) -{ - initializeEntityStatus("pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN); - - detection_sensor_->update( - current_simulation_time_, status_, current_ros_time_, lidar_detected_entities_); - - // Spin the node to process callbacks - rclcpp::spin_some(node_); - - ASSERT_NE(received_msg_, nullptr); - ASSERT_EQ(received_msg_->objects.size(), static_cast(1)); - EXPECT_EQ(received_msg_->objects[0].classification[0].label, ObjectClassification::PEDESTRIAN); - EXPECT_POSITION_NEAR( - received_msg_->objects[0].kinematics.pose_with_covariance.pose.position, entity_pose_.position, - std::numeric_limits::epsilon()); -} +INSTANTIATE_TEST_SUITE_P( + DetectionSensorTests, DetectionSensorTestParameterized, + ::testing::Values( + DetectionTestParam{ + "unknown", EntityType::VEHICLE, EntitySubtype::UNKNOWN, ObjectClassification::UNKNOWN}, + DetectionTestParam{"car", EntityType::VEHICLE, EntitySubtype::CAR, ObjectClassification::CAR}, + DetectionTestParam{ + "truck", EntityType::VEHICLE, EntitySubtype::TRUCK, ObjectClassification::TRUCK}, + DetectionTestParam{"bus", EntityType::VEHICLE, EntitySubtype::BUS, ObjectClassification::BUS}, + DetectionTestParam{ + "trailer", EntityType::VEHICLE, EntitySubtype::TRAILER, ObjectClassification::TRAILER}, + DetectionTestParam{ + "motorcycle", EntityType::VEHICLE, EntitySubtype::MOTORCYCLE, + ObjectClassification::MOTORCYCLE}, + DetectionTestParam{ + "bicycle", EntityType::VEHICLE, EntitySubtype::BICYCLE, ObjectClassification::BICYCLE}, + DetectionTestParam{ + "pedestrian", EntityType::PEDESTRIAN, EntitySubtype::PEDESTRIAN, + ObjectClassification::PEDESTRIAN})); /** * @note Test basic functionality. Test update process correctness with no position noise, a diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp index 1ec6ee79348..b16ad8f7087 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/detection_sensor/test_detection_sensor.hpp @@ -100,4 +100,17 @@ class DetectionSensorTest : public ::testing::Test } }; +struct DetectionTestParam +{ + std::string entity_name_; + EntityType::Enum entity_type_; + EntitySubtype::Enum entity_subtype_; + int expected_label_; +}; + +class DetectionSensorTestParameterized : public DetectionSensorTest, + public ::testing::WithParamInterface +{ +}; + #endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_DETECTION_SENSOR_HPP_ From e839dd8b6f90c037ba9f950141cade8c60fa7e3f Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Wed, 26 Jun 2024 13:02:18 +0200 Subject: [PATCH 5/6] Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Added unit tests to TrafficLightsDetector --- .../test/CMakeLists.txt | 5 + .../test/map/lanelet2_map.osm | 6105 +++++++++++++++++ .../traffic_lights/CMakeLists.txt | 2 + .../test_traffic_light_detector.cpp | 96 + .../test_traffic_light_detector.hpp | 117 + 5 files changed, 6325 insertions(+) create mode 100644 simulation/simple_sensor_simulator/test/map/lanelet2_map.osm create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/CMakeLists.txt create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.cpp create mode 100644 simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp diff --git a/simulation/simple_sensor_simulator/test/CMakeLists.txt b/simulation/simple_sensor_simulator/test/CMakeLists.txt index e195c8ccb51..14856674d1c 100644 --- a/simulation/simple_sensor_simulator/test/CMakeLists.txt +++ b/simulation/simple_sensor_simulator/test/CMakeLists.txt @@ -1,7 +1,12 @@ find_package(Protobuf REQUIRED) include_directories(${Protobuf_INCLUDE_DIRS}) +install( + DIRECTORY map + DESTINATION share/${PROJECT_NAME}) + add_subdirectory(src/sensor_simulation/lidar) add_subdirectory(src/sensor_simulation/primitives) add_subdirectory(src/sensor_simulation/occupancy_grid) add_subdirectory(src/sensor_simulation/detection_sensor) +add_subdirectory(src/sensor_simulation/traffic_lights) diff --git a/simulation/simple_sensor_simulator/test/map/lanelet2_map.osm b/simulation/simple_sensor_simulator/test/map/lanelet2_map.osm new file mode 100644 index 00000000000..7ecba866372 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/map/lanelet2_map.osm @@ -0,0 +1,6105 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/CMakeLists.txt b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/CMakeLists.txt new file mode 100644 index 00000000000..8624ec40095 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/CMakeLists.txt @@ -0,0 +1,2 @@ +ament_add_gtest(test_traffic_light_detector test_traffic_light_detector.cpp) +target_link_libraries(test_traffic_light_detector simple_sensor_simulator_component ${Protobuf_LIBRARIES}) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.cpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.cpp new file mode 100644 index 00000000000..e90cfb45146 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.cpp @@ -0,0 +1,96 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// 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 "test_traffic_light_detector.hpp" + +auto makeUpdateTrafficLightsRequest(const int32_t id, const TrafficLightColor color) + -> simulation_api_schema::UpdateTrafficLightsRequest +{ + simulation_api_schema::UpdateTrafficLightsRequest request; + auto * signal = request.add_states(); + signal->set_id(id); + auto * traffic_light = signal->add_traffic_light_status(); + traffic_light->set_color(color); + return request; +} + +/** + * @note Test basic functionality. Test updating frame correctness with a vector containing some + * traffic light state - the goal is to verify that correct data is published on correct topic. + */ +TEST_F(TrafficLightDetectorTest_AutoPerceptionMsgs, updateFrame_correctState) +{ + traffic_lights_detector_auto_->updateFrame( + stamp_, makeUpdateTrafficLightsRequest( + static_cast(34802), TrafficLightColor::TrafficLight_Color_RED)); + rclcpp::spin_some(node_); + + ASSERT_NE(received_msg_auto_, nullptr); + EXPECT_EQ(received_msg_auto_->header.frame_id, "camera_link"); + EXPECT_EQ(received_msg_auto_->signals.size(), static_cast(1)); + EXPECT_EQ(received_msg_auto_->signals[0].map_primitive_id, 34802); + EXPECT_EQ(received_msg_auto_->signals[0].lights.size(), static_cast(1)); + EXPECT_EQ(received_msg_auto_->signals[0].lights[0].color, red_light_); +} + +/** + * @note Test function behavior when called with an empty vector. + */ +TEST_F(TrafficLightDetectorTest_AutoPerceptionMsgs, updateFrame_emptyVector) +{ + traffic_lights_detector_auto_->updateFrame( + stamp_, simulation_api_schema::UpdateTrafficLightsRequest()); + rclcpp::spin_some(node_); + + ASSERT_NE(received_msg_auto_, nullptr); + EXPECT_EQ(received_msg_auto_->header.frame_id, "camera_link"); + EXPECT_EQ(received_msg_auto_->signals.size(), static_cast(0)); +} + +/** + * @note Test basic functionality. Test updating frame correctness with a vector containing some + * traffic light state - the goal is to verify that correct data is published on correct topic. + */ +TEST_F(TrafficLightDetectorTest_PerceptionMsgs, updateFrame_correctState) +{ + traffic_lights_detector_perception_->updateFrame( + stamp_, makeUpdateTrafficLightsRequest( + static_cast(34802), TrafficLightColor::TrafficLight_Color_RED)); + rclcpp::spin_some(node_); + + ASSERT_NE(received_msg_perception_, nullptr); + EXPECT_EQ(received_msg_perception_->signals.size(), static_cast(1)); + EXPECT_EQ(received_msg_perception_->signals[0].traffic_signal_id, 34806); + EXPECT_EQ(received_msg_perception_->signals[0].elements.size(), static_cast(1)); + EXPECT_EQ(received_msg_perception_->signals[0].elements[0].color, red_light_); +} + +/** + * @note Test function behavior when called with an empty vector. + */ +TEST_F(TrafficLightDetectorTest_PerceptionMsgs, updateFrame_emptyVector) +{ + traffic_lights_detector_perception_->updateFrame( + stamp_, simulation_api_schema::UpdateTrafficLightsRequest()); + rclcpp::spin_some(node_); + + ASSERT_NE(received_msg_perception_, nullptr); + EXPECT_EQ(received_msg_perception_->signals.size(), static_cast(0)); +} + +int main(int argc, char ** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp new file mode 100644 index 00000000000..ddeec501f70 --- /dev/null +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp @@ -0,0 +1,117 @@ +// Copyright 2015 TIER IV, Inc. All rights reserved. +// +// 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. + +#ifndef SIMPLE_SENSOR_SIMULATOR__TEST__TEST_TRAFFIC_LIGHT_DETECTOR_HPP_ +#define SIMPLE_SENSOR_SIMULATOR__TEST__TEST_TRAFFIC_LIGHT_DETECTOR_HPP_ + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using namespace simple_sensor_simulator; + +using AutoPerceptionTrafficSignalArray = autoware_auto_perception_msgs::msg::TrafficSignalArray; +using PerceptionTrafficSignalArray = autoware_perception_msgs::msg::TrafficSignalArray; +using TrafficSignalElement = autoware_perception_msgs::msg::TrafficSignalElement; +using TrafficLightColor = simulation_api_schema::TrafficLight_Color; + +class TrafficLightDetectorTestBase : public ::testing::Test +{ +protected: + TrafficLightDetectorTestBase() + { + rclcpp::init(0, nullptr); + node_ = std::make_shared("traffic_light_detector_test_node"); + + hdmap_utils_ = std::make_shared( + ament_index_cpp::get_package_share_directory("simple_sensor_simulator") + + "/map/lanelet2_map.osm", + geographic_msgs::build() + .latitude(35.61836750154) + .longitude(139.78066608243) + .altitude(0.0)); + } + + ~TrafficLightDetectorTestBase() { rclcpp::shutdown(); } + + rclcpp::Node::SharedPtr node_; + std::shared_ptr hdmap_utils_; + const uint8_t red_light_{TrafficSignalElement::RED}; + const rclcpp::Time stamp_{0}; +}; + +// awf/universe +class TrafficLightDetectorTest_AutoPerceptionMsgs : public TrafficLightDetectorTestBase +{ +protected: + TrafficLightDetectorTest_AutoPerceptionMsgs() + { + publisher_auto_ = + std::make_shared>( + perception_msgs_topic_, node_, hdmap_utils_); + subscription_auto_ = node_->create_subscription( + perception_msgs_topic_, 10, + [this](const AutoPerceptionTrafficSignalArray::SharedPtr msg) { received_msg_auto_ = msg; }); + + traffic_lights_detector_auto_ = + std::make_unique(publisher_auto_); + } + + const std::string perception_msgs_topic_ = "traffic_light_detector_output_perception"; + + std::shared_ptr publisher_auto_; + std::unique_ptr traffic_lights_detector_auto_; + rclcpp::Subscription::SharedPtr subscription_auto_; + AutoPerceptionTrafficSignalArray::SharedPtr received_msg_auto_; +}; + +// awf/universe/20230906 +class TrafficLightDetectorTest_PerceptionMsgs : public TrafficLightDetectorTestBase +{ +protected: + TrafficLightDetectorTest_PerceptionMsgs() + { + publisher_perception_ = + std::make_shared>( + auto_perception_msgs_topic_, node_, hdmap_utils_); + subscription_perception_ = node_->create_subscription( + auto_perception_msgs_topic_, 10, [this](const PerceptionTrafficSignalArray::SharedPtr msg) { + received_msg_perception_ = msg; + }); + + traffic_lights_detector_perception_ = + std::make_unique(publisher_perception_); + } + + const std::string auto_perception_msgs_topic_ = "traffic_light_detector_output_auto"; + + std::shared_ptr publisher_perception_; + std::unique_ptr traffic_lights_detector_perception_; + rclcpp::Subscription::SharedPtr subscription_perception_; + PerceptionTrafficSignalArray::SharedPtr received_msg_perception_; +}; + +#endif // SIMPLE_SENSOR_SIMULATOR__TEST__TEST_TRAFFIC_LIGHT_DETECTOR_HPP_ From ec72c27f2257d492b6df659c544f6266ef17b203 Mon Sep 17 00:00:00 2001 From: SzymonParapura Date: Thu, 27 Jun 2024 09:12:28 +0200 Subject: [PATCH 6/6] Test: [RJD-937] to Implement Unit tests on simple_sensor_simulator - Refactored TrafficLightDetector unit tests --- .../test_traffic_light_detector.hpp | 41 +++++++++---------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp index ddeec501f70..8385f09740e 100644 --- a/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp +++ b/simulation/simple_sensor_simulator/test/src/sensor_simulation/traffic_lights/test_traffic_light_detector.hpp @@ -68,23 +68,22 @@ class TrafficLightDetectorTest_AutoPerceptionMsgs : public TrafficLightDetectorT { protected: TrafficLightDetectorTest_AutoPerceptionMsgs() - { - publisher_auto_ = + : publisher_auto_( std::make_shared>( - perception_msgs_topic_, node_, hdmap_utils_); - subscription_auto_ = node_->create_subscription( + perception_msgs_topic_, node_, hdmap_utils_)), + subscription_auto_(node_->create_subscription( perception_msgs_topic_, 10, - [this](const AutoPerceptionTrafficSignalArray::SharedPtr msg) { received_msg_auto_ = msg; }); - - traffic_lights_detector_auto_ = - std::make_unique(publisher_auto_); + [this](const AutoPerceptionTrafficSignalArray::SharedPtr msg) { received_msg_auto_ = msg; })), + traffic_lights_detector_auto_( + std::make_unique(publisher_auto_)) + { } - const std::string perception_msgs_topic_ = "traffic_light_detector_output_perception"; + const std::string perception_msgs_topic_{"traffic_light_detector_output_perception"}; std::shared_ptr publisher_auto_; - std::unique_ptr traffic_lights_detector_auto_; rclcpp::Subscription::SharedPtr subscription_auto_; + std::unique_ptr traffic_lights_detector_auto_; AutoPerceptionTrafficSignalArray::SharedPtr received_msg_auto_; }; @@ -93,24 +92,24 @@ class TrafficLightDetectorTest_PerceptionMsgs : public TrafficLightDetectorTestB { protected: TrafficLightDetectorTest_PerceptionMsgs() - { - publisher_perception_ = + : publisher_perception_( std::make_shared>( - auto_perception_msgs_topic_, node_, hdmap_utils_); - subscription_perception_ = node_->create_subscription( - auto_perception_msgs_topic_, 10, [this](const PerceptionTrafficSignalArray::SharedPtr msg) { + auto_perception_msgs_topic_, node_, hdmap_utils_)), + subscription_perception_(node_->create_subscription( + auto_perception_msgs_topic_, 10, + [this](const PerceptionTrafficSignalArray::SharedPtr msg) { received_msg_perception_ = msg; - }); - - traffic_lights_detector_perception_ = - std::make_unique(publisher_perception_); + })), + traffic_lights_detector_perception_( + std::make_unique(publisher_perception_)) + { } - const std::string auto_perception_msgs_topic_ = "traffic_light_detector_output_auto"; + const std::string auto_perception_msgs_topic_{"traffic_light_detector_output_auto"}; std::shared_ptr publisher_perception_; - std::unique_ptr traffic_lights_detector_perception_; rclcpp::Subscription::SharedPtr subscription_perception_; + std::unique_ptr traffic_lights_detector_perception_; PerceptionTrafficSignalArray::SharedPtr received_msg_perception_; };