diff --git a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml index d3a452a0db4f1..56ab9130ed19e 100644 --- a/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml +++ b/planning/behavior_path_planner/config/pull_over/pull_over.param.yaml @@ -87,6 +87,7 @@ min_stop_distance: 5.0 stop_time: 2.0 hysteresis_buffer_distance: 2.0 + prediction_time_resolution: 0.5 enable_collision_check_at_prepare_phase: false use_predicted_path_outside_lanelet: false use_all_predicted_path: false diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp index c08afe5f7b97f..6f5acfd30ef2b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/freespace_pull_over.hpp @@ -29,11 +29,8 @@ namespace behavior_path_planner { using freespace_planning_algorithms::AbstractPlanningAlgorithm; -using freespace_planning_algorithms::AstarParam; using freespace_planning_algorithms::AstarSearch; -using freespace_planning_algorithms::PlannerCommonParam; using freespace_planning_algorithms::RRTStar; -using freespace_planning_algorithms::RRTStarParam; class FreespacePullOver : public PullOverPlannerBase { @@ -47,10 +44,6 @@ class FreespacePullOver : public PullOverPlannerBase boost::optional plan(const Pose & goal_pose) override; protected: - PlannerCommonParam getCommonParam(rclcpp::Node & node) const; - AstarParam getAstarParam(rclcpp::Node & node) const; - RRTStarParam getRRTStarParam(rclcpp::Node & node) const; - std::unique_ptr planner_; double velocity_; bool use_back_; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp index 6a78f2ed692d9..48baadc7d731b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/pull_over/pull_over_parameters.hpp @@ -16,11 +16,20 @@ #ifndef BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ #define BEHAVIOR_PATH_PLANNER__UTIL__PULL_OVER__PULL_OVER_PARAMETERS_HPP_ +#include +#include +#include + #include #include namespace behavior_path_planner { + +using freespace_planning_algorithms::AstarParam; +using freespace_planning_algorithms::PlannerCommonParam; +using freespace_planning_algorithms::RRTStarParam; + struct PullOverParameters { double request_length; @@ -44,8 +53,8 @@ struct PullOverParameters bool use_occupancy_grid; bool use_occupancy_grid_for_longitudinal_margin; double occupancy_grid_collision_check_margin; - double theta_size; - double obstacle_threshold; + int theta_size; + int obstacle_threshold; // object recognition bool use_object_recognition; double object_recognition_collision_check_margin; @@ -90,6 +99,14 @@ struct PullOverParameters std::vector drivable_area_types_to_skip; // debug bool print_debug_info; + + // freespace pull over + std::string algorithm; + double freespace_parking_velocity; + double vehicle_shape_margin; + PlannerCommonParam common_parameters; + AstarParam astar_parameters; + RRTStarParam rrt_star_parameters; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 3132a0c5fc45d..7fc12eb616da9 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -690,97 +690,156 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() PullOverParameters BehaviorPathPlannerNode::getPullOverParam() { - const auto dp = [this](const std::string & str, auto def_val) { - std::string name = "pull_over." + str; - return this->declare_parameter(name, def_val); - }; - PullOverParameters p; - p.request_length = dp("request_length", 200.0); - p.th_stopped_velocity = dp("th_stopped_velocity", 0.01); - p.th_arrived_distance = dp("th_arrived_distance", 0.3); - p.th_stopped_time = dp("th_stopped_time", 2.0); - p.margin_from_boundary = dp("margin_from_boundary", 0.3); - p.decide_path_distance = dp("decide_path_distance", 10.0); - p.maximum_deceleration = dp("maximum_deceleration", 1.0); - // goal research - p.enable_goal_research = dp("enable_goal_research", true); - p.search_priority = dp("search_priority", "efficient_path"); - p.forward_goal_search_length = dp("forward_goal_search_length", 20.0); - p.backward_goal_search_length = dp("backward_goal_search_length", 20.0); - p.goal_search_interval = dp("goal_search_interval", 5.0); - p.longitudinal_margin = dp("longitudinal_margin", 3.0); - p.max_lateral_offset = dp("max_lateral_offset", 1.0); - p.lateral_offset_interval = dp("lateral_offset_interval", 0.25); - p.ignore_distance_from_lane_start = dp("ignore_distance_from_lane_start", 15.0); - // occupancy grid map - p.use_occupancy_grid = dp("use_occupancy_grid", true); - p.use_occupancy_grid_for_longitudinal_margin = - dp("use_occupancy_grid_for_longitudinal_margin", false); - p.occupancy_grid_collision_check_margin = dp("occupancy_grid_collision_check_margin", 0.0); - p.theta_size = dp("theta_size", 360); - p.obstacle_threshold = dp("obstacle_threshold", 90); - // object recognition - p.use_object_recognition = dp("use_object_recognition", true); - p.object_recognition_collision_check_margin = - dp("object_recognition_collision_check_margin", 1.0); - // shift path - p.enable_shift_parking = dp("enable_shift_parking", true); - p.pull_over_sampling_num = dp("pull_over_sampling_num", 4); - p.maximum_lateral_jerk = dp("maximum_lateral_jerk", 3.0); - p.minimum_lateral_jerk = dp("minimum_lateral_jerk", 1.0); - p.deceleration_interval = dp("deceleration_interval", 10.0); - p.pull_over_velocity = dp("pull_over_velocity", 8.3); - p.pull_over_minimum_velocity = dp("pull_over_minimum_velocity", 0.3); - p.after_pull_over_straight_distance = dp("after_pull_over_straight_distance", 3.0); - // parallel parking - p.enable_arc_forward_parking = dp("enable_arc_forward_parking", true); - p.enable_arc_backward_parking = dp("enable_arc_backward_parking", true); - p.after_forward_parking_straight_distance = dp("after_forward_parking_straight_distance", 0.5); - p.after_backward_parking_straight_distance = dp("after_backward_parking_straight_distance", 0.5); - p.forward_parking_velocity = dp("forward_parking_velocity", 1.0); - p.backward_parking_velocity = dp("backward_parking_velocity", -0.5); - p.forward_parking_lane_departure_margin = dp("forward_parking_lane_departure_margin", 0.0); - p.backward_parking_lane_departure_margin = dp("backward_parking_lane_departure_margin", 0.0); - p.arc_path_interval = dp("arc_path_interval", 1.0); - p.pull_over_max_steer_angle = dp("pull_over_max_steer_angle", 0.35); // 20deg - // freespace parking - p.enable_freespace_parking = dp("enable_freespace_parking", true); - // hazard - p.hazard_on_threshold_distance = dp("hazard_on_threshold_distance", 1.0); - p.hazard_on_threshold_velocity = dp("hazard_on_threshold_velocity", 0.5); - // safety with dynamic objects. Not used now. - p.pull_over_duration = dp("pull_over_duration", 4.0); - p.pull_over_prepare_duration = dp("pull_over_prepare_duration", 2.0); - p.min_stop_distance = dp("min_stop_distance", 5.0); - p.stop_time = dp("stop_time", 2.0); - p.hysteresis_buffer_distance = dp("hysteresis_buffer_distance", 2.0); - p.prediction_time_resolution = dp("prediction_time_resolution", 0.5); - p.enable_collision_check_at_prepare_phase = dp("enable_collision_check_at_prepare_phase", true); - p.use_predicted_path_outside_lanelet = dp("use_predicted_path_outside_lanelet", true); - p.use_all_predicted_path = dp("use_all_predicted_path", false); - // drivable area - p.drivable_area_right_bound_offset = dp("drivable_area_right_bound_offset", 0.0); - p.drivable_area_left_bound_offset = dp("drivable_area_left_bound_offset", 0.0); - p.drivable_area_types_to_skip = - dp("drivable_area_types_to_skip", std::vector({"road_border"})); - // debug - p.print_debug_info = dp("print_debug_info", false); - // validation of parameters - if (p.pull_over_sampling_num < 1) { - RCLCPP_FATAL_STREAM( - get_logger(), "pull_over_sampling_num must be positive integer. Given parameter: " - << p.pull_over_sampling_num << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); + { + std::string ns = "pull_over."; + p.request_length = declare_parameter(ns + "request_length"); + p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); + p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); + p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); + p.margin_from_boundary = declare_parameter(ns + "margin_from_boundary"); + p.decide_path_distance = declare_parameter(ns + "decide_path_distance"); + p.maximum_deceleration = declare_parameter(ns + "maximum_deceleration"); + // goal research + p.enable_goal_research = declare_parameter(ns + "enable_goal_research"); + p.search_priority = declare_parameter(ns + "search_priority"); + p.forward_goal_search_length = declare_parameter(ns + "forward_goal_search_length"); + p.backward_goal_search_length = declare_parameter(ns + "backward_goal_search_length"); + p.goal_search_interval = declare_parameter(ns + "goal_search_interval"); + p.longitudinal_margin = declare_parameter(ns + "longitudinal_margin"); + p.max_lateral_offset = declare_parameter(ns + "max_lateral_offset"); + p.lateral_offset_interval = declare_parameter(ns + "lateral_offset_interval"); + p.ignore_distance_from_lane_start = + declare_parameter(ns + "ignore_distance_from_lane_start"); + // occupancy grid map + p.use_occupancy_grid = declare_parameter(ns + "use_occupancy_grid"); + p.use_occupancy_grid_for_longitudinal_margin = + declare_parameter(ns + "use_occupancy_grid_for_longitudinal_margin"); + p.occupancy_grid_collision_check_margin = + declare_parameter(ns + "occupancy_grid_collision_check_margin"); + p.theta_size = declare_parameter(ns + "theta_size"); + p.obstacle_threshold = declare_parameter(ns + "obstacle_threshold"); + // object recognition + p.use_object_recognition = declare_parameter(ns + "use_object_recognition"); + p.object_recognition_collision_check_margin = + declare_parameter(ns + "object_recognition_collision_check_margin"); + // shift path + p.enable_shift_parking = declare_parameter(ns + "enable_shift_parking"); + p.pull_over_sampling_num = declare_parameter(ns + "pull_over_sampling_num"); + p.maximum_lateral_jerk = declare_parameter(ns + "maximum_lateral_jerk"); + p.minimum_lateral_jerk = declare_parameter(ns + "minimum_lateral_jerk"); + p.deceleration_interval = declare_parameter(ns + "deceleration_interval"); + p.pull_over_velocity = declare_parameter(ns + "pull_over_velocity"); + p.pull_over_minimum_velocity = declare_parameter(ns + "pull_over_minimum_velocity"); + p.after_pull_over_straight_distance = + declare_parameter(ns + "after_pull_over_straight_distance"); + // parallel parking + p.enable_arc_forward_parking = declare_parameter(ns + "enable_arc_forward_parking"); + p.enable_arc_backward_parking = declare_parameter(ns + "enable_arc_backward_parking"); + p.after_forward_parking_straight_distance = + declare_parameter(ns + "after_forward_parking_straight_distance"); + p.after_backward_parking_straight_distance = + declare_parameter(ns + "after_backward_parking_straight_distance"); + p.forward_parking_velocity = declare_parameter(ns + "forward_parking_velocity"); + p.backward_parking_velocity = declare_parameter(ns + "backward_parking_velocity"); + p.forward_parking_lane_departure_margin = + declare_parameter(ns + "forward_parking_lane_departure_margin"); + p.backward_parking_lane_departure_margin = + declare_parameter(ns + "backward_parking_lane_departure_margin"); + p.arc_path_interval = declare_parameter(ns + "arc_path_interval"); + p.pull_over_max_steer_angle = + declare_parameter(ns + "pull_over_max_steer_angle"); // 20deg + // freespace parking + p.enable_freespace_parking = declare_parameter(ns + "enable_freespace_parking"); + // hazard + p.hazard_on_threshold_distance = declare_parameter(ns + "hazard_on_threshold_distance"); + p.hazard_on_threshold_velocity = declare_parameter(ns + "hazard_on_threshold_velocity"); + // safety with dynamic objects. Not used now. + p.pull_over_duration = declare_parameter(ns + "pull_over_duration"); + p.pull_over_prepare_duration = declare_parameter(ns + "pull_over_prepare_duration"); + p.min_stop_distance = declare_parameter(ns + "min_stop_distance"); + p.stop_time = declare_parameter(ns + "stop_time"); + p.hysteresis_buffer_distance = declare_parameter(ns + "hysteresis_buffer_distance"); + p.prediction_time_resolution = declare_parameter(ns + "prediction_time_resolution"); + p.enable_collision_check_at_prepare_phase = + declare_parameter(ns + "enable_collision_check_at_prepare_phase"); + p.use_predicted_path_outside_lanelet = + declare_parameter(ns + "use_predicted_path_outside_lanelet"); + p.use_all_predicted_path = declare_parameter(ns + "use_all_predicted_path"); + // drivable area + p.drivable_area_right_bound_offset = + declare_parameter(ns + "drivable_area_right_bound_offset"); + p.drivable_area_left_bound_offset = + declare_parameter(ns + "drivable_area_left_bound_offset"); + p.drivable_area_types_to_skip = + declare_parameter>(ns + "drivable_area_types_to_skip"); + // debug + p.print_debug_info = declare_parameter(ns + "print_debug_info"); + + // validation of parameters + if (p.pull_over_sampling_num < 1) { + RCLCPP_FATAL_STREAM( + get_logger(), "pull_over_sampling_num must be positive integer. Given parameter: " + << p.pull_over_sampling_num << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } + if (p.maximum_deceleration < 0.0) { + RCLCPP_FATAL_STREAM( + get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " + << p.maximum_deceleration << std::endl + << "Terminating the program..."); + exit(EXIT_FAILURE); + } } - if (p.maximum_deceleration < 0.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "maximum_deceleration cannot be negative value. Given parameter: " - << p.maximum_deceleration << std::endl - << "Terminating the program..."); - exit(EXIT_FAILURE); + + { + std::string ns = "pull_over.freespace_parking."; + // search configs + p.algorithm = declare_parameter(ns + "planning_algorithm"); + p.freespace_parking_velocity = declare_parameter(ns + "velocity"); + p.vehicle_shape_margin = declare_parameter(ns + "vehicle_shape_margin"); + p.common_parameters.time_limit = declare_parameter(ns + "time_limit"); + p.common_parameters.minimum_turning_radius = + declare_parameter(ns + "minimum_turning_radius"); + p.common_parameters.maximum_turning_radius = + declare_parameter(ns + "maximum_turning_radius"); + p.common_parameters.turning_radius_size = declare_parameter(ns + "turning_radius_size"); + p.common_parameters.maximum_turning_radius = std::max( + p.common_parameters.maximum_turning_radius, p.common_parameters.minimum_turning_radius); + p.common_parameters.turning_radius_size = std::max(p.common_parameters.turning_radius_size, 1); + + p.common_parameters.theta_size = declare_parameter(ns + "theta_size"); + p.common_parameters.angle_goal_range = declare_parameter(ns + "angle_goal_range"); + + p.common_parameters.curve_weight = declare_parameter(ns + "curve_weight"); + p.common_parameters.reverse_weight = declare_parameter(ns + "reverse_weight"); + p.common_parameters.lateral_goal_range = declare_parameter(ns + "lateral_goal_range"); + p.common_parameters.longitudinal_goal_range = + declare_parameter(ns + "longitudinal_goal_range"); + + // costmap configs + p.common_parameters.obstacle_threshold = declare_parameter(ns + "obstacle_threshold"); + } + + { + std::string ns = "pull_over.freespace_parking.astar."; + p.astar_parameters.only_behind_solutions = + declare_parameter(ns + "only_behind_solutions"); + p.astar_parameters.use_back = declare_parameter(ns + "use_back"); + p.astar_parameters.distance_heuristic_weight = + declare_parameter(ns + "distance_heuristic_weight"); + } + + { + std::string ns = "pull_over.freespace_parking.rrtstar."; + p.rrt_star_parameters.enable_update = declare_parameter(ns + "enable_update"); + p.rrt_star_parameters.use_informed_sampling = + declare_parameter(ns + "use_informed_sampling"); + p.rrt_star_parameters.max_planning_time = declare_parameter(ns + "max_planning_time"); + p.rrt_star_parameters.neighbor_radius = declare_parameter(ns + "neighbor_radius"); + p.rrt_star_parameters.margin = declare_parameter(ns + "margin"); } return p; diff --git a/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp b/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp index fc433f9f63213..69e71a54d0b34 100644 --- a/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp +++ b/planning/behavior_path_planner/src/util/pull_over/freespace_pull_over.cpp @@ -25,92 +25,21 @@ namespace behavior_path_planner FreespacePullOver::FreespacePullOver( rclcpp::Node & node, const PullOverParameters & parameters, const vehicle_info_util::VehicleInfo & vehicle_info) -: PullOverPlannerBase{node, parameters} +: PullOverPlannerBase{node, parameters}, velocity_{parameters.freespace_parking_velocity} { - velocity_ = node.declare_parameter("pull_over.freespace_parking.velocity", 1.0); - - const double vehicle_shape_margin = - node.declare_parameter("pull_over.freespace_parking.vehicle_shape_margin", 1.0); - freespace_planning_algorithms::VehicleShape vehicle_shape(vehicle_info, vehicle_shape_margin); - - const auto algorithm = - node.declare_parameter("pull_over.freespace_parking.planning_algorithm", "astar"); - if (algorithm == "astar") { - const auto astar_param = getAstarParam(node); - use_back_ = astar_param.use_back; - planner_ = std::make_unique(getCommonParam(node), vehicle_shape, astar_param); - } else if (algorithm == "rrtstar") { + freespace_planning_algorithms::VehicleShape vehicle_shape( + vehicle_info, parameters.vehicle_shape_margin); + if (parameters.algorithm == "astar") { + use_back_ = parameters.astar_parameters.use_back; + planner_ = std::make_unique( + parameters.common_parameters, vehicle_shape, parameters.astar_parameters); + } else if (parameters.algorithm == "rrtstar") { use_back_ = true; // no option for disabling back in rrtstar - planner_ = - std::make_unique(getCommonParam(node), vehicle_shape, getRRTStarParam(node)); + planner_ = std::make_unique( + parameters.common_parameters, vehicle_shape, parameters.rrt_star_parameters); } } -PlannerCommonParam FreespacePullOver::getCommonParam(rclcpp::Node & node) const -{ - const auto dp = [&node](const std::string & str, auto def_val) { - std::string name = "pull_over.freespace_parking." + str; - return node.declare_parameter(name, def_val); - }; - - PlannerCommonParam p{}; - - // search configs - p.time_limit = dp("time_limit", 5000.0); - p.minimum_turning_radius = dp("minimum_turning_radius", 0.5); - p.maximum_turning_radius = dp("maximum_turning_radius", 6.0); - p.turning_radius_size = dp("turning_radius_size", 11); - p.maximum_turning_radius = std::max(p.maximum_turning_radius, p.minimum_turning_radius); - p.turning_radius_size = std::max(p.turning_radius_size, 1); - - p.theta_size = dp("theta_size", 48); - p.angle_goal_range = dp("angle_goal_range", 6.0); - - p.curve_weight = dp("curve_weight", 1.2); - p.reverse_weight = dp("reverse_weight", 2.00); - p.lateral_goal_range = dp("lateral_goal_range", 0.5); - p.longitudinal_goal_range = dp("longitudinal_goal_range", 2.0); - - // costmap configs - p.obstacle_threshold = dp("obstacle_threshold", 100); - - return p; -} - -AstarParam FreespacePullOver::getAstarParam(rclcpp::Node & node) const -{ - const auto dp = [&node](const std::string & str, auto def_val) { - std::string name = "pull_over.freespace_parking.astar." + str; - return node.declare_parameter(name, def_val); - }; - - AstarParam p{}; - - p.only_behind_solutions = dp("only_behind_solutions", false); - p.use_back = dp("use_back", true); - p.distance_heuristic_weight = dp("distance_heuristic_weight", 1.0); - - return p; -} - -RRTStarParam FreespacePullOver::getRRTStarParam(rclcpp::Node & node) const -{ - const auto dp = [&node](const std::string & str, auto def_val) { - std::string name = "pull_over.freespace_parking.rrtstar." + str; - return node.declare_parameter(name, def_val); - }; - - RRTStarParam p; - - p.enable_update = dp("enable_update", true); - p.use_informed_sampling = dp("use_informed_sampling", true); - p.max_planning_time = dp("max_planning_time", 150.0); - p.neighbor_radius = dp("neighbor_radius", 8.0); - p.margin = dp("margin", 0.1); - - return p; -} - boost::optional FreespacePullOver::plan(const Pose & goal_pose) { const Pose & current_pose = planner_data_->self_odometry->pose.pose;