fix timer

Signed-off-by: Maxime CLEMENT <maxime.clement@tier4.jp>

Author: maxime-clem

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.cpp

@@ -180,13 +180,4 @@ double calculate_detection_range(
   const auto time_to_crosswalk = dist_ego_to_crosswalk / std::max(min_ego_velocity, ego_velocity);
   return time_to_crosswalk > 0.0 ? time_to_crosswalk / occluded_object_velocity : 20.0;
 }
-
-void update_occlusion_timers(
-  std::optional<rclcpp::Time> & initial_time,
-  std::optional<rclcpp::Time> & most_recent_slowdown_time, const rclcpp::Time & now,
-  const double buffer)
-{
-  if (!initial_time) initial_time = now;
-  if ((now - *initial_time).seconds() >= buffer) most_recent_slowdown_time = now;
-}
 }  // namespace behavior_velocity_planner

planning/behavior_velocity_crosswalk_module/src/occluded_crosswalk.hpp

@@ -96,15 +96,6 @@ std::vector<autoware_auto_perception_msgs::msg::PredictedObject> select_and_infl
 void clear_occlusions_behind_objects(
   grid_map::GridMap & grid_map,
   const std::vector<autoware_auto_perception_msgs::msg::PredictedObject> & objects);
-
-/// @brief update timers so that the slowdown activates if the initial time is older than the buffer
-/// @param initial_time initial time
-/// @param most_recent_slowdown_time time to set only if initial_time is older than the buffer
-/// @param buffer [s] time buffer
-void update_occlusion_timers(
-  std::optional<rclcpp::Time> & initial_time,
-  std::optional<rclcpp::Time> & most_recent_slowdown_time, const rclcpp::Time & now,
-  const double buffer);
 }  // namespace behavior_velocity_planner
 
 #endif  // OCCLUDED_CROSSWALK_HPP_

planning/behavior_velocity_crosswalk_module/src/scene_crosswalk.cpp

@@ -32,6 +32,7 @@
 
 #include <algorithm>
 #include <cmath>
+#include <functional>
 #include <limits>
 #include <set>
 #include <tuple>
@@ -234,6 +235,10 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
       static_cast<float>(crosswalk_.attribute("safety_slow_down_speed").asDouble().get()));
   }
   // Apply safety slow down speed if the crosswalk is occluded
+  const auto now = clock_->now();
+  const auto cmp_with_time_buffer = [&](const auto & t, const auto cmp_fn) {
+    return t && cmp_fn((now - *t).seconds(), planner_param_.occlusion_time_buffer);
+  };
   if (
     planner_param_.occlusion_enable && !path_intersects.empty() &&
     !is_crosswalk_ignored(crosswalk_, planner_param_.occlusion_ignore_with_traffic_light)) {
@@ -247,21 +252,21 @@ bool CrosswalkModule::modifyPathVelocity(PathWithLaneId * path, StopReason * sto
     if (!is_ego_on_the_crosswalk) {
       if (is_crosswalk_occluded(
             crosswalk_, *planner_data_->occupancy_grid, path_intersects.front(), detection_range,
-            objects_ptr->objects, planner_param_))
-        update_occlusion_timers(
-          current_initial_occlusion_time_, most_recent_occlusion_time_, clock_->now(),
-          planner_param_.occlusion_time_buffer);
-      else
+            objects_ptr->objects, planner_param_)) {
+        if (!current_initial_occlusion_time_) current_initial_occlusion_time_ = now;
+        if (cmp_with_time_buffer(current_initial_occlusion_time_, std::greater_equal<double>{}))
+          most_recent_occlusion_time_ = now;
+      } else if (!cmp_with_time_buffer(most_recent_occlusion_time_, std::greater<double>{})) {
         current_initial_occlusion_time_.reset();
+      }
 
-      const auto is_last_occlusion_within_time_buffer =
-        most_recent_occlusion_time_ && (clock_->now() - *most_recent_occlusion_time_).seconds() <=
-                                         planner_param_.occlusion_time_buffer;
-      if (is_last_occlusion_within_time_buffer) {
+      if (cmp_with_time_buffer(most_recent_occlusion_time_, std::less_equal<double>{})) {
         const auto target_velocity = calcTargetVelocity(path_intersects.front(), *path);
         applySafetySlowDownSpeed(
           *path, path_intersects,
           std::max(target_velocity, planner_param_.occlusion_slow_down_velocity));
+      } else {
+        most_recent_occlusion_time_.reset();
       }
     }
   }