feat(behavior_path_planner): remove redundant process (autowarefounda…

…tion#4077) Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
tier4 · Jun 26, 2023 · be7e8f6 · be7e8f6
1 parent 4dc7ade
commit be7e8f6
Show file tree

Hide file tree

Showing 4 changed files with 35 additions and 15 deletions.
diff --git a/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
@@ -83,6 +83,8 @@ class LaneChangeBase
 
   virtual bool hasFinishedAbort() const = 0;
 
+  virtual bool isLaneChangeRequired() const = 0;
+
   virtual bool isAbortState() const = 0;
 
   virtual bool isAbleToReturnCurrentLane() const = 0;
@@ -193,7 +195,7 @@ class LaneChangeBase
   virtual bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,
     const lanelet::ConstLanelets & target_lanelets, Direction direction,
-    LaneChangePaths * candidate_paths) const = 0;
+    LaneChangePaths * candidate_paths, const bool check_safety) const = 0;
 
   virtual std::vector<DrivableLanes> getDrivableLanes() const = 0;
 

diff --git a/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
@@ -81,6 +81,8 @@ class NormalLaneChange : public LaneChangeBase
 
   bool isAbortState() const override;
 
+  bool isLaneChangeRequired() const override;
+
 protected:
   lanelet::ConstLanelets getCurrentLanes() const override;
 
@@ -96,7 +98,7 @@ class NormalLaneChange : public LaneChangeBase
   bool getLaneChangePaths(
     const lanelet::ConstLanelets & original_lanelets,
     const lanelet::ConstLanelets & target_lanelets, Direction direction,
-    LaneChangePaths * candidate_paths) const override;
+    LaneChangePaths * candidate_paths, const bool check_safety = true) const override;
 
   std::vector<DrivableLanes> getDrivableLanes() const override;
 

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
@@ -67,22 +67,12 @@ bool LaneChangeInterface::isExecutionRequested() const
     return true;
   }
 
-  LaneChangePath selected_path;
-  module_type_->setPreviousModulePaths(
-    getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
-  const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path);
-
-  return found_valid_path;
+  return module_type_->isLaneChangeRequired();
 }
 
 bool LaneChangeInterface::isExecutionReady() const
 {
-  LaneChangePath selected_path;
-  module_type_->setPreviousModulePaths(
-    getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
-  const auto [found_valid_path, found_safe_path] = module_type_->getSafePath(selected_path);
-
-  return found_safe_path;
+  return module_type_->isSafe();
 }
 
 ModuleStatus LaneChangeInterface::updateState()

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
@@ -89,6 +89,28 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
   return {true, found_safe_path};
 }
 
+bool NormalLaneChange::isLaneChangeRequired() const
+{
+  const auto current_lanes = getCurrentLanes();
+
+  if (current_lanes.empty()) {
+    return false;
+  }
+
+  const auto target_lanes = getLaneChangeLanes(current_lanes, direction_);
+
+  if (target_lanes.empty()) {
+    return false;
+  }
+
+  // find candidate paths
+  LaneChangePaths valid_paths{};
+  [[maybe_unused]] const auto found_safe_path =
+    getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths, false);
+
+  return !valid_paths.empty();
+}
+
 LaneChangePath NormalLaneChange::getLaneChangePath() const
 {
   return isAbortState() ? *abort_path_ : status_.lane_change_path;
@@ -397,7 +419,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment(
 
 bool NormalLaneChange::getLaneChangePaths(
   const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
-  Direction direction, LaneChangePaths * candidate_paths) const
+  Direction direction, LaneChangePaths * candidate_paths, const bool check_safety) const
 {
   object_debug_.clear();
   if (original_lanelets.empty() || target_lanelets.empty()) {
@@ -648,6 +670,10 @@ bool NormalLaneChange::getLaneChangePaths(
       }
       candidate_paths->push_back(*candidate_path);
 
+      if (!check_safety) {
+        return false;
+      }
+
       const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe(
         *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(),
         common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration,