feat(lane_change, avoidance): add force execution and cancel

planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp

@@ -59,6 +59,8 @@ class RTCInterface
   void removeExpiredCooperateStatus();
   void clearCooperateStatus();
   bool isActivated(const UUID & uuid) const;
+  bool isForceActivated(const UUID & uuid) const;
+  bool isForceDeactivated(const UUID & UUID) const;
   bool isRegistered(const UUID & uuid) const;
   bool isRTCEnabled(const UUID & uuid) const;
   bool isTerminated(const UUID & uuid) const;

planning/autoware_rtc_interface/src/rtc_interface.cpp

@@ -345,6 +345,54 @@ bool RTCInterface::isActivated(const UUID & uuid) const
   return false;
 }
 
+bool RTCInterface::isForceActivated(const UUID & uuid) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  const auto itr = std::find_if(
+    registered_status_.statuses.begin(), registered_status_.statuses.end(),
+    [uuid](const auto & s) { return s.uuid == uuid; });
+
+  if (itr != registered_status_.statuses.end()) {
+    if (itr->state.type != State::WAITING_FOR_EXECUTION && itr->state.type != State::RUNNING) {
+      return false;
+    }
+    if (itr->command_status.type == Command::ACTIVATE && !itr->safe) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(),
+    "[isForceActivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl);
+  return false;
+}
+
+bool RTCInterface::isForceDeactivated(const UUID & uuid) const
+{
+  std::lock_guard<std::mutex> lock(mutex_);
+  const auto itr = std::find_if(
+    registered_status_.statuses.begin(), registered_status_.statuses.end(),
+    [uuid](const auto & s) { return s.uuid == uuid; });
+
+  if (itr != registered_status_.statuses.end()) {
+    if (itr->state.type != State::RUNNING) {
+      return false;
+    }
+    if (itr->command_status.type == Command::DEACTIVATE && itr->safe && !itr->auto_mode) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(),
+    "[isForceDeactivated] uuid : " << uuid_to_string(uuid) << " is not found" << std::endl);
+  return false;
+}
+
 bool RTCInterface::isRegistered(const UUID & uuid) const
 {
   std::lock_guard<std::mutex> lock(mutex_);

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -123,9 +123,18 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   } else {
     const auto path =
       assignToCandidate(module_type_->getLaneChangePath(), module_type_->getEgoPosition());
-    updateRTCStatus(
-      path.start_distance_to_path_change, path.finish_distance_to_path_change, true,
-      State::RUNNING);
+    const auto force_activated = std::any_of(
+      rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
+      [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
+    if (!force_activated) {
+      updateRTCStatus(
+        path.start_distance_to_path_change, path.finish_distance_to_path_change, true,
+        State::RUNNING);
+    } else {
+      updateRTCStatus(
+        path.start_distance_to_path_change, path.finish_distance_to_path_change, false,
+        State::RUNNING);
+    }
   }
 
   return output;
@@ -226,6 +235,15 @@ bool LaneChangeInterface::canTransitFailureState()
   updateDebugMarker();
   log_debug_throttled(__func__);
 
+  const auto force_activated = std::any_of(
+    rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
+    [&](const auto & rtc) { return rtc.second->isForceActivated(uuid_map_.at(rtc.first)); });
+
+  if (force_activated) {
+    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed");
+    return false;
+  }
+
   if (module_type_->isAbortState() && !module_type_->hasFinishedAbort()) {
     log_debug_throttled("Abort process has on going.");
     return false;
@@ -256,6 +274,19 @@ bool LaneChangeInterface::canTransitFailureState()
       return true;
     }
 
+    const auto force_deactivated = std::any_of(
+      rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(),
+      [&](const auto & rtc) { return rtc.second->isForceDeactivated(uuid_map_.at(rtc.first)); });
+
+    if (force_deactivated && module_type_->isAbleToReturnCurrentLane()) {
+      log_debug_throttled("Cancel lane change due to force deactivation");
+      module_type_->toCancelState();
+      updateRTCStatus(
+        std::numeric_limits<double>::lowest(), std::numeric_limits<double>::lowest(), true,
+        State::FAILED);
+      return true;
+    }
+
     if (post_process_safety_status_.is_safe) {
       log_debug_throttled("Can't transit to failure state. Ego is on prepare, and it's safe.");
       return false;

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

@@ -118,8 +118,16 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
       const double start_distance = autoware::motion_utils::calcSignedArcLength(
         path.points, ego_idx, left_shift.start_pose.position);
       const double finish_distance = start_distance + left_shift.relative_longitudinal;
-      rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
-        left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
+
+      // If force activated keep safety to false
+      if (rtc_interface_ptr_map_.at("left")->isForceActivated(left_shift.uuid)) {
+        rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
+          left_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now());
+      } else {
+        rtc_interface_ptr_map_.at("left")->updateCooperateStatus(
+          left_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
+      }
+
       if (finish_distance > -1.0e-03) {
         steering_factor_interface_ptr_->updateSteeringFactor(
           {left_shift.start_pose, left_shift.finish_pose}, {start_distance, finish_distance},
@@ -131,8 +139,15 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
       const double start_distance = autoware::motion_utils::calcSignedArcLength(
         path.points, ego_idx, right_shift.start_pose.position);
       const double finish_distance = start_distance + right_shift.relative_longitudinal;
-      rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
-        right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
+
+      if (rtc_interface_ptr_map_.at("right")->isForceActivated(right_shift.uuid)) {
+        rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
+          right_shift.uuid, false, State::RUNNING, start_distance, finish_distance, clock_->now());
+      } else {
+        rtc_interface_ptr_map_.at("right")->updateCooperateStatus(
+          right_shift.uuid, true, State::RUNNING, start_distance, finish_distance, clock_->now());
+      }
+
       if (finish_distance > -1.0e-03) {
         steering_factor_interface_ptr_->updateSteeringFactor(
           {right_shift.start_pose, right_shift.finish_pose}, {start_distance, finish_distance},
@@ -462,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.
    */
@@ -579,6 +597,45 @@ void StaticObstacleAvoidanceModule::fillEgoStatus(
     return;
   }
 
+  auto candidate_sl_force_activated = [&](const std::string & direction) {
+    // If statement to avoid unnecessary warning occurring from isForceActivated function
+    if (candidate_uuid_ == uuid_map_.at(direction)) {
+      if (rtc_interface_ptr_map_.at(direction)->isForceActivated(candidate_uuid_)) {
+        return true;
+      }
+    }
+    return false;
+  };
+
+  auto registered_sl_force_activated =
+    [&](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)->isForceActivated(shift_line.uuid);
+        });
+    };
+
+  /**
+   * Check if the candidate avoidance path is force activated
+   */
+  if (candidate_sl_force_activated("left") || candidate_sl_force_activated("right")) {
+    data.yield_required = false;
+    data.safe_shift_line = data.new_shift_line;
+    return;
+  }
+
+  /**
+   * Check if any registered shift line is force activated
+   */
+  if (
+    registered_sl_force_activated("left", left_shift_array_) ||
+    registered_sl_force_activated("right", right_shift_array_)) {
+    data.yield_required = false;
+    data.safe_shift_line = data.new_shift_line;
+    RCLCPP_WARN_THROTTLE(getLogger(), *clock_, 5000, "unsafe but force executed");
+    return;
+  }
+
   /**
    * If the yield maneuver is disabled, use unapproved_new_sl for avoidance path generation even if
    * the shift line is unsafe.
@@ -716,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.
   }
@@ -1327,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()