Merge branch 'beta/v0.7.0' into feat/pullover_hotfix

tier4 · Jan 17, 2023 · 557270f · 557270f
2 parents e4678d9 + b9fce1f
commit 557270f
Show file tree

Hide file tree

Showing 3 changed files with 9 additions and 3 deletions.
diff --git a/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/...ath_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp
@@ -169,6 +169,7 @@ class LaneChangeModule : public SceneModuleInterface
   void updateSteeringFactorPtr(
     const CandidateOutput & output, const LaneChangePath & selected_path) const;
   bool isSafe() const;
+  bool isValidPath() const;
   bool isValidPath(const PathWithLaneId & path) const;
   bool isNearEndOfLane() const;
   bool isCurrentSpeedLow() const;

diff --git a/...lanner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp b/...lanner/include/behavior_path_planner/scene_module/lane_change/lane_change_module_data.hpp
@@ -55,6 +55,7 @@ struct LaneChangeStatus
   std::vector<uint64_t> lane_follow_lane_ids;
   std::vector<uint64_t> lane_change_lane_ids;
   bool is_safe;
+  bool is_valid_path = true;
   double start_distance;
 };
 }  // namespace behavior_path_planner

diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
@@ -59,7 +59,7 @@ BehaviorModuleOutput LaneChangeModule::run()
   is_activated_ = isActivated();
   auto output = plan();
 
-  if (!isSafe()) {
+  if (!isValidPath()) {
     current_state_ = BT::NodeStatus::SUCCESS;  // for breaking loop
     return output;
   }
@@ -118,7 +118,7 @@ bool LaneChangeModule::isExecutionReady() const
 BT::NodeStatus LaneChangeModule::updateState()
 {
   RCLCPP_DEBUG(getLogger(), "LANE_CHANGE updateState");
-  if (!isSafe()) {
+  if (!isValidPath()) {
     current_state_ = BT::NodeStatus::SUCCESS;
     return current_state_;
   }
@@ -148,8 +148,10 @@ BehaviorModuleOutput LaneChangeModule::plan()
   auto path = status_.lane_change_path.path;
 
   if (!isValidPath(path)) {
-    status_.is_safe = false;
+    status_.is_valid_path = false;
     return BehaviorModuleOutput{};
+  } else {
+    status_.is_valid_path = true;
   }
   generateExtendedDrivableArea(path);
 
@@ -374,6 +376,8 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(
 
 bool LaneChangeModule::isSafe() const { return status_.is_safe; }
 
+bool LaneChangeModule::isValidPath() const { return status_.is_valid_path; }
+
 bool LaneChangeModule::isValidPath(const PathWithLaneId & path) const
 {
   const auto & route_handler = planner_data_->route_handler;