diff --git a/planning/behavior_velocity_planner/config/blind_spot.param.yaml b/planning/behavior_velocity_planner/config/blind_spot.param.yaml index 332e8bfb48d68..31f75d7f7c5df 100644 --- a/planning/behavior_velocity_planner/config/blind_spot.param.yaml +++ b/planning/behavior_velocity_planner/config/blind_spot.param.yaml @@ -1,6 +1,7 @@ /**: ros__parameters: blind_spot: + use_pass_judge_line: true stop_line_margin: 1.0 # [m] backward_length: 15.0 # [m] ignore_width_from_center_line: 0.7 # [m] diff --git a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp index 6ca5d7e3cafa0..58d324ec06765 100644 --- a/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/blind_spot/scene.hpp @@ -98,8 +98,9 @@ class BlindSpotModule : public SceneModuleInterface public: struct PlannerParam { - double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary - double backward_length; //! distance[m] from closest path point to the edge of beginning point + bool use_pass_judge_line; //! distance which ego can stop with max brake + double stop_line_margin; //! distance from auto-generated stopline to detection_area boundary + double backward_length; //! distance[m] from closest path point to the edge of beginning point double ignore_width_from_center_line; //! ignore width from center line from detection_area double max_future_movement_time; //! maximum time[second] for considering future movement of object @@ -216,7 +217,7 @@ class BlindSpotModule : public SceneModuleInterface */ int insertPoint( const int insert_idx_ip, const autoware_auto_planning_msgs::msg::PathWithLaneId path_ip, - autoware_auto_planning_msgs::msg::PathWithLaneId * path) const; + autoware_auto_planning_msgs::msg::PathWithLaneId * path, bool & is_point_inserted) const; /** * @brief Calculate first path index that is conflicting lanelets. diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp index 6189c003eb926..55295f722db1d 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/manager.cpp @@ -59,6 +59,7 @@ BlindSpotModuleManager::BlindSpotModuleManager(rclcpp::Node & node) : SceneModuleManagerInterface(node, getModuleName()) { const std::string ns(getModuleName()); + planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false); planner_param_.stop_line_margin = node.declare_parameter(ns + ".stop_line_margin", 1.0); planner_param_.backward_length = node.declare_parameter(ns + ".backward_length", 15.0); planner_param_.ignore_width_from_center_line = diff --git a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp index 1a4bff3b0dc0b..f0811f35012b9 100644 --- a/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/blind_spot/scene.cpp @@ -89,7 +89,7 @@ bool BlindSpotModule::modifyPathVelocity( return false; } - if (stop_line_idx <= 0 || pass_judge_line_idx <= 0) { + if (stop_line_idx <= 0 || (planner_param_.use_pass_judge_line && pass_judge_line_idx <= 0)) { RCLCPP_DEBUG( logger_, "[Blind Spot] stop line or pass judge line is at path[0], ignore planning."); *path = input_path; // reset path @@ -118,10 +118,12 @@ bool BlindSpotModule::modifyPathVelocity( geometry_msgs::msg::Pose pass_judge_line = path->points.at(pass_judge_line_idx).point.pose; is_over_pass_judge_line = util::isAheadOf(current_pose.pose, pass_judge_line); } - if (current_state == State::GO && is_over_pass_judge_line) { - RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); - *path = input_path; // reset path - return true; // no plan needed. + if (planner_param_.use_pass_judge_line) { + if (current_state == State::GO && is_over_pass_judge_line) { + RCLCPP_DEBUG(logger_, "over the pass judge line. no plan needed."); + *path = input_path; // reset path + return true; // no plan needed. + } } /* get dynamic object */ @@ -230,7 +232,8 @@ bool BlindSpotModule::generateStopLine( } /* insert stop_point */ - *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path); + [[maybe_unused]] bool is_stop_point_inserted = true; + *stop_line_idx = insertPoint(stop_idx_ip, path_ip, path, is_stop_point_inserted); /* if another stop point exist before intersection stop_line, disable judge_line. */ bool has_prior_stopline = false; @@ -248,9 +251,12 @@ bool BlindSpotModule::generateStopLine( if (has_prior_stopline || stop_idx_ip == pass_judge_idx_ip) { *pass_judge_line_idx = *stop_line_idx; } else { + bool is_pass_judge_point_inserted = true; //! insertPoint check if there is no duplicated point - *pass_judge_line_idx = insertPoint(pass_judge_idx_ip, path_ip, path); - ++(*stop_line_idx); // stop index is incremented by judge line insertion + *pass_judge_line_idx = + insertPoint(pass_judge_idx_ip, path_ip, path, is_pass_judge_point_inserted); + if (is_pass_judge_point_inserted) + ++(*stop_line_idx); // stop index is incremented by judge line insertion } RCLCPP_DEBUG( @@ -287,7 +293,7 @@ void BlindSpotModule::cutPredictPathWithDuration( int BlindSpotModule::insertPoint( const int insert_idx_ip, const autoware_auto_planning_msgs::msg::PathWithLaneId path_ip, - autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path) const + autoware_auto_planning_msgs::msg::PathWithLaneId * inout_path, bool & is_point_inserted) const { double insert_point_s = 0.0; for (int i = 1; i <= insert_idx_ip; i++) { @@ -296,7 +302,8 @@ int BlindSpotModule::insertPoint( } int insert_idx = -1; // initialize with epsilon so that comparison with insert_point_s = 0.0 would work - double accum_s = 1e-6; + constexpr double eps = 1e-4; + double accum_s = eps * 2.0; for (size_t i = 1; i < inout_path->points.size(); i++) { accum_s += planning_utils::calcDist2d( inout_path->points[i].point.pose.position, inout_path->points[i - 1].point.pose.position); @@ -311,15 +318,17 @@ int BlindSpotModule::insertPoint( // copy from previous point inserted_point = inout_path->points.at(std::max(insert_idx - 1, 0)); inserted_point.point.pose = path_ip.points[insert_idx_ip].point.pose; - constexpr double min_dist = 0.001; + constexpr double min_dist = eps; // to make sure path point is forward insert index //! avoid to insert duplicated point if ( planning_utils::calcDist2d(inserted_point, inout_path->points.at(insert_idx).point) < min_dist) { inout_path->points.at(insert_idx).point.longitudinal_velocity_mps = 0.0; + is_point_inserted = false; return insert_idx; } inout_path->points.insert(it, inserted_point); + is_point_inserted = true; } return insert_idx; } @@ -467,7 +476,7 @@ boost::optional BlindSpotModule::generateBlindSpotPolygons( const auto conflict_area = lanelet::utils::getPolygonFromArcLength( blind_spot_lanelets, current_arc.length, stop_line_arc.length); const auto detection_area = lanelet::utils::getPolygonFromArcLength( - blind_spot_lanelets, detection_area_start_length, current_arc.length); + blind_spot_lanelets, detection_area_start_length, stop_line_arc.length); BlindSpotPolygons blind_spot_polygons; blind_spot_polygons.conflict_area = conflict_area;