feat(obstacle_cruise_planner): suppress flickering to entry slow down (…

…#3904) * feat(obstacle_cruise_planner): suppress flickering to entry slow down Signed-off-by: Takayuki Murooka <[email protected]> * fix Signed-off-by: Takayuki Murooka <[email protected]> * update config Signed-off-by: Takayuki Murooka <[email protected]> --------- Signed-off-by: Takayuki Murooka <[email protected]>
autowarefoundation · Jun 5, 2023 · 30253b1 · 30253b1
1 parent f07b0e0
commit 30253b1
Show file tree

Hide file tree

Showing 4 changed files with 111 additions and 15 deletions.
diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
@@ -96,6 +96,9 @@
         max_lat_margin: 1.1  # lateral margin between obstacle and trajectory band with ego's width
         lat_hysteresis_margin: 0.2
 
+        successive_num_to_entry_slow_down_condition: 5
+        successive_num_to_exit_slow_down_condition: 5
+
     cruise:
       pid_based_planner:
         use_velocity_limit_based_planner: true

diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp
@@ -24,6 +24,7 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <algorithm>
 #include <memory>
 #include <mutex>
 #include <optional>
@@ -186,12 +187,60 @@ class ObstacleCruisePlannerNode : public rclcpp::Node
     double max_lat_margin_for_cruise;
     double max_lat_margin_for_slow_down;
     double lat_hysteresis_margin_for_slow_down;
+    int successive_num_to_entry_slow_down_condition;
+    int successive_num_to_exit_slow_down_condition;
   };
   BehaviorDeterminationParam behavior_determination_param_;
 
   std::unordered_map<std::string, bool> need_to_clear_vel_limit_{
     {"cruise", false}, {"slow_down", false}};
 
+  struct SlowDownConditionCounter
+  {
+    void resetCurrentUuids() { current_uuids_.clear(); }
+    void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); }
+    void removeCounterUnlessUpdated()
+    {
+      std::vector<std::string> obsolete_uuids;
+      for (const auto & key_and_value : counter_) {
+        if (
+          std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) ==
+          current_uuids_.end()) {
+          obsolete_uuids.push_back(key_and_value.first);
+        }
+      }
+
+      for (const auto & obsolete_uuid : obsolete_uuids) {
+        counter_.erase(obsolete_uuid);
+      }
+    }
+
+    int increaseCounter(const std::string & uuid)
+    {
+      if (counter_.count(uuid) != 0) {
+        counter_.at(uuid) = std::max(1, counter_.at(uuid) + 1);
+      } else {
+        counter_.emplace(uuid, 1);
+      }
+      return counter_.at(uuid);
+    }
+    int decreaseCounter(const std::string & uuid)
+    {
+      if (counter_.count(uuid) != 0) {
+        counter_.at(uuid) = std::min(-1, counter_.at(uuid) - 1);
+      } else {
+        counter_.emplace(uuid, -1);
+      }
+      return counter_.at(uuid);
+    }
+    void reset(const std::string & uuid) { counter_.emplace(uuid, 0); }
+
+    // NOTE: positive is for meeting entrying condition, and negative is for exiting.
+    std::unordered_map<std::string, int> counter_;
+    std::vector<std::string> current_uuids_;
+  };
+  SlowDownConditionCounter slow_down_condition_counter_;
+
   EgoNearestParam ego_nearest_param_;
 
   bool is_driving_forward_{true};

diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp
@@ -263,6 +263,10 @@ ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationPara
     node.declare_parameter<double>("behavior_determination.slow_down.max_lat_margin");
   lat_hysteresis_margin_for_slow_down =
     node.declare_parameter<double>("behavior_determination.slow_down.lat_hysteresis_margin");
+  successive_num_to_entry_slow_down_condition = node.declare_parameter<int>(
+    "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition");
+  successive_num_to_exit_slow_down_condition = node.declare_parameter<int>(
+    "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition");
 }
 
 void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
@@ -315,6 +319,12 @@ void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam(
   tier4_autoware_utils::updateParam<double>(
     parameters, "behavior_determination.slow_down.lat_hysteresis_margin",
     lat_hysteresis_margin_for_slow_down);
+  tier4_autoware_utils::updateParam<int>(
+    parameters, "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition",
+    successive_num_to_entry_slow_down_condition);
+  tier4_autoware_utils::updateParam<int>(
+    parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition",
+    successive_num_to_exit_slow_down_condition);
 }
 
 ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options)
@@ -621,6 +631,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
   std::vector<StopObstacle> stop_obstacles;
   std::vector<CruiseObstacle> cruise_obstacles;
   std::vector<SlowDownObstacle> slow_down_obstacles;
+  slow_down_condition_counter_.resetCurrentUuids();
   for (const auto & obstacle : obstacles) {
     const auto obstacle_poly = obstacle.toPolygon();
 
@@ -651,6 +662,7 @@ ObstacleCruisePlannerNode::determineEgoBehaviorAgainstObstacles(
       continue;
     }
   }
+  slow_down_condition_counter_.removeCounterUnlessUpdated();
 
   // Check target obstacles' consistency
   checkConsistency(objects_ptr_->header.stamp, *objects_ptr_, traj_points, stop_obstacles);
@@ -953,9 +965,9 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
   const std::vector<TrajectoryPoint> & traj_points, const Obstacle & obstacle,
   const double precise_lat_dist)
 {
-  std::cerr << precise_lat_dist << std::endl;
   const auto & object_id = obstacle.uuid.substr(0, 4);
   const auto & p = behavior_determination_param_;
+  slow_down_condition_counter_.addCurrentUuid(obstacle.uuid);
 
   const bool is_prev_obstacle_slow_down =
     getObstacleFromUuid(prev_slow_down_obstacles_, obstacle.uuid).has_value();
@@ -969,7 +981,30 @@ std::optional<SlowDownObstacle> ObstacleCruisePlannerNode::createSlowDownObstacl
     precise_lat_dist, is_prev_obstacle_slow_down,
     p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0,
     p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0);
-  if (!is_lat_dist_low) {
+
+  const bool is_slow_down_required = [&]() {
+    if (is_prev_obstacle_slow_down) {
+      // check if exiting slow down
+      if (!is_lat_dist_low) {
+        const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid);
+        if (count <= -p.successive_num_to_exit_slow_down_condition) {
+          slow_down_condition_counter_.reset(obstacle.uuid);
+          return false;
+        }
+      }
+      return true;
+    }
+    // check if entrying slow down
+    if (is_lat_dist_low) {
+      const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid);
+      if (p.successive_num_to_entry_slow_down_condition <= count) {
+        slow_down_condition_counter_.reset(obstacle.uuid);
+        return true;
+      }
+    }
+    return false;
+  }();
+  if (!is_slow_down_required) {
     RCLCPP_INFO_EXPRESSION(
       get_logger(), enable_debug_info_,
       "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(),

diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp
@@ -471,14 +471,16 @@ std::vector<TrajectoryPoint> PlannerInterface::generateSlowDownTrajectory(
 
     // add debug data and virtual wall
     slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist);
+    slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start);
+    slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end);
+    slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel);
     slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel);
+    slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0);
+    slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0);
     if (slow_down_start_idx) {
-      slow_down_debug_multi_array_.data.push_back(
-        slow_down_start_idx ? *slow_down_start_idx : -1.0);
       add_slow_down_marker(i, slow_down_start_idx, true);
     }
     if (slow_down_end_idx) {
-      slow_down_debug_multi_array_.data.push_back(slow_down_end_idx ? *slow_down_end_idx : -1.0);
       add_slow_down_marker(i, slow_down_end_idx, false);
     }
 
@@ -548,20 +550,27 @@ PlannerInterface::calculateDistanceToSlowDownWithConstraints(
   // calculate offset distance to first collision considering relative velocity
   const double relative_vel = planner_data.ego_vel - obstacle.velocity;
   const double offset_dist_to_collision = [&]() {
-    if (relative_vel < 0) {
-      return 100.0;
+    if (dist_to_front_collision < dist_to_ego + abs_ego_offset) {
+      return 0.0;
     }
-    const double time_to_collision = [&]() {
-      if (dist_to_front_collision < dist_to_ego + abs_ego_offset) {
-        return 0.0;
-      }
-      return (dist_to_front_collision - dist_to_ego - abs_ego_offset) / std::max(1.0, relative_vel);
-    }();
-    return obstacle_vel * time_to_collision;
+
+    // NOTE: This min_relative_vel forces the relative velocity positive if the ego velocity is
+    // lower than the obstacle velocity. Without this, the slow down feature will flicker where the
+    // ego velocity is very close to the obstacle velocity.
+    constexpr double min_relative_vel = 1.0;
+    const double time_to_collision = (dist_to_front_collision - dist_to_ego - abs_ego_offset) /
+                                     std::max(min_relative_vel, relative_vel);
+
+    constexpr double time_to_collision_margin = 1.0;
+    const double cropped_time_to_collision =
+      std::max(0.0, time_to_collision - time_to_collision_margin);
+    return obstacle_vel * cropped_time_to_collision;
   }();
 
   // calculate distance during deceleration, slow down preparation, and slow down
-  const double slow_down_prepare_dist = slow_down_vel * p.time_margin_on_target_velocity;
+  const double min_slow_down_prepare_dist = 3.0;
+  const double slow_down_prepare_dist =
+    std::max(min_slow_down_prepare_dist, slow_down_vel * p.time_margin_on_target_velocity);
   const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision -
                                    abs_ego_offset - dist_to_ego - slow_down_prepare_dist;
   const double slow_down_dist =