diff --git a/perception/map_based_prediction/CMakeLists.txt b/perception/map_based_prediction/CMakeLists.txt index 9378e09f099cc..786446457143f 100644 --- a/perception/map_based_prediction/CMakeLists.txt +++ b/perception/map_based_prediction/CMakeLists.txt @@ -26,6 +26,22 @@ rclcpp_components_register_node(map_based_prediction_node EXECUTABLE map_based_prediction ) +## Tests +if(BUILD_TESTING) + find_package(ament_cmake_ros REQUIRED) + list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify) + + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() + + file(GLOB_RECURSE test_files test/**/*.cpp) + ament_add_ros_isolated_gtest(test_map_based_prediction ${test_files}) + + target_link_libraries(test_map_based_prediction + map_based_prediction_node + ) +endif() + ament_auto_package( INSTALL_TO_SHARE config diff --git a/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp new file mode 100644 index 0000000000000..3a8906341afb7 --- /dev/null +++ b/perception/map_based_prediction/test/map_based_prediction/test_path_generator.cpp @@ -0,0 +1,218 @@ +// 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 "map_based_prediction/path_generator.hpp" + +#include +#include + +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObject; +using autoware_auto_perception_msgs::msg::PredictedObjectKinematics; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::PredictedPath; +using autoware_auto_perception_msgs::msg::TrackedObject; +using autoware_auto_perception_msgs::msg::TrackedObjectKinematics; +using autoware_auto_perception_msgs::msg::TrackedObjects; + +TrackedObject generate_static_object(const int label) +{ + ObjectClassification classification; + classification.probability = 1.0; + classification.label = label; + + TrackedObjectKinematics kinematics; + kinematics.pose_with_covariance = geometry_msgs::msg::PoseWithCovariance(); // At origin + kinematics.twist_with_covariance = geometry_msgs::msg::TwistWithCovariance(); // Not moving + kinematics.acceleration_with_covariance = + geometry_msgs::msg::AccelWithCovariance(); // Not accelerating + kinematics.orientation_availability = TrackedObjectKinematics::UNAVAILABLE; + + TrackedObject tracked_object; + tracked_object.object_id = unique_identifier_msgs::msg::UUID(); + tracked_object.existence_probability = 1.0; + tracked_object.classification.push_back(classification); + tracked_object.kinematics = kinematics; + + return tracked_object; +} + +TEST(PathGenerator, test_generatePathForNonVehicleObject) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate pedestrian object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForNonVehicleObject(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForLowSpeedVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForLowSpeedVehicle(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOffLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + const PredictedPath predicted_path = path_generator.generatePathForOffLaneVehicle(tracked_object); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForOnLaneVehicle) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate reference path + map_based_prediction::PosePath ref_paths; + geometry_msgs::msg::Pose pose; + pose.position.x = 0.0; + pose.position.y = 0.0; + pose.position.z = 0.0; + ref_paths.push_back(pose); + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForOnLaneVehicle(tracked_object, ref_paths); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathForCrosswalkUser) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::PEDESTRIAN); + + // Generage dummy crosswalk + map_based_prediction::CrosswalkEdgePoints reachable_crosswalk; + reachable_crosswalk.front_center_point << 0.0, 0.0; + reachable_crosswalk.front_right_point << 1.0, 0.0; + reachable_crosswalk.front_left_point << -1.0, 0.0; + reachable_crosswalk.back_center_point << 0.0, 1.0; + reachable_crosswalk.back_right_point << 1.0, 1.0; + reachable_crosswalk.back_left_point << -1.0, 1.0; + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathForCrosswalkUser(tracked_object, reachable_crosswalk); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} + +TEST(PathGenerator, test_generatePathToTargetPoint) +{ + // Generate Path generator + const double prediction_time_horizon = 10.0; + const double lateral_control_time_horizon = 5.0; + const double prediction_sampling_time_interval = 0.5; + const double min_crosswalk_user_velocity = 0.1; + const map_based_prediction::PathGenerator path_generator = map_based_prediction::PathGenerator( + prediction_time_horizon, lateral_control_time_horizon, prediction_sampling_time_interval, + min_crosswalk_user_velocity); + + // Generate dummy object + TrackedObject tracked_object = generate_static_object(ObjectClassification::CAR); + + // Generate target point + Eigen::Vector2d target_point; + target_point << 0.0, 0.0; + + // Generate predicted path + const PredictedPath predicted_path = + path_generator.generatePathToTargetPoint(tracked_object, target_point); + + // Check + EXPECT_FALSE(predicted_path.path.empty()); + EXPECT_EQ(predicted_path.path[0].position.x, 0.0); + EXPECT_EQ(predicted_path.path[0].position.y, 0.0); + EXPECT_EQ(predicted_path.path[0].position.z, 0.0); +} diff --git a/perception/map_based_prediction/test/test_map_based_prediction.cpp b/perception/map_based_prediction/test/test_map_based_prediction.cpp new file mode 100644 index 0000000000000..4c8ad7dd25916 --- /dev/null +++ b/perception/map_based_prediction/test/test_map_based_prediction.cpp @@ -0,0 +1,26 @@ +// 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 + +#include + +int main(int argc, char * argv[]) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + bool result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}