add feature

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
tier4 · Nov 15, 2024 · b7f9c8c · b7f9c8c
1 parent 4f97fc0
commit b7f9c8c
Show file tree

Hide file tree

Showing 3 changed files with 22 additions and 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
@@ -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
@@ -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
@@ -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,15 +362,17 @@ 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);
   const PointWithSearchRangeIndex dst_point_with_search_range_index = {
     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(