From 42ee39829b7618839ddb87de42804fdd8d6e33de Mon Sep 17 00:00:00 2001 From: kyoichi-sugahara Date: Tue, 24 Dec 2024 19:19:46 +0900 Subject: [PATCH] test(autoware_behavior_path_start_planner_module): refactor unit tests for shift pull out path generation by using parameter initialization function Signed-off-by: kyoichi-sugahara --- .../test/test_geometric_pull_out.cpp | 134 ++++++------------ 1 file changed, 44 insertions(+), 90 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp index fb39e186afd4e..5a9546c5510b4 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/test/test_geometric_pull_out.cpp @@ -37,6 +37,7 @@ using autoware::test_utils::get_absolute_path_to_config; using autoware_planning_msgs::msg::LaneletRoute; using RouteSections = std::vector; using autoware_planning_test_manager::utils::makeBehaviorRouteFromLaneId; +using geometry_msgs::msg::Pose; namespace autoware::behavior_path_planner { @@ -44,7 +45,7 @@ namespace autoware::behavior_path_planner class TestGeometricPullOut : public ::testing::Test { public: - std::optional plan( + std::optional call_plan( const Pose & start_pose, const Pose & goal_pose, PlannerDebugData & planner_debug_data) { return geometric_pull_out_->plan(start_pose, goal_pose, planner_debug_data); @@ -54,27 +55,50 @@ class TestGeometricPullOut : public ::testing::Test void SetUp() override { rclcpp::init(0, nullptr); - node_ = rclcpp::Node::make_shared("geometric_pull_out", get_node_options()); + node_ = rclcpp::Node::make_shared("geometric_pull_out", make_node_options()); - load_parameters(); - initialize_vehicle_info(); initialize_lane_departure_checker(); - initialize_routeHandler(); initialize_geometric_pull_out_planner(); - initialize_planner_data(); } - void TearDown() override { rclcpp::shutdown(); } + + PlannerData make_planner_data( + const Pose & start_pose, const int route_start_lane_id, const int route_goal_lane_id) + { + PlannerData planner_data; + planner_data.init_parameters(*node_); + + // Load a sample lanelet map and create a route handler + const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( + "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); + const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); + auto route_handler = std::make_shared(map_bin_msg); + + // Set up current odometry at start pose + auto odometry = std::make_shared(); + odometry->pose.pose = start_pose; + odometry->header.frame_id = "map"; + planner_data.self_odometry = odometry; + + // Setup route + const auto route = makeBehaviorRouteFromLaneId( + route_start_lane_id, route_goal_lane_id, "autoware_test_utils", + "road_shoulder/lanelet2_map.osm"); + route_handler->setRoute(route); + + // Update planner data with the route handler + planner_data.route_handler = route_handler; + + return planner_data; + } + // Member variables std::shared_ptr node_; - std::shared_ptr route_handler_; - autoware::vehicle_info_utils::VehicleInfo vehicle_info_; std::shared_ptr geometric_pull_out_; std::shared_ptr lane_departure_checker_; - PlannerData planner_data_; private: - rclcpp::NodeOptions get_node_options() const + rclcpp::NodeOptions make_node_options() const { // Load common configuration files auto node_options = rclcpp::NodeOptions{}; @@ -102,85 +126,25 @@ class TestGeometricPullOut : public ::testing::Test return node_options; } - void load_parameters() - { - const auto dp_double = [&](const std::string & s) { - return node_->declare_parameter(s); - }; - const auto dp_bool = [&](const std::string & s) { return node_->declare_parameter(s); }; - // Load parameters required for planning - const std::string ns = "start_planner."; - lane_departure_check_expansion_margin_ = - dp_double(ns + "lane_departure_check_expansion_margin"); - pull_out_max_steer_angle_ = dp_double(ns + "pull_out_max_steer_angle"); - pull_out_arc_path_interval_ = dp_double(ns + "arc_path_interval"); - center_line_path_interval_ = dp_double(ns + "center_line_path_interval"); - th_moving_object_velocity_ = dp_double(ns + "th_moving_object_velocity"); - divide_pull_out_path_ = dp_bool(ns + "divide_pull_out_path"); - backward_path_length_ = dp_double("backward_path_length"); - forward_path_length_ = dp_double("forward_path_length"); - } - - void initialize_vehicle_info() - { - vehicle_info_ = autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); - } - void initialize_lane_departure_checker() { + const auto vehicle_info = + autoware::vehicle_info_utils::VehicleInfoUtils(*node_).getVehicleInfo(); lane_departure_checker_ = std::make_shared(); - lane_departure_checker_->setVehicleInfo(vehicle_info_); + lane_departure_checker_->setVehicleInfo(vehicle_info); autoware::lane_departure_checker::Param lane_departure_checker_params{}; - lane_departure_checker_params.footprint_extra_margin = lane_departure_check_expansion_margin_; lane_departure_checker_->setParam(lane_departure_checker_params); } - void initialize_routeHandler() - { - // Load a sample lanelet map and create a route handler - const auto shoulder_map_path = autoware::test_utils::get_absolute_path_to_lanelet_map( - "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); - const auto map_bin_msg = autoware::test_utils::make_map_bin_msg(shoulder_map_path, 0.5); - - route_handler_ = std::make_shared(map_bin_msg); - } - void initialize_geometric_pull_out_planner() { - auto parameters = std::make_shared(); - parameters->parallel_parking_parameters.pull_out_max_steer_angle = pull_out_max_steer_angle_; - parameters->parallel_parking_parameters.pull_out_arc_path_interval = - pull_out_arc_path_interval_; - parameters->parallel_parking_parameters.center_line_path_interval = center_line_path_interval_; - parameters->th_moving_object_velocity = th_moving_object_velocity_; - parameters->divide_pull_out_path = divide_pull_out_path_; + auto parameters = StartPlannerParameters::init(*node_); auto time_keeper = std::make_shared(); geometric_pull_out_ = - std::make_shared(*node_, *parameters, lane_departure_checker_, time_keeper); - } - - void initialize_planner_data() - { - planner_data_.parameters.backward_path_length = backward_path_length_; - planner_data_.parameters.forward_path_length = forward_path_length_; - planner_data_.parameters.wheel_base = vehicle_info_.wheel_base_m; - planner_data_.parameters.wheel_tread = vehicle_info_.wheel_tread_m; - planner_data_.parameters.front_overhang = vehicle_info_.front_overhang_m; - planner_data_.parameters.left_over_hang = vehicle_info_.left_overhang_m; - planner_data_.parameters.right_over_hang = vehicle_info_.right_overhang_m; + std::make_shared(*node_, parameters, lane_departure_checker_, time_keeper); } - - // Parameter variables - double lane_departure_check_expansion_margin_{0.0}; - double pull_out_max_steer_angle_{0.0}; - double pull_out_arc_path_interval_{0.0}; - double center_line_path_interval_{0.0}; - double th_moving_object_velocity_{0.0}; - double backward_path_length_{0.0}; - double forward_path_length_{0.0}; - bool divide_pull_out_path_{false}; }; TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) @@ -199,26 +163,16 @@ TEST_F(TestGeometricPullOut, GenerateValidGeometricPullOutPath) geometry_msgs::build().x(0.0).y(0.0).z(0.705897).w( 0.708314)); - // Set up current odometry at start pose - auto odometry = std::make_shared(); - odometry->pose.pose = start_pose; - odometry->header.frame_id = "map"; - planner_data_.self_odometry = odometry; - - // Setup route - const auto route = makeBehaviorRouteFromLaneId( - 4619, 4635, "autoware_test_utils", "road_shoulder/lanelet2_map.osm"); - route_handler_->setRoute(route); + const auto planner_data = make_planner_data(start_pose, 4619, 4635); // Update planner data with the route handler - planner_data_.route_handler = route_handler_; - geometric_pull_out_->setPlannerData(std::make_shared(planner_data_)); + geometric_pull_out_->setPlannerData(std::make_shared(planner_data)); // Plan the pull out path PlannerDebugData debug_data; - auto result = plan(start_pose, goal_pose, debug_data); + auto result = call_plan(start_pose, goal_pose, debug_data); - // Assert that a valid geometric geometric pull out path is generated + // Assert that a valid geometric pull out path is generated ASSERT_TRUE(result.has_value()) << "Geometric pull out path generation failed."; EXPECT_EQ(result->partial_paths.size(), 2UL) << "Generated geometric pull out path does not have the expected number of partial paths.";