From db407e125ee9b2f264a0bbf34c51fcda385799f5 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 8 Nov 2024 18:25:37 +0900 Subject: [PATCH] add feature Signed-off-by: Yuki Takagi --- .../scene_module/detection_area/scene.hpp | 5 ++++- .../scene_module/detection_area/manager.cpp | 4 ++++ .../src/scene_module/detection_area/scene.cpp | 19 ++++++++++++++----- 3 files changed, 22 insertions(+), 6 deletions(-) diff --git a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp index 38f977dfc902f..35493c6bb34ca 100644 --- a/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp +++ b/planning/behavior_velocity_planner/include/scene_module/detection_area/scene.hpp @@ -58,6 +58,8 @@ class DetectionAreaModule : public SceneModuleInterface double dead_line_margin; bool use_pass_judge_line; double state_clear_time; + double distance_to_judge_over_stop_line; + bool suppress_pass_judge_when_stopping; }; public: @@ -82,7 +84,8 @@ class DetectionAreaModule : public SceneModuleInterface bool isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double margin) const; bool hasEnoughBrakingDistance( const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const; diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp index b508dbf353d6d..be46ff74cc1ce 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/manager.cpp @@ -69,6 +69,10 @@ DetectionAreaModuleManager::DetectionAreaModuleManager(rclcpp::Node & node) planner_param_.dead_line_margin = node.declare_parameter(ns + ".dead_line_margin", 5.0); planner_param_.use_pass_judge_line = node.declare_parameter(ns + ".use_pass_judge_line", false); planner_param_.state_clear_time = node.declare_parameter(ns + ".state_clear_time", 2.0); + planner_param_.distance_to_judge_over_stop_line = + node.declare_parameter(ns + ".distance_to_judge_over_stop_line", 0.0); + planner_param_.suppress_pass_judge_when_stopping = + node.declare_parameter(ns + ".suppress_pass_judge_when_stopping", false); } void DetectionAreaModuleManager::launchNewModules( diff --git a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp index 1ca450dfb1a08..3c2aa3fe75fa5 100644 --- a/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp +++ b/planning/behavior_velocity_planner/src/scene_module/detection_area/scene.cpp @@ -231,7 +231,11 @@ bool DetectionAreaModule::modifyPathVelocity( // Check state if (canClearStopState()) { - state_ = State::GO; + if ( + !planner_param_.suppress_pass_judge_when_stopping || + planner_data_->current_velocity->twist.linear.x > 1e-3) { + state_ = State::GO; + } last_obstacle_found_time_ = {}; return true; } @@ -252,7 +256,7 @@ bool DetectionAreaModule::modifyPathVelocity( const auto & dead_line_pose = dead_line_point->second; debug_data_.dead_line_poses.push_back(dead_line_pose); - if (isOverLine(original_path, self_pose, dead_line_pose)) { + if (isOverLine(original_path, self_pose, dead_line_pose, 0.0)) { RCLCPP_WARN(logger_, "[detection_area] vehicle is over dead line"); return true; } @@ -268,7 +272,10 @@ bool DetectionAreaModule::modifyPathVelocity( const auto & stop_pose = stop_point->second; // Ignore objects detected after stop_line if not in STOP state - if (state_ != State::STOP && isOverLine(original_path, self_pose, stop_pose)) { + if ( + state_ != State::STOP && + isOverLine( + original_path, self_pose, stop_pose, -planner_param_.distance_to_judge_over_stop_line)) { return true; } @@ -355,7 +362,8 @@ bool DetectionAreaModule::canClearStopState() const bool DetectionAreaModule::isOverLine( const autoware_auto_planning_msgs::msg::PathWithLaneId & path, - const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose) const + const geometry_msgs::msg::Pose & self_pose, const geometry_msgs::msg::Pose & line_pose, + const double margin) const { const PointWithSearchRangeIndex src_point_with_search_range_index = planning_utils::findFirstNearSearchRangeIndex(path.points, self_pose.position); @@ -363,7 +371,8 @@ bool DetectionAreaModule::isOverLine( line_pose.position, planning_utils::getPathIndexRangeIncludeLaneId(path, lane_id_)}; return planning_utils::calcSignedArcLengthWithSearchIndex( - path.points, src_point_with_search_range_index, dst_point_with_search_range_index) < 0; + path.points, src_point_with_search_range_index, dst_point_with_search_range_index) < + margin; } bool DetectionAreaModule::hasEnoughBrakingDistance(