diff --git a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp
index 62f9a55c75455..23f7f0b4b36c5 100644
--- a/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp
+++ b/planning/autoware_rtc_interface/include/autoware/rtc_interface/rtc_interface.hpp
@@ -61,8 +61,10 @@ class RTCInterface
   bool isActivated(const UUID & uuid) const;
   bool isRegistered(const UUID & uuid) const;
   bool isRTCEnabled(const UUID & uuid) const;
+  bool isTerminated(const UUID & uuid) const;
   void lockCommandUpdate();
   void unlockCommandUpdate();
+  void print() const;
 
 private:
   void onCooperateCommandService(
diff --git a/planning/autoware_rtc_interface/src/rtc_interface.cpp b/planning/autoware_rtc_interface/src/rtc_interface.cpp
index 985e38b64f2bd..44678077d5dd1 100644
--- a/planning/autoware_rtc_interface/src/rtc_interface.cpp
+++ b/planning/autoware_rtc_interface/src/rtc_interface.cpp
@@ -20,7 +20,7 @@ namespace
 {
 using tier4_rtc_msgs::msg::Module;
 
-std::string to_string(const unique_identifier_msgs::msg::UUID & uuid)
+std::string uuid_to_string(const unique_identifier_msgs::msg::UUID & uuid)
 {
   std::stringstream ss;
   for (auto i = 0; i < 16; ++i) {
@@ -29,6 +29,34 @@ std::string to_string(const unique_identifier_msgs::msg::UUID & uuid)
   return ss.str();
 }
 
+std::string command_to_string(const uint8_t type)
+{
+  if (type == tier4_rtc_msgs::msg::Command::ACTIVATE) {
+    return "ACTIVATE";
+  } else if (type == tier4_rtc_msgs::msg::Command::DEACTIVATE) {
+    return "DEACTIVATE";
+  }
+
+  throw std::domain_error("invalid rtc command.");
+}
+
+std::string state_to_string(const uint8_t type)
+{
+  if (type == tier4_rtc_msgs::msg::State::WAITING_FOR_EXECUTION) {
+    return "WAITING_FOR_EXECUTION";
+  } else if (type == tier4_rtc_msgs::msg::State::RUNNING) {
+    return "RUNNING";
+  } else if (type == tier4_rtc_msgs::msg::State::ABORTING) {
+    return "ABORTING";
+  } else if (type == tier4_rtc_msgs::msg::State::SUCCEEDED) {
+    return "SUCCEEDED";
+  } else if (type == tier4_rtc_msgs::msg::State::FAILED) {
+    return "FAILED";
+  }
+
+  throw std::domain_error("invalid rtc state.");
+}
+
 Module getModuleType(const std::string & module_name)
 {
   Module module;
@@ -158,14 +186,14 @@ std::vector<CooperateResponse> RTCInterface::validateCooperateCommands(
       } else {
         RCLCPP_WARN_STREAM(
           getLogger(), "[validateCooperateCommands] uuid : "
-                         << to_string(command.uuid)
+                         << uuid_to_string(command.uuid)
                          << " state is not WAITING_FOR_EXECUTION or RUNNING. state : "
                          << itr->state.type << std::endl);
         response.success = false;
       }
     } else {
       RCLCPP_WARN_STREAM(
-        getLogger(), "[validateCooperateCommands] uuid : " << to_string(command.uuid)
+        getLogger(), "[validateCooperateCommands] uuid : " << uuid_to_string(command.uuid)
                                                            << " is not found." << std::endl);
       response.success = false;
     }
@@ -262,7 +290,7 @@ void RTCInterface::removeCooperateStatus(const UUID & uuid)
 
   RCLCPP_WARN_STREAM(
     getLogger(),
-    "[removeCooperateStatus] uuid : " << to_string(uuid) << " is not found." << std::endl);
+    "[removeCooperateStatus] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
 }
 
 void RTCInterface::removeStoredCommand(const UUID & uuid)
@@ -313,7 +341,7 @@ bool RTCInterface::isActivated(const UUID & uuid) const
   }
 
   RCLCPP_WARN_STREAM(
-    getLogger(), "[isActivated] uuid : " << to_string(uuid) << " is not found." << std::endl);
+    getLogger(), "[isActivated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
   return false;
 }
 
@@ -338,7 +366,23 @@ bool RTCInterface::isRTCEnabled(const UUID & uuid) const
   }
 
   RCLCPP_WARN_STREAM(
-    getLogger(), "[isRTCEnabled] uuid : " << to_string(uuid) << " is not found." << std::endl);
+    getLogger(), "[isRTCEnabled] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
+  return is_auto_mode_enabled_;
+}
+
+bool RTCInterface::isTerminated(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](auto & s) { return s.uuid == uuid; });
+
+  if (itr != registered_status_.statuses.end()) {
+    return itr->state.type == State::SUCCEEDED || itr->state.type == State::FAILED;
+  }
+
+  RCLCPP_WARN_STREAM(
+    getLogger(), "[isTerminated] uuid : " << uuid_to_string(uuid) << " is not found." << std::endl);
   return is_auto_mode_enabled_;
 }
 
@@ -363,4 +407,16 @@ bool RTCInterface::isLocked() const
   return is_locked_;
 }
 
+void RTCInterface::print() const
+{
+  RCLCPP_INFO_STREAM(getLogger(), "---print rtc cooperate statuses---" << std::endl);
+  for (const auto status : registered_status_.statuses) {
+    RCLCPP_INFO_STREAM(
+      getLogger(), "uuid:" << uuid_to_string(status.uuid)
+                           << " command:" << command_to_string(status.command_status.type)
+                           << std::boolalpha << " safe:" << status.safe
+                           << " state:" << state_to_string(status.state.type) << std::endl);
+  }
+}
+
 }  // namespace autoware::rtc_interface
diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp
index 02b90186d9b2f..8096d2944ee2b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/manager.cpp
@@ -137,9 +137,11 @@ void AvoidanceByLaneChangeModuleManager::init(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.policy_ambiguous_vehicle = getOrDeclareParameter<std::string>(*node, ns + "policy");
+    p.wait_and_see_target_behaviors =
+      getOrDeclareParameter<std::vector<std::string>>(*node, ns + "wait_and_see.target_behaviors");
+    p.wait_and_see_th_closest_distance =
+      getOrDeclareParameter<double>(*node, ns + "wait_and_see.th_closest_distance");
     p.time_threshold_for_ambiguous_vehicle =
       getOrDeclareParameter<double>(*node, ns + "condition.th_stopped_time");
     p.distance_threshold_for_ambiguous_vehicle =
diff --git a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
index 7571a1f61d1c1..a7a7b30e4eec0 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -114,10 +114,8 @@ void AvoidanceByLaneChange::updateSpecialData()
                    : Direction::RIGHT;
   }
 
-  utils::static_obstacle_avoidance::updateRegisteredObject(
-    registered_objects_, avoidance_data_.target_objects, p);
-  utils::static_obstacle_avoidance::compensateDetectionLost(
-    registered_objects_, avoidance_data_.target_objects, avoidance_data_.other_objects);
+  utils::static_obstacle_avoidance::compensateLostTargetObjects(
+    registered_objects_, avoidance_data_, clock_.now(), planner_data_, p);
 
   std::sort(
     avoidance_data_.target_objects.begin(), avoidance_data_.target_objects.end(),
diff --git a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp
index 95eb82e0b4c23..c4a05d171654d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_interface.hpp
@@ -335,8 +335,15 @@ class SceneModuleInterface
   {
     return std::any_of(
       rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) {
-        return rtc.second->isRegistered(uuid_map_.at(rtc.first)) &&
-               rtc.second->isActivated(uuid_map_.at(rtc.first));
+        if (!rtc.second->isRegistered(uuid_map_.at(rtc.first))) {
+          return false;
+        }
+
+        if (rtc.second->isTerminated(uuid_map_.at(rtc.first))) {
+          return true;
+        }
+
+        return rtc.second->isActivated(uuid_map_.at(rtc.first));
       });
   }
 
@@ -345,7 +352,8 @@ class SceneModuleInterface
     return std::any_of(
       rtc_interface_ptr_map_.begin(), rtc_interface_ptr_map_.end(), [&](const auto & rtc) {
         return rtc.second->isRegistered(uuid_map_.at(rtc.first)) &&
-               !rtc.second->isActivated(uuid_map_.at(rtc.first));
+               !rtc.second->isActivated(uuid_map_.at(rtc.first)) &&
+               !rtc.second->isTerminated(uuid_map_.at(rtc.first));
       });
   }
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md
index ddd0d473d2cbf..d32ab36301810 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/README.md
@@ -401,6 +401,15 @@ else (\n no)
 #00FFB1 :IGNORE;
 stop
 endif
+if(Is UNKNOWN objects?) then (yes)
+if(isSatisfiedWithUnknownTypeObjectCodition) then (yes)
+#FF006C :AVOID;
+stop
+else (\n no)
+#00FFB1 :IGNORE;
+stop
+endif
+else (\n no)
 if(Is vehicle type?) then (yes)
 if(isSatisfiedWithVehicleCodition()) then (yes)
 else (\n no)
@@ -618,6 +627,18 @@ title Filter vehicle which is obviously an avoidance target
 start
 
 partition isObviousAvoidanceTarget() {
+if(Is object within freespace?) then (yes)
+if(Is object on ego lane?) then (no)
+if(Is object stopping longer than threshold?) then (yes)
+#FF006C :return true;
+stop
+else (\n no)
+endif
+else (\n yes)
+endif
+else (\n no)
+endif
+
 if(Is object within intersection?) then (yes)
 else (\n no)
 if(Is object parallel to ego lane? AND Is object a parked vehicle?) then (yes)
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
index 3087ccc93934b..0e60e3216361d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/config/static_obstacle_avoidance.param.yaml
@@ -138,8 +138,11 @@
 
         # params for avoidance of vehicle type objects that are ambiguous as to whether they are parked.
         avoidance_for_ambiguous_vehicle:
-          enable: true                                  # [-]
-          closest_distance_to_wait_and_see: 10.0        # [m]
+          # policy for ego behavior for ambiguous vehicle.
+          # "auto"   : generate candidate path. if RTC is running as AUTO mode, the ego avoids it automatically.
+          # "manual" : generate candidate path and wait operator approval even if RTC is running as AUTO mode.
+          # "ignore" : never avoid it.
+          policy: "auto"                                # [-]
           condition:
             th_stopped_time: 3.0                        # [s]
             th_moving_distance: 1.0                     # [m]
@@ -149,11 +152,18 @@
             crosswalk:
               front_distance: 30.0                      # [m]
               behind_distance: 30.0                     # [m]
+          wait_and_see:
+            target_behaviors: ["MERGING", "DEVIATING"]  # [-]
+            th_closest_distance: 10.0                   # [m]
 
         # params for filtering objects that are in intersection
         intersection:
           yaw_deviation: 0.349                          # [rad] (default 20.0deg)
 
+        freespace:
+          condition:
+            th_stopped_time: 5.0                        # [-]
+
       # For safety check
       safety_check:
         # safety check target type
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
index 49599004e0952..4e14dc4886768 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/data_structs.hpp
@@ -108,7 +108,7 @@ struct AvoidanceParameters
   bool enable_cancel_maneuver{false};
 
   // enable avoidance for all parking vehicle
-  bool enable_avoidance_for_ambiguous_vehicle{false};
+  std::string policy_ambiguous_vehicle{"ignore"};
 
   // enable yield maneuver.
   bool enable_yield_maneuver{false};
@@ -191,8 +191,12 @@ struct AvoidanceParameters
   // minimum road shoulder width. maybe 0.5 [m]
   double object_check_min_road_shoulder_width{0.0};
 
+  // time threshold for vehicle in freespace.
+  double freespace_condition_th_stopped_time{0.0};
+
   // force avoidance
-  double closest_distance_to_wait_and_see_for_ambiguous_vehicle{0.0};
+  std::vector<std::string> wait_and_see_target_behaviors{"NONE", "MERGING", "DEVIATING"};
+  double wait_and_see_th_closest_distance{0.0};
   double time_threshold_for_ambiguous_vehicle{0.0};
   double distance_threshold_for_ambiguous_vehicle{0.0};
 
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp
index 316501fbbd37f..b205838896038 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp
@@ -47,6 +47,11 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
 MarkerArray createOtherObjectsMarkerArray(
   const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose);
 
+MarkerArray createAmbiguousObjectsMarkerArray(
+  const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy);
+
+MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data);
+
 MarkerArray createDebugMarkerArray(
   const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
   const std::shared_ptr<AvoidanceParameters> & parameters);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp
index 8c46affbc64e3..bfeb942c82be3 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/helper.hpp
@@ -23,6 +23,7 @@
 #include <algorithm>
 #include <limits>
 #include <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -325,10 +326,27 @@ class AvoidanceHelper
 
     const auto object = objects.front();
 
+    // if the object is NOT ambiguous, this module doesn't wait operator approval if RTC is running
+    // as AUTO mode.
     if (!object.is_ambiguous) {
       return true;
     }
 
+    // check only front objects.
+    if (object.longitudinal < 0.0) {
+      return true;
+    }
+
+    // if the policy is "manual", this module generates candidate path and waits approval.
+    if (parameters_->policy_ambiguous_vehicle == "manual") {
+      return false;
+    }
+
+    // don't delay avoidance start position if it's not MERGING or DEVIATING vehicle.
+    if (!isWaitAndSeeTarget(object)) {
+      return true;
+    }
+
     if (!object.avoid_margin.has_value()) {
       return true;
     }
@@ -341,9 +359,32 @@ class AvoidanceHelper
     const auto constant_distance = getFrontConstantDistance(object);
     const auto avoidance_distance = getMinAvoidanceDistance(desire_shift_length);
 
-    return object.longitudinal <
-           prepare_distance + constant_distance + avoidance_distance +
-             parameters_->closest_distance_to_wait_and_see_for_ambiguous_vehicle;
+    return object.longitudinal < prepare_distance + constant_distance + avoidance_distance +
+                                   parameters_->wait_and_see_th_closest_distance;
+  }
+
+  bool isWaitAndSeeTarget(const ObjectData & object) const
+  {
+    const auto & behaviors = parameters_->wait_and_see_target_behaviors;
+    if (object.behavior == ObjectData::Behavior::MERGING) {
+      return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) {
+        return behavior == "MERGING";
+      });
+    }
+
+    if (object.behavior == ObjectData::Behavior::DEVIATING) {
+      return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) {
+        return behavior == "DEVIATING";
+      });
+    }
+
+    if (object.behavior == ObjectData::Behavior::NONE) {
+      return std::any_of(behaviors.begin(), behaviors.end(), [](const std::string & behavior) {
+        return behavior == "NONE";
+      });
+    }
+
+    return false;
   }
 
   static bool isAbsolutelyNotAvoidable(const ObjectData & object)
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
index 84cf7c4e33d26..56ac3d7f4f1bb 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/parameter_helper.hpp
@@ -137,9 +137,11 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
 
   {
     const std::string ns = "avoidance.target_filtering.avoidance_for_ambiguous_vehicle.";
-    p.enable_avoidance_for_ambiguous_vehicle = getOrDeclareParameter<bool>(*node, ns + "enable");
-    p.closest_distance_to_wait_and_see_for_ambiguous_vehicle =
-      getOrDeclareParameter<double>(*node, ns + "closest_distance_to_wait_and_see");
+    p.policy_ambiguous_vehicle = getOrDeclareParameter<std::string>(*node, ns + "policy");
+    p.wait_and_see_target_behaviors =
+      getOrDeclareParameter<std::vector<std::string>>(*node, ns + "wait_and_see.target_behaviors");
+    p.wait_and_see_th_closest_distance =
+      getOrDeclareParameter<double>(*node, ns + "wait_and_see.th_closest_distance");
     p.time_threshold_for_ambiguous_vehicle =
       getOrDeclareParameter<double>(*node, ns + "condition.th_stopped_time");
     p.distance_threshold_for_ambiguous_vehicle =
@@ -152,6 +154,12 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
       getOrDeclareParameter<double>(*node, ns + "ignore_area.crosswalk.behind_distance");
   }
 
+  {
+    const std::string ns = "avoidance.target_filtering.freespace.";
+    p.freespace_condition_th_stopped_time =
+      getOrDeclareParameter<double>(*node, ns + "condition.th_stopped_time");
+  }
+
   {
     const std::string ns = "avoidance.target_filtering.detection_area.";
     p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
index 635edb7c84f40..34d06a46d9ac8 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp
@@ -266,6 +266,12 @@ class StaticObstacleAvoidanceModule : public SceneModuleInterface
    */
   void fillAvoidanceTargetObjects(AvoidancePlanningData & data, DebugData & debug) const;
 
+  /**
+   * @brief fill additional data which are necessary to plan avoidance path/velocity.
+   * @param avoidance target objects.
+   */
+  void fillAvoidanceTargetData(ObjectDataArray & objects) const;
+
   /**
    * @brief fill candidate shift lines.
    * @param avoidance data.
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
index 1f68ef7d49c47..909d28ed4bab6 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
@@ -117,15 +117,12 @@ void fillObjectStoppableJudge(
   ObjectData & object_data, const ObjectDataArray & registered_objects,
   const double feasible_stop_distance, const std::shared_ptr<AvoidanceParameters> & parameters);
 
-void updateRegisteredObject(
-  ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
-  const std::shared_ptr<AvoidanceParameters> & parameters);
-
 void updateClipObject(ObjectDataArray & clip_objects, AvoidancePlanningData & data);
 
-void compensateDetectionLost(
-  const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
-  ObjectDataArray & other_objects);
+void compensateLostTargetObjects(
+  ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
+  const std::shared_ptr<const PlannerData> & planner_data,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
 
 void filterTargetObjects(
   ObjectDataArray & objects, AvoidancePlanningData & data, const double forward_detection_range,
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
index db3215fa8d238..38a91ac39525b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/schema/static_obstacle_avoidance.schema.json
@@ -716,9 +716,10 @@
             "avoidance_for_ambiguous_vehicle": {
               "type": "object",
               "properties": {
-                "enable": {
-                  "type": "boolean",
-                  "description": "Enable avoidance maneuver for ambiguous vehicles.",
+                "policy": {
+                  "type": "string",
+                  "enum": ["auto", "manual", "ignore"],
+                  "description": "Ego behavior policy for ambiguous vehicle.",
                   "default": "true"
                 },
                 "closest_distance_to_wait_and_see": {
@@ -778,14 +779,26 @@
                   },
                   "required": ["traffic_light", "crosswalk"],
                   "additionalProperties": false
+                },
+                "wait_and_see": {
+                  "type": "object",
+                  "properties": {
+                    "target_behaviors": {
+                      "type": "array",
+                      "default": ["MERGING", "DEVIATING"],
+                      "description": "This module doesn't avoid these behaviors vehicle until it gets closer than threshold."
+                    },
+                    "th_closest_distance": {
+                      "type": "number",
+                      "default": 10.0,
+                      "description": "Threshold to check whether the ego gets close enough the ambiguous vehicle."
+                    }
+                  },
+                  "required": ["target_behaviors", "th_closest_distance"],
+                  "additionalProperties": false
                 }
               },
-              "required": [
-                "enable",
-                "closest_distance_to_wait_and_see",
-                "condition",
-                "ignore_area"
-              ],
+              "required": ["policy", "condition", "ignore_area", "wait_and_see"],
               "additionalProperties": false
             },
             "intersection": {
@@ -799,6 +812,25 @@
               },
               "required": ["yaw_deviation"],
               "additionalProperties": false
+            },
+            "freespace": {
+              "type": "object",
+              "properties": {
+                "condition": {
+                  "type": "object",
+                  "properties": {
+                    "th_stopped_time": {
+                      "type": "number",
+                      "description": "This module delays avoidance maneuver to see vehicle behavior in freespace.",
+                      "default": 5.0
+                    }
+                  },
+                  "required": ["th_stopped_time"],
+                  "additionalProperties": false
+                }
+              },
+              "required": ["condition"],
+              "additionalProperties": false
             }
           },
           "required": [
@@ -810,7 +842,8 @@
             "merging_vehicle",
             "parked_vehicle",
             "avoidance_for_ambiguous_vehicle",
-            "intersection"
+            "intersection",
+            "freespace"
           ],
           "additionalProperties": false
         },
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
index e9838eb4a2cfc..a542107e890aa 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
@@ -456,7 +456,7 @@ MarkerArray createOtherObjectsMarkerArray(
   appendMarkerArray(
     createObjectsCubeMarkerArray(
       filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
-      createMarkerColor(0.0, 1.0, 0.0, 0.8)),
+      createMarkerColor(0.5, 0.5, 0.5, 0.8)),
     &msg);
   appendMarkerArray(
     createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg);
@@ -466,6 +466,94 @@ MarkerArray createOtherObjectsMarkerArray(
   return msg;
 }
 
+MarkerArray createAmbiguousObjectsMarkerArray(
+  const ObjectDataArray & objects, const Pose & ego_pose, const std::string & policy)
+{
+  MarkerArray msg;
+
+  if (policy != "manual") {
+    return msg;
+  }
+
+  for (const auto & object : objects) {
+    if (!object.is_ambiguous || !object.is_avoidable) {
+      continue;
+    }
+
+    {
+      auto marker = createDefaultMarker(
+        "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target", 0L, Marker::ARROW,
+        createMarkerScale(0.5, 1.0, 1.0), createMarkerColor(1.0, 1.0, 0.0, 0.999));
+
+      Point src, dst;
+      src = object.getPosition();
+      src.z += 4.0;
+      dst = object.getPosition();
+      dst.z += 2.0;
+
+      marker.points.push_back(src);
+      marker.points.push_back(dst);
+      marker.id = uuidToInt32(object.object.object_id);
+
+      msg.markers.push_back(marker);
+    }
+
+    {
+      auto marker = createDefaultMarker(
+        "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "ambiguous_target_text", 0L,
+        Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5),
+        createMarkerColor(1.0, 1.0, 0.0, 1.0));
+
+      marker.id = uuidToInt32(object.object.object_id);
+      marker.pose = object.getPose();
+      marker.pose.position.z += 4.5;
+      std::ostringstream string_stream;
+      string_stream << "SHOULD AVOID?";
+      marker.text = string_stream.str();
+      marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999);
+      marker.scale = createMarkerScale(0.8, 0.8, 0.8);
+      msg.markers.push_back(marker);
+    }
+
+    {
+      auto marker = createDefaultMarker(
+        "map", rclcpp::Clock{RCL_ROS_TIME}.now(), "request_text", 0L, Marker::TEXT_VIEW_FACING,
+        createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));
+
+      marker.id = uuidToInt32(object.object.object_id);
+      marker.pose = ego_pose;
+      marker.pose.position.z += 2.0;
+      std::ostringstream string_stream;
+      string_stream << "SYSTEM REQUESTS OPERATOR SUPPORT.";
+      marker.text = string_stream.str();
+      marker.color = createMarkerColor(1.0, 1.0, 0.0, 0.999);
+      marker.scale = createMarkerScale(0.8, 0.8, 0.8);
+      msg.markers.push_back(marker);
+    }
+
+    return msg;
+  }
+
+  return msg;
+}
+
+MarkerArray createStopTargetObjectMarkerArray(const AvoidancePlanningData & data)
+{
+  MarkerArray msg;
+
+  if (!data.stop_target_object.has_value()) {
+    return msg;
+  }
+
+  appendMarkerArray(
+    createObjectsCubeMarkerArray(
+      {data.stop_target_object.value()}, "stop_target", createMarkerScale(3.4, 1.9, 1.9),
+      createMarkerColor(1.0, 0.0, 0.42, 0.5)),
+    &msg);
+
+  return msg;
+}
+
 MarkerArray createDrivableBounds(
   const AvoidancePlanningData & data, std::string && ns, const float & r, const float & g,
   const float & b)
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
index 2d02f33e19870..5655696ff086d 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/manager.cpp
@@ -132,10 +132,9 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
 
   {
     const std::string ns = "avoidance.avoidance.lateral.avoidance_for_ambiguous_vehicle.";
-    updateParam<bool>(parameters, ns + "enable", p->enable_avoidance_for_ambiguous_vehicle);
+    updateParam<std::string>(parameters, ns + "policy", p->policy_ambiguous_vehicle);
     updateParam<double>(
-      parameters, ns + "closest_distance_to_wait_and_see",
-      p->closest_distance_to_wait_and_see_for_ambiguous_vehicle);
+      parameters, ns + "wait_and_see.th_closest_distance", p->wait_and_see_th_closest_distance);
     updateParam<double>(
       parameters, ns + "condition.th_stopped_time", p->time_threshold_for_ambiguous_vehicle);
     updateParam<double>(
@@ -151,6 +150,12 @@ void StaticObstacleAvoidanceModuleManager::updateModuleParams(
       p->object_ignore_section_crosswalk_behind_distance);
   }
 
+  {
+    const std::string ns = "avoidance.target_filtering.freespace.";
+    updateParam<double>(
+      parameters, ns + "condition.th_stopped_time", p->freespace_condition_th_stopped_time);
+  }
+
   {
     const std::string ns = "avoidance.target_filtering.intersection.";
     updateParam<double>(parameters, ns + "yaw_deviation", p->object_check_yaw_deviation);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
index b7ab41ec99abd..d125f043f477b 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
@@ -288,16 +288,22 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
   data.to_start_point = utils::static_obstacle_avoidance::calcDistanceToAvoidStartLine(
     data.current_lanelets, data.reference_path_rough, planner_data_, parameters_);
 
-  // target objects for avoidance
+  // filter only for the latest detected objects.
   fillAvoidanceTargetObjects(data, debug);
 
-  // lost object compensation
-  utils::static_obstacle_avoidance::updateRegisteredObject(
-    registered_objects_, data.target_objects, parameters_);
-  utils::static_obstacle_avoidance::compensateDetectionLost(
-    registered_objects_, data.target_objects, data.other_objects);
+  // compensate lost object which was avoidance target. if the time hasn't passed more than
+  // threshold since perception module lost the target yet, this module keeps it as avoidance
+  // target.
+  utils::static_obstacle_avoidance::compensateLostTargetObjects(
+    registered_objects_, data, clock_->now(), planner_data_, parameters_);
+
+  // once an object filtered for boundary clipping, this module keeps the information until the end
+  // of execution.
   utils::static_obstacle_avoidance::updateClipObject(clip_objects_, data);
 
+  // calculate various data for each target objects.
+  fillAvoidanceTargetData(data.target_objects);
+
   // sort object order by longitudinal distance
   std::sort(data.target_objects.begin(), data.target_objects.end(), [](auto a, auto b) {
     return a.longitudinal < b.longitudinal;
@@ -310,8 +316,7 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
 void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
   AvoidancePlanningData & data, DebugData & debug) const
 {
-  using utils::static_obstacle_avoidance::fillAvoidanceNecessity;
-  using utils::static_obstacle_avoidance::fillObjectStoppableJudge;
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
   using utils::static_obstacle_avoidance::filterTargetObjects;
   using utils::static_obstacle_avoidance::separateObjectsByPath;
   using utils::static_obstacle_avoidance::updateRoadShoulderDistance;
@@ -347,15 +352,6 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
   filterTargetObjects(objects, data, forward_detection_range, planner_data_, parameters_);
   updateRoadShoulderDistance(data, planner_data_, parameters_);
 
-  // Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
-  const auto & vehicle_width = planner_data_->parameters.vehicle_width;
-  const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
-  std::for_each(data.target_objects.begin(), data.target_objects.end(), [&, this](auto & o) {
-    fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_);
-    o.to_stop_line = calcDistanceToStopLine(o);
-    fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
-  });
-
   // debug
   {
     std::vector<AvoidanceDebugMsg> debug_info_array;
@@ -373,6 +369,22 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
   }
 }
 
+void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const
+{
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  using utils::static_obstacle_avoidance::fillAvoidanceNecessity;
+  using utils::static_obstacle_avoidance::fillObjectStoppableJudge;
+
+  // Calculate the distance needed to safely decelerate the ego vehicle to a stop line.
+  const auto & vehicle_width = planner_data_->parameters.vehicle_width;
+  const auto feasible_stop_distance = helper_->getFeasibleDecelDistance(0.0, false);
+  std::for_each(objects.begin(), objects.end(), [&, this](auto & o) {
+    fillAvoidanceNecessity(o, registered_objects_, vehicle_width, parameters_);
+    o.to_stop_line = calcDistanceToStopLine(o);
+    fillObjectStoppableJudge(o, registered_objects_, feasible_stop_distance, parameters_);
+  });
+}
+
 ObjectData StaticObstacleAvoidanceModule::createObjectData(
   const AvoidancePlanningData & data, const PredictedObject & object) const
 {
@@ -1392,11 +1404,19 @@ void StaticObstacleAvoidanceModule::updateRTCData()
 
 void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const
 {
+  universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
+  using utils::static_obstacle_avoidance::createAmbiguousObjectsMarkerArray;
+  using utils::static_obstacle_avoidance::createStopTargetObjectMarkerArray;
   using utils::static_obstacle_avoidance::createTargetObjectsMarkerArray;
 
   info_marker_.markers.clear();
   appendMarkerArray(
     createTargetObjectsMarkerArray(data.target_objects, "target_objects"), &info_marker_);
+  appendMarkerArray(createStopTargetObjectMarkerArray(data), &info_marker_);
+  appendMarkerArray(
+    createAmbiguousObjectsMarkerArray(
+      data.target_objects, getEgoPose(), parameters_->policy_ambiguous_vehicle),
+    &info_marker_);
 }
 
 void StaticObstacleAvoidanceModule::updateDebugMarker(
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
index c3a67eb074d73..5e58466fa4229 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/shift_line_generator.cpp
@@ -239,10 +239,18 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
 
   const auto is_forward_object = [](const auto & object) { return object.longitudinal > 0.0; };
 
+  const auto is_on_path = [this](const auto & object) {
+    const auto [overhang, point] = object.overhang_points.front();
+    return std::abs(overhang) < 0.5 * data_->parameters.vehicle_width;
+  };
+
   const auto is_valid_shift_line = [](const auto & s) {
     return s.start_longitudinal > 0.0 && s.start_longitudinal < s.end_longitudinal;
   };
 
+  ObjectDataArray unavoidable_objects;
+
+  // target objects are sorted by longitudinal distance.
   AvoidOutlines outlines;
   for (auto & o : data.target_objects) {
     if (!o.avoid_margin.has_value()) {
@@ -253,22 +261,22 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
       } else {
         o.info = ObjectInfo::INSUFFICIENT_DRIVABLE_SPACE;
       }
-      if (o.avoid_required && is_forward_object(o)) {
+      if (o.avoid_required && is_forward_object(o) && is_on_path(o)) {
         break;
       } else {
+        unavoidable_objects.push_back(o);
         continue;
       }
     }
 
-    const auto is_object_on_right = utils::static_obstacle_avoidance::isOnRight(o);
     const auto desire_shift_length =
-      helper_->getShiftLength(o, is_object_on_right, o.avoid_margin.value());
-    if (utils::static_obstacle_avoidance::isSameDirectionShift(
-          is_object_on_right, desire_shift_length)) {
+      helper_->getShiftLength(o, isOnRight(o), o.avoid_margin.value());
+    if (utils::static_obstacle_avoidance::isSameDirectionShift(isOnRight(o), desire_shift_length)) {
       o.info = ObjectInfo::SAME_DIRECTION_SHIFT;
-      if (o.avoid_required && is_forward_object(o)) {
+      if (o.avoid_required && is_forward_object(o) && is_on_path(o)) {
         break;
       } else {
+        unavoidable_objects.push_back(o);
         continue;
       }
     }
@@ -276,13 +284,25 @@ AvoidOutlines ShiftLineGenerator::generateAvoidOutline(
     // calculate feasible shift length based on behavior policy
     const auto feasible_shift_profile = get_shift_profile(o, desire_shift_length);
     if (!feasible_shift_profile.has_value()) {
-      if (o.avoid_required && is_forward_object(o)) {
+      if (o.avoid_required && is_forward_object(o) && is_on_path(o)) {
         break;
       } else {
+        unavoidable_objects.push_back(o);
         continue;
       }
     }
 
+    // If there is an object that cannot be avoided, this module only avoids object on the same side
+    // as unavoidable object.
+    if (!unavoidable_objects.empty()) {
+      if (isOnRight(unavoidable_objects.front()) && !isOnRight(o)) {
+        break;
+      }
+      if (!isOnRight(unavoidable_objects.front()) && isOnRight(o)) {
+        break;
+      }
+    }
+
     // use absolute dist for return-to-center, relative dist from current for avoiding.
     const auto feasible_return_distance =
       helper_->getMaxAvoidanceDistance(feasible_shift_profile.value().first);
diff --git a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
index 016972dccb38e..0830c0f10dd4e 100644
--- a/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
+++ b/planning/behavior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
@@ -339,6 +339,32 @@ bool isWithinIntersection(
     lanelet::utils::to2D(polygon.basicPolygon()));
 }
 
+bool isWithinFreespace(
+  const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
+{
+  auto polygons = lanelet::utils::query::getAllParkingLots(route_handler->getLaneletMapPtr());
+  if (polygons.empty()) {
+    return false;
+  }
+
+  std::sort(polygons.begin(), polygons.end(), [&object](const auto & a, const auto & b) {
+    const double a_distance = boost::geometry::distance(
+      lanelet::utils::to2D(a).basicPolygon(),
+      lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+        .basicPoint());
+    const double b_distance = boost::geometry::distance(
+      lanelet::utils::to2D(b).basicPolygon(),
+      lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+        .basicPoint());
+    return a_distance < b_distance;
+  });
+
+  return boost::geometry::within(
+    lanelet::utils::to2D(lanelet::utils::conversion::toLaneletPoint(object.getPosition()))
+      .basicPoint(),
+    lanelet::utils::to2D(polygons.front().basicPolygon()));
+}
+
 bool isOnEgoLane(const ObjectData & object, const std::shared_ptr<RouteHandler> & route_handler)
 {
   if (boost::geometry::within(
@@ -708,6 +734,14 @@ bool isObviousAvoidanceTarget(
   [[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data,
   [[maybe_unused]] const std::shared_ptr<AvoidanceParameters> & parameters)
 {
+  if (isWithinFreespace(object, planner_data->route_handler)) {
+    if (!object.is_on_ego_lane) {
+      if (object.stop_time > parameters->freespace_condition_th_stopped_time) {
+        return true;
+      }
+    }
+  }
+
   if (!object.is_within_intersection) {
     if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) {
       RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle.");
@@ -880,7 +914,7 @@ bool isSatisfiedWithVehicleCondition(
 
   // from here, filtering for ambiguous vehicle.
 
-  if (!parameters->enable_avoidance_for_ambiguous_vehicle) {
+  if (parameters->policy_ambiguous_vehicle == "ignore") {
     object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
     return false;
   }
@@ -889,6 +923,7 @@ bool isSatisfiedWithVehicleCondition(
     object.stop_time > parameters->time_threshold_for_ambiguous_vehicle;
   if (!stop_time_longer_than_threshold) {
     object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
+    object.is_ambiguous = false;
     return false;
   }
 
@@ -897,6 +932,7 @@ bool isSatisfiedWithVehicleCondition(
     parameters->distance_threshold_for_ambiguous_vehicle;
   if (is_moving_distance_longer_than_threshold) {
     object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
+    object.is_ambiguous = false;
     return false;
   }
 
@@ -907,22 +943,9 @@ bool isSatisfiedWithVehicleCondition(
       return true;
     }
   } else {
-    if (object.behavior == ObjectData::Behavior::MERGING) {
-      object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
-      object.is_ambiguous = true;
-      return true;
-    }
-
-    if (object.behavior == ObjectData::Behavior::DEVIATING) {
-      object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
-      object.is_ambiguous = true;
-      return true;
-    }
-
-    if (object.behavior == ObjectData::Behavior::NONE) {
-      object.is_ambiguous = false;
-      return true;
-    }
+    object.info = ObjectInfo::AMBIGUOUS_STOPPED_VEHICLE;
+    object.is_ambiguous = true;
+    return true;
   }
 
   object.info = ObjectInfo::IS_NOT_PARKING_OBJECT;
@@ -1645,31 +1668,39 @@ void fillObjectStoppableJudge(
   object_data.is_stoppable = same_id_obj->is_stoppable;
 }
 
-void updateRegisteredObject(
-  ObjectDataArray & registered_objects, const ObjectDataArray & now_objects,
+void compensateLostTargetObjects(
+  ObjectDataArray & stored_objects, AvoidancePlanningData & data, const rclcpp::Time & now,
+  const std::shared_ptr<const PlannerData> & planner_data,
   const std::shared_ptr<AvoidanceParameters> & parameters)
 {
-  const auto updateIfDetectedNow = [&now_objects](auto & registered_object) {
-    const auto & n = now_objects;
-    const auto r_id = registered_object.object.object_id;
-    const auto same_id_obj = std::find_if(
-      n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
+  const auto include = [](const auto & objects, const auto & search_id) {
+    return std::all_of(objects.begin(), objects.end(), [&search_id](const auto & o) {
+      return o.object.object_id != search_id;
+    });
+  };
+
+  // STEP.1 UPDATE STORED OBJECTS.
+  const auto match = [&data](auto & object) {
+    const auto & search_id = object.object.object_id;
+    const auto same_id_object = std::find_if(
+      data.target_objects.begin(), data.target_objects.end(),
+      [&search_id](const auto & o) { return o.object.object_id == search_id; });
 
     // same id object is detected. update registered.
-    if (same_id_obj != n.end()) {
-      registered_object = *same_id_obj;
+    if (same_id_object != data.target_objects.end()) {
+      object = *same_id_object;
       return true;
     }
 
-    constexpr auto POS_THR = 1.5;
-    const auto r_pos = registered_object.getPose();
-    const auto similar_pos_obj = std::find_if(n.begin(), n.end(), [&](const auto & o) {
-      return calcDistance2d(r_pos, o.getPose()) < POS_THR;
-    });
+    const auto similar_pos_obj = std::find_if(
+      data.target_objects.begin(), data.target_objects.end(), [&object](const auto & o) {
+        constexpr auto POS_THR = 1.5;
+        return calcDistance2d(object.getPose(), o.getPose()) < POS_THR;
+      });
 
     // same id object is not detected, but object is found around registered. update registered.
-    if (similar_pos_obj != n.end()) {
-      registered_object = *similar_pos_obj;
+    if (similar_pos_obj != data.target_objects.end()) {
+      object = *similar_pos_obj;
       return true;
     }
 
@@ -1677,61 +1708,54 @@ void updateRegisteredObject(
     return false;
   };
 
-  const rclcpp::Time now = rclcpp::Clock(RCL_ROS_TIME).now();
-
-  // -- check registered_objects, remove if lost_count exceeds limit. --
-  for (int i = static_cast<int>(registered_objects.size()) - 1; i >= 0; --i) {
-    auto & r = registered_objects.at(i);
-
-    // registered object is not detected this time. lost count up.
-    if (!updateIfDetectedNow(r)) {
-      r.lost_time = (now - r.last_seen).seconds();
-    } else {
-      r.last_seen = now;
-      r.lost_time = 0.0;
-    }
+  // STEP1-1: REMOVE EXPIRED OBJECTS.
+  const auto itr = std::remove_if(
+    stored_objects.begin(), stored_objects.end(), [&now, &match, &parameters](auto & o) {
+      if (!match(o)) {
+        o.lost_time = (now - o.last_seen).seconds();
+      } else {
+        o.last_seen = now;
+        o.lost_time = 0.0;
+      }
 
-    // lost count exceeds threshold. remove object from register.
-    if (r.lost_time > parameters->object_last_seen_threshold) {
-      registered_objects.erase(registered_objects.begin() + i);
-    }
-  }
+      return o.lost_time > parameters->object_last_seen_threshold;
+    });
 
-  const auto isAlreadyRegistered = [&](const auto & n_id) {
-    const auto & r = registered_objects;
-    return std::any_of(
-      r.begin(), r.end(), [&n_id](const auto & o) { return o.object.object_id == n_id; });
-  };
+  stored_objects.erase(itr, stored_objects.end());
 
-  // -- check now_objects, add it if it has new object id --
-  for (const auto & now_obj : now_objects) {
-    if (!isAlreadyRegistered(now_obj.object.object_id)) {
-      registered_objects.push_back(now_obj);
+  // STEP1-2: UPDATE STORED OBJECTS IF THERE ARE NEW OBJECTS.
+  for (const auto & current_object : data.target_objects) {
+    if (!include(stored_objects, current_object.object.object_id)) {
+      stored_objects.push_back(current_object);
     }
   }
-}
 
-void compensateDetectionLost(
-  const ObjectDataArray & registered_objects, ObjectDataArray & now_objects,
-  ObjectDataArray & other_objects)
-{
-  const auto isDetectedNow = [&](const auto & r_id) {
-    const auto & n = now_objects;
+  // STEP2: COMPENSATE CURRENT TARGET OBJECTS
+  const auto is_detected = [&](const auto & object_id) {
     return std::any_of(
-      n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
+      data.target_objects.begin(), data.target_objects.end(),
+      [&object_id](const auto & o) { return o.object.object_id == object_id; });
   };
 
-  const auto isIgnoreObject = [&](const auto & r_id) {
-    const auto & n = other_objects;
+  const auto is_ignored = [&](const auto & object_id) {
     return std::any_of(
-      n.begin(), n.end(), [&r_id](const auto & o) { return o.object.object_id == r_id; });
+      data.other_objects.begin(), data.other_objects.end(),
+      [&object_id](const auto & o) { return o.object.object_id == object_id; });
   };
 
-  for (const auto & registered : registered_objects) {
-    if (
-      !isDetectedNow(registered.object.object_id) && !isIgnoreObject(registered.object.object_id)) {
-      now_objects.push_back(registered);
+  for (auto & stored_object : stored_objects) {
+    if (is_detected(stored_object.object.object_id)) {
+      continue;
     }
+    if (is_ignored(stored_object.object.object_id)) {
+      continue;
+    }
+
+    const auto & ego_pos = planner_data->self_odometry->pose.pose.position;
+    fillLongitudinalAndLengthByClosestEnvelopeFootprint(
+      data.reference_path_rough, ego_pos, stored_object);
+
+    data.target_objects.push_back(stored_object);
   }
 }
 
@@ -1852,17 +1876,19 @@ void filterTargetObjects(
       utils::static_obstacle_avoidance::calcEnvelopeOverhangDistance(o, data.reference_path);
     o.to_road_shoulder_distance = filtering_utils::getRoadShoulderDistance(o, data, planner_data);
 
-    // TODO(Satoshi Ota) parametrize stop time threshold if need.
-    constexpr double STOP_TIME_THRESHOLD = 3.0;  // [s]
     if (filtering_utils::isUnknownTypeObject(o)) {
+      // TARGET: UNKNOWN
+
+      // TODO(Satoshi Ota) parametrize stop time threshold if need.
+      constexpr double STOP_TIME_THRESHOLD = 3.0;  // [s]
       if (o.stop_time < STOP_TIME_THRESHOLD) {
         o.info = ObjectInfo::UNSTABLE_OBJECT;
         data.other_objects.push_back(o);
         continue;
       }
-    }
+    } else if (filtering_utils::isVehicleTypeObject(o)) {
+      // TARGET: CAR, TRUCK, BUS, TRAILER, MOTORCYCLE
 
-    if (filtering_utils::isVehicleTypeObject(o)) {
       o.is_within_intersection =
         filtering_utils::isWithinIntersection(o, planner_data->route_handler);
       o.is_parked =
@@ -1879,6 +1905,8 @@ void filterTargetObjects(
         continue;
       }
     } else {
+      // TARGET: PEDESTRIAN, BICYCLE
+
       o.is_within_intersection =
         filtering_utils::isWithinIntersection(o, planner_data->route_handler);
       o.is_parked = false;