fix(static_obstacle_avoidance): fix stop position

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Jul 8, 2024 · ca065a0 · ca065a0
1 parent 26e87ea
commit ca065a0
Show file tree

Hide file tree

Showing 7 changed files with 119 additions and 86 deletions.
diff --git a/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/...ehavior_path_planner/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp
@@ -113,10 +113,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/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp b/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/debug.hpp
@@ -47,6 +47,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
 MarkerArray createOtherObjectsMarkerArray(
   const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose);
 
+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/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/scene.hpp b/...voidance_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/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp b/...voidance_module/include/autoware/behavior_path_static_obstacle_avoidance_module/utils.hpp
@@ -118,15 +118,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/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/debug.cpp
@@ -466,6 +466,23 @@ MarkerArray createOtherObjectsMarkerArray(
   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/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/scene.cpp
@@ -286,16 +286,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;
@@ -308,8 +314,6 @@ void StaticObstacleAvoidanceModule::fillFundamentalData(
 void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
   AvoidancePlanningData & data, DebugData & debug) const
 {
-  using utils::static_obstacle_avoidance::fillAvoidanceNecessity;
-  using utils::static_obstacle_avoidance::fillObjectStoppableJudge;
   using utils::static_obstacle_avoidance::filterTargetObjects;
   using utils::static_obstacle_avoidance::separateObjectsByPath;
   using utils::static_obstacle_avoidance::updateRoadShoulderDistance;
@@ -345,15 +349,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;
@@ -371,6 +366,21 @@ void StaticObstacleAvoidanceModule::fillAvoidanceTargetObjects(
   }
 }
 
+void StaticObstacleAvoidanceModule::fillAvoidanceTargetData(ObjectDataArray & objects) const
+{
+  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
 {
@@ -1376,11 +1386,13 @@ void StaticObstacleAvoidanceModule::updateRTCData()
 
 void StaticObstacleAvoidanceModule::updateInfoMarker(const AvoidancePlanningData & data) const
 {
+  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_);
 }
 
 void StaticObstacleAvoidanceModule::updateDebugMarker(

diff --git a/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp b/...havior_path_planner/autoware_behavior_path_static_obstacle_avoidance_module/src/utils.cpp
@@ -1645,93 +1645,94 @@ 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;
     }
 
     // Same ID nor similar position object does not found.
     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);
   }
 }