fix(lane_change): safety check not working when cancel disabled

Signed-off-by: Muhammad Zulfaqar <zulfaqar.azmi@tier4.jp>
tier4 · May 12, 2023 · 198b2ee · 198b2ee
1 parent 789fee7
commit 198b2ee
Show file tree

Hide file tree

Showing 3 changed files with 5 additions and 5 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
@@ -150,6 +150,8 @@ class LaneChangeBase
 
   bool isValidPath() const { return status_.is_valid_path; }
 
+  bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; }
+
   std_msgs::msg::Header getRouteHeader() const
   {
     return planner_data_->route_handler->getRouteHeader();

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
@@ -14,6 +14,7 @@
 
 #include "behavior_path_planner/scene_module/lane_change/interface.hpp"
 
+#include "behavior_path_planner/module_status.hpp"
 #include "behavior_path_planner/scene_module/scene_module_visitor.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
 #include "behavior_path_planner/utils/path_utils.hpp"
@@ -97,7 +98,8 @@ ModuleStatus LaneChangeInterface::updateState()
   }
 
   if (module_type_->isCancelConditionSatisfied()) {
-    current_state_ = ModuleStatus::FAILURE;
+    current_state_ =
+      module_type_->isCancelEnabled() ? ModuleStatus::FAILURE : ModuleStatus::RUNNING;
     return current_state_;
   }
 

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
@@ -155,10 +155,6 @@ bool NormalLaneChange::isCancelConditionSatisfied()
 {
   current_lane_change_state_ = LaneChangeStates::Normal;
 
-  if (!lane_change_parameters_->enable_cancel_lane_change) {
-    return false;
-  }
-
   Pose ego_pose_before_collision;
   const auto is_path_safe = isApprovedPathSafe(ego_pose_before_collision);