feat(static_obstacle_avoidance): force deactivation (autowarefoundation#8288)

* add force cancel function

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* fix format

Signed-off-by: Go Sakayori <gsakayori@gmail.com>

* fix json schema

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* fix spelling

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

* fix

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>

---------

Signed-off-by: Go Sakayori <go.sakayori@tier4.jp>
Signed-off-by: Go Sakayori <gsakayori@gmail.com>

Author: go-sakayori

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml

@@ -243,6 +243,8 @@
       # For cancel maneuver
       cancel:
         enable: true                                    # [-]
+        force:
+          duration_time: 2.0                            # [s]
 
       # For yield maneuver
       yield:

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp

@@ -107,6 +107,8 @@ struct AvoidanceParameters
   // if this param is true, it reverts avoidance path when the path is no longer needed.
   bool enable_cancel_maneuver{false};
 
+  double force_deactivate_duration_time{0.0};
+
   // enable avoidance for all parking vehicle
   std::string policy_ambiguous_vehicle{"ignore"};
 
@@ -581,6 +583,8 @@ struct AvoidancePlanningData
 
   bool found_avoidance_path{false};
 
+  bool force_deactivated{false};
+
   double to_stop_line{std::numeric_limits<double>::max()};
 
   double to_start_point{std::numeric_limits<double>::lowest()};

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp

@@ -301,6 +301,8 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   {
     const std::string ns = "avoidance.cancel.";
     p.enable_cancel_maneuver = getOrDeclareParameter<bool>(*node, ns + "enable");
+    p.force_deactivate_duration_time =
+      getOrDeclareParameter<double>(*node, ns + "force.duration_time");
   }
 
   // yield

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp

@@ -477,6 +477,9 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
   mutable std::vector<AvoidanceDebugMsg> debug_avoidance_initializer_for_shift_line_;
 
   mutable rclcpp::Time debug_avoidance_initializer_for_shift_line_time_;
+
+  bool force_deactivated_{false};
+  rclcpp::Time last_deactivation_triggered_time_;
 };
 
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json

@@ -1263,6 +1263,16 @@
               "type": "boolean",
               "description": "Flag to enable cancel maneuver.",
               "default": "true"
+            },
+            "force": {
+              "type": "object",
+              "properties": {
+                "duration_time": {
+                  "type": "number",
+                  "description": "force deactivate duration time",
+                  "default": 2.0
+                }
+              }
             }
           },
           "required": ["enable"],

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp

@@ -182,6 +182,11 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
     updateParam<bool>(parameters, ns + "consider_rear_overhang", p->consider_rear_overhang);
   }
 
+  {
+    const std::string ns = "avoidance.cancel.";
+    updateParam<double>(parameters, ns + "force.duration_time", p->force_deactivate_duration_time);
+  }
+
   {
     const std::string ns = "avoidance.stop.";
     updateParam<double>(parameters, ns + "max_distance", p->stop_max_distance);

planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp

@@ -570,6 +570,24 @@ void StaticObstacleAvoidanceModule::fillEgoStatus(
     return;
   }
 
+  const auto registered_sl_force_deactivated =
+    [&](const std::string & direction, const RegisteredShiftLineArray shift_line_array) {
+      return std::any_of(
+        shift_line_array.begin(), shift_line_array.end(), [&](const auto & shift_line) {
+          return rtc_interface_ptr_map_.at(direction)->isForceDeactivated(shift_line.uuid);
+        });
+    };
+
+  const auto is_force_deactivated = registered_sl_force_deactivated("left", left_shift_array_) ||
+                                    registered_sl_force_deactivated("right", right_shift_array_);
+  if (is_force_deactivated && can_yield_maneuver) {
+    data.yield_required = true;
+    data.safe_shift_line = data.new_shift_line;
+    data.force_deactivated = true;
+    RCLCPP_INFO(getLogger(), "this module is force deactivated. wait until reactivation");
+    return;
+  }
+
   /**
    * If the avoidance path is safe, use unapproved_new_sl for avoidance path generation.
    */
@@ -755,6 +773,10 @@ bool StaticObstacleAvoidanceModule::isSafePath(
   universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   const auto & p = planner_data_->parameters;
 
+  if (force_deactivated_) {
+    return false;
+  }
+
   if (!parameters_->enable_safety_check) {
     return true;  // if safety check is disabled, it always return safe.
   }
@@ -1366,6 +1388,19 @@ void StaticObstacleAvoidanceModule::updateData()
   }
 
   safe_ = avoid_data_.safe;
+
+  if (!force_deactivated_) {
+    last_deactivation_triggered_time_ = clock_->now();
+    force_deactivated_ = avoid_data_.force_deactivated;
+    return;
+  }
+
+  if (
+    (clock_->now() - last_deactivation_triggered_time_).seconds() >
+    parameters_->force_deactivate_duration_time) {
+    RCLCPP_INFO(getLogger(), "The force deactivation is released");
+    force_deactivated_ = false;
+  }
 }
 
 void StaticObstacleAvoidanceModule::processOnEntry()