|
| 1 | +// Copyright 2024 TIER IV, Inc. |
| 2 | +// |
| 3 | +// Licensed under the Apache License, Version 2.0 (the "License"); |
| 4 | +// you may not use this file except in compliance with the License. |
| 5 | +// You may obtain a copy of the License at |
| 6 | +// |
| 7 | +// http://www.apache.org/licenses/LICENSE-2.0 |
| 8 | +// |
| 9 | +// Unless required by applicable law or agreed to in writing, software |
| 10 | +// distributed under the License is distributed on an "AS IS" BASIS, |
| 11 | +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. |
| 12 | +// See the License for the specific language governing permissions and |
| 13 | +// limitations under the License. |
| 14 | + |
| 15 | +#include <ament_index_cpp/get_package_share_directory.hpp> |
| 16 | +#include <autoware/behavior_path_start_planner_module/shift_pull_out.hpp> |
| 17 | +#include <autoware/behavior_path_start_planner_module/start_planner_module.hpp> |
| 18 | +#include <autoware/behavior_path_start_planner_module/util.hpp> |
| 19 | +#include <autoware/lane_departure_checker/lane_departure_checker.hpp> |
| 20 | +#include <autoware/route_handler/route_handler.hpp> |
| 21 | +#include <autoware_lanelet2_extension/utility/query.hpp> |
| 22 | +#include <autoware_planning_test_manager/autoware_planning_test_manager_utils.hpp> |
| 23 | +#include <autoware_test_utils/autoware_test_utils.hpp> |
| 24 | + |
| 25 | +#include <gtest/gtest.h> |
| 26 | + |
| 27 | +#include <memory> |
| 28 | +#include <string> |
| 29 | +#include <vector> |
| 30 | + |
| 31 | +using autoware::behavior_path_planner::ShiftPullOut; |
| 32 | +using autoware::behavior_path_planner::StartPlannerParameters; |
| 33 | +using autoware::lane_departure_checker::LaneDepartureChecker; |
| 34 | +using autoware::test_utils::get_absolute_path_to_config; |
| 35 | +using autoware_planning_msgs::msg::LaneletRoute; |
| 36 | +using RouteSections = std::vector<autoware_planning_msgs::msg::LaneletSegment>; |
| 37 | +using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; |
| 38 | + |
| 39 | +namespace autoware::behavior_path_planner |
| 40 | +{ |
| 41 | + |
| 42 | +class TestShiftPullOut : public ::testing::Test |
| 43 | +{ |
| 44 | +public: |
| 45 | + std::optional<PullOutPath> call_plan( |
| 46 | + const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) |
| 47 | + { |
| 48 | + return shift_pull_out_->plan(start_pose, goal_pose, planner_debug_data); |
| 49 | + } |
| 50 | + |
| 51 | +protected: |
| 52 | + void SetUp() override |
| 53 | + { |
| 54 | + rclcpp::init(0, nullptr); |
| 55 | + node_ = rclcpp::Node::make_shared("shift_pull_out", make_node_options()); |
| 56 | + |
| 57 | + initialize_lane_departure_checker(); |
| 58 | + initialize_shift_pull_out_planner(); |
| 59 | + } |
| 60 | + |
| 61 | + void TearDown() override { rclcpp::shutdown(); } |
| 62 | + |
| 63 | + PlannerData make_planner_data( |
| 64 | + const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) |
| 65 | + { |
| 66 | + PlannerData planner_data; |
| 67 | + planner_data.init_parameters(*node_); |
| 68 | + |
| 69 | + // Load a sample lanelet map and create a route handler |
| 70 | + const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( |
| 71 | + "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); |
| 72 | + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); |
| 73 | + auto route_handler = std::make_shared<autoware::route_handler::RouteHandler>(map_bin_msg); |
| 74 | + |
| 75 | + // Set up current odometry at start pose |
| 76 | + auto odometry = std::make_shared<nav_msgs::msg::Odometry>(); |
| 77 | + odometry->pose.pose = start_pose; |
| 78 | + odometry->header.frame_id = "map"; |
| 79 | + planner_data.self_odometry = odometry; |
| 80 | + |
| 81 | + // Setup route |
| 82 | + const auto route = makeBehaviorRouteFromLaneId( |
| 83 | + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", |
| 84 | + "road_shoulder/lanelet2_map.osm"); |
| 85 | + route_handler->setRoute(route); |
| 86 | + |
| 87 | + // Update planner data with the route handler |
| 88 | + planner_data.route_handler = route_handler; |
| 89 | + |
| 90 | + return planner_data; |
| 91 | + } |
| 92 | + |
| 93 | + // Member variables |
| 94 | + std::shared_ptr<rclcpp::Node> node_; |
| 95 | + std::shared_ptr<ShiftPullOut> shift_pull_out_; |
| 96 | + std::shared_ptr<LaneDepartureChecker> lane_departure_checker_; |
| 97 | + |
| 98 | +private: |
| 99 | + rclcpp::NodeOptions make_node_options() const |
| 100 | + { |
| 101 | + // Load common configuration files |
| 102 | + auto node_options = rclcpp::NodeOptions{}; |
| 103 | + |
| 104 | + const auto common_param_path = |
| 105 | + get_absolute_path_to_config("autoware_test_utils", "test_common.param.yaml"); |
| 106 | + const auto nearest_search_param_path = |
| 107 | + get_absolute_path_to_config("autoware_test_utils", "test_nearest_search.param.yaml"); |
| 108 | + const auto vehicle_info_param_path = |
| 109 | + get_absolute_path_to_config("autoware_test_utils", "test_vehicle_info.param.yaml"); |
| 110 | + const auto behavior_path_planner_param_path = get_absolute_path_to_config( |
| 111 | + "autoware_behavior_path_planner", "behavior_path_planner.param.yaml"); |
| 112 | + const auto drivable_area_expansion_param_path = get_absolute_path_to_config( |
| 113 | + "autoware_behavior_path_planner", "drivable_area_expansion.param.yaml"); |
| 114 | + const auto scene_module_manager_param_path = get_absolute_path_to_config( |
| 115 | + "autoware_behavior_path_planner", "scene_module_manager.param.yaml"); |
| 116 | + const auto start_planner_param_path = get_absolute_path_to_config( |
| 117 | + "autoware_behavior_path_start_planner_module", "start_planner.param.yaml"); |
| 118 | + |
| 119 | + autoware::test_utils::updateNodeOptions( |
| 120 | + node_options, {common_param_path, nearest_search_param_path, vehicle_info_param_path, |
| 121 | + behavior_path_planner_param_path, drivable_area_expansion_param_path, |
| 122 | + scene_module_manager_param_path, start_planner_param_path}); |
| 123 | + |
| 124 | + return node_options; |
| 125 | + } |
| 126 | + |
| 127 | + void initialize_lane_departure_checker() |
| 128 | + { |
| 129 | + const auto vehicle_info = |
| 130 | + autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); |
| 131 | + lane_departure_checker_ = std::make_shared<LaneDepartureChecker>(); |
| 132 | + lane_departure_checker_->setVehicleInfo(vehicle_info); |
| 133 | + |
| 134 | + autoware::lane_departure_checker::Param lane_departure_checker_params{}; |
| 135 | + lane_departure_checker_->setParam(lane_departure_checker_params); |
| 136 | + } |
| 137 | + |
| 138 | + void initialize_shift_pull_out_planner() |
| 139 | + { |
| 140 | + auto parameters = StartPlannerParameters::init(*node_); |
| 141 | + |
| 142 | + auto time_keeper = std::make_shared<autoware::universe_utils::TimeKeeper>(); |
| 143 | + shift_pull_out_ = |
| 144 | + std::make_shared<ShiftPullOut>(*node_, parameters, lane_departure_checker_, time_keeper); |
| 145 | + } |
| 146 | +}; |
| 147 | + |
| 148 | +TEST_F(TestShiftPullOut, GenerateValidShiftPullOutPath) |
| 149 | +{ |
| 150 | + const auto start_pose = |
| 151 | + geometry_msgs::build<geometry_msgs::msg::Pose>() |
| 152 | + .position(geometry_msgs::build<geometry_msgs::msg::Point>().x(362.181).y(362.164).z(100.000)) |
| 153 | + .orientation( |
| 154 | + geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(0.0).y(0.0).z(0.709650).w( |
| 155 | + 0.704554)); |
| 156 | + |
| 157 | + const auto goal_pose = |
| 158 | + geometry_msgs::build<geometry_msgs::msg::Pose>() |
| 159 | + .position(geometry_msgs::build<geometry_msgs::msg::Point>().x(365.658).y(507.253).z(100.000)) |
| 160 | + .orientation( |
| 161 | + geometry_msgs::build<geometry_msgs::msg::Quaternion>().x(0.0).y(0.0).z(0.705897).w( |
| 162 | + 0.708314)); |
| 163 | + const auto planner_data = make_planner_data(start_pose, 4619, 4635); |
| 164 | + |
| 165 | + shift_pull_out_-> setPlannerData(std::make_shared<PlannerData>(planner_data)); |
| 166 | + |
| 167 | + // Plan the pull out path |
| 168 | + PlannerDebugData debug_data; |
| 169 | + auto result = call_plan(start_pose, goal_pose, debug_data); |
| 170 | + |
| 171 | + // Assert that a valid shift pull out path is generated |
| 172 | + ASSERT_TRUE(result.has_value()) << "shift pull out path generation failed."; |
| 173 | + EXPECT_EQ(result->partial_paths.size(), 1UL) |
| 174 | + << "Generated shift pull out path does not have the expected number of partial paths."; |
| 175 | + EXPECT_EQ(debug_data.conditions_evaluation.back(), "success") |
| 176 | + << "shift pull out path planning did not succeed."; |
| 177 | +} |
| 178 | + |
| 179 | +} // namespace autoware::behavior_path_planner |
0 commit comments