fix engage obstacle clear and stop threshold

tier4 · Dec 19, 2023 · 809b85e · 809b85e
1 parent 87ef309
commit 809b85e
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 6 deletions.
diff --git a/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp b/planning/obstacle_stop_planner/include/obstacle_stop_planner/node.hpp
@@ -320,10 +320,10 @@ class ObstacleStopPlannerNode : public rclcpp::Node
   void publishDebugData(
     const PlannerData & planner_data, const double current_acc, const double current_vel);
 
-  void updateObstacleHistory(const rclcpp::Time & now)
+  void updateObstacleHistory(const rclcpp::Time & now, const double chattering_threshold)
   {
     for (auto itr = obstacle_history_.begin(); itr != obstacle_history_.end();) {
-      const auto expired = (now - itr->detection_time).seconds() > node_param_.chattering_threshold;
+      const auto expired = (now - itr->detection_time).seconds() > chattering_threshold;
 
       if (expired) {
         itr = obstacle_history_.erase(itr);

diff --git a/planning/obstacle_stop_planner/src/node.cpp b/planning/obstacle_stop_planner/src/node.cpp
@@ -662,10 +662,9 @@ void ObstacleStopPlannerNode::searchObstacle(
   }
 
   const auto now = this->now();
-
-  if (std::fabs(current_velocity_ptr->twist.twist.linear.x) < 1e-9) {
-    updateObstacleHistory(now);
-  }
+  const bool is_stopping = (std::fabs(current_velocity_ptr->twist.twist.linear.x) < 0.001);
+  const double history_erase_sec = (is_stopping) ? node_param_.chattering_threshold : 0.0; 
+  updateObstacleHistory(now, history_erase_sec);
 
   for (size_t i = 0; i < decimate_trajectory.size() - 1; ++i) {
     // create one step circle center for vehicle