fix logic to avoid chattering

Signed-off-by: Maxime CLEMENT <[email protected]>

planning/behavior_velocity_dynamic_obstacle_stop_module/src/object_filtering.cpp

@@ -65,25 +65,27 @@ std::vector<autoware_auto_perception_msgs::msg::PredictedObject> filter_predicte
     const auto ego_yaw = tf2::getYaw(ego_data.pose.orientation);
     const auto yaw_diff = std::abs(tier4_autoware_utils::normalizeRadian(o_yaw - ego_yaw));
     const auto opposite_heading = yaw_diff > M_PI_2 + M_PI_4;
-    auto distance = std::abs(
-      (o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) -
-      (o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw));
+    const auto collision_distance_threshold =
+      params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + params.hysteresis;
+    auto has_collision = std::abs(
+                           (o_pose.position.y - ego_data.pose.position.y) * std::cos(o_yaw) -
+                           (o_pose.position.x - ego_data.pose.position.x) * std::sin(o_yaw)) <=
+                         collision_distance_threshold;
     if (ego_data.earliest_stop_pose) {
-      distance = std::min(
-        distance,
+      const auto collision_at_earliest_stop_pose =
         std::abs(
           (o_pose.position.y - ego_data.earliest_stop_pose->position.y) * std::cos(o_yaw) -
-          (o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)));
+          (o_pose.position.x - ego_data.earliest_stop_pose->position.x) * std::sin(o_yaw)) <=
+        collision_distance_threshold;
+      has_collision |= collision_at_earliest_stop_pose;
     }
-    return opposite_heading &&
-           distance <= params.ego_lateral_offset + o.shape.dimensions.y / 2.0 + hysteresis;
+    return opposite_heading && has_collision;
   };
   for (const auto & object : objects.objects) {
+    const auto is_not_too_slow = object.kinematics.initial_twist_with_covariance.twist.linear.x >=
+                                 params.minimum_object_velocity;
     if (
-      is_vehicle(object) &&
-      object.kinematics.initial_twist_with_covariance.twist.linear.x >=
-        params.minimum_object_velocity &&
-      is_in_range(object) && is_not_too_close(object) &&
+      is_vehicle(object) && is_not_too_slow && is_in_range(object) && is_not_too_close(object) &&
       (!params.ignore_unavoidable_collisions || !is_unavoidable(object)))
       filtered_objects.push_back(object);
   }

planning/behavior_velocity_dynamic_obstacle_stop_module/src/scene_dynamic_obstacle_stop.cpp

@@ -44,7 +44,9 @@ DynamicObstacleStopModule::DynamicObstacleStopModule(
   const rclcpp::Clock::SharedPtr clock)
 : SceneModuleInterface(module_id, logger, clock), params_(std::move(planner_param))
 {
-  prev_stop_decision_time_ = rclcpp::Time(int64_t{0}, clock->get_clock_type());
+  prev_stop_decision_time_ =
+    clock->now() -
+    rclcpp::Duration(std::chrono::duration<double>(params_.decision_duration_buffer));
   velocity_factor_.init(PlanningBehavior::UNKNOWN);
 }
 
@@ -76,8 +78,9 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
     ego_data.path.points, ego_data.pose.position, min_stop_distance);
 
   make_ego_footprint_rtree(ego_data, params_);
+  const auto start_time = clock_->now();
   const auto has_decided_to_stop =
-    (clock_->now() - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer;
+    (start_time - prev_stop_decision_time_).seconds() < params_.decision_duration_buffer;
   if (!has_decided_to_stop) current_stop_pose_.reset();
   double hysteresis = has_decided_to_stop ? params_.hysteresis : 0.0;
   const auto dynamic_obstacles =
@@ -109,7 +112,7 @@ bool DynamicObstacleStopModule::modifyPathVelocity(PathWithLaneId * path, StopRe
                                                                path->points, stop_pose->position,
                                                                current_stop_pose_->position) > 0.0;
       if (use_new_stop_pose) current_stop_pose_ = *stop_pose;
-      prev_stop_decision_time_ = clock_->now();
+      prev_stop_decision_time_ = start_time;
     }
   }