From deb72a6a79c1dd02c021f464aadee0cc9efdbd76 Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Wed, 23 Oct 2024 16:19:38 +0900
Subject: [PATCH 01/12] refactor(autoware_planning_validator): add collision
 checking for predicted objects

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
---
 .../planning_validator/planning_validator.hpp |  30 ++-
 .../launch/planning_validator.launch.xml      |   1 +
 .../msg/PlanningValidatorStatus.msg           |   1 +
 .../src/planning_validator.cpp                | 173 ++++++++++++++++++
 4 files changed, 204 insertions(+), 1 deletion(-)

diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
index e996855b9b4da..58aff7231c1f1 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
@@ -16,6 +16,7 @@
 #define AUTOWARE__PLANNING_VALIDATOR__PLANNING_VALIDATOR_HPP_
 
 #include "autoware/planning_validator/debug_marker.hpp"
+#include "autoware/universe_utils/geometry/boost_geometry.hpp"
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 #include "autoware/universe_utils/ros/polling_subscriber.hpp"
 #include "autoware/universe_utils/system/stop_watch.hpp"
@@ -26,6 +27,7 @@
 #include <diagnostic_updater/diagnostic_updater.hpp>
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 #include <diagnostic_msgs/msg/diagnostic_array.hpp>
 #include <nav_msgs/msg/odometry.hpp>
@@ -36,7 +38,10 @@
 
 namespace autoware::planning_validator
 {
+using autoware::universe_utils::Polygon2d;
 using autoware::universe_utils::StopWatch;
+using autoware::vehicle_info_utils::VehicleInfo;
+using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 using autoware_planning_validator::msg::PlanningValidatorStatus;
@@ -86,6 +91,26 @@ class PlanningValidator : public rclcpp::Node
   bool checkValidDistanceDeviation(const Trajectory & trajectory);
   bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
   bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
+  bool checkValidNoCollision(const Trajectory & trajectory);
+  /**
+   * @brief Check for potential collisions between the ego vehicle's planned trajectory and the
+   * predicted paths of nearby objects.
+   * This function considers the highest confidence predicted path for each object within a
+   * specified distance threshold and performs collision detection over time.
+   * @param predicted_objects List of predicted objects with their predicted paths.
+   * @param trajectory Planned trajectory of the ego vehicle.
+   * @param current_ego_position Current position of the ego vehicle.
+   * @param vehicle_info Information about the ego vehicle (e.g., dimensions).
+   * @param collision_check_distance_threshold Maximum distance to consider objects for collision
+   * checking.
+   * @return True if a potential collision is detected; false otherwise.
+   */
+  bool checkCollision(
+    const PredictedObjects & objects, const Trajectory & trajectory,
+    const geometry_msgs::msg::Point & current_ego_point, const VehicleInfo & vehicle_info,
+    const double collision_check_distance_threshold = 10.0);
+  Polygon2d createVehicleFootprintPolygon(
+    const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info);
 
 private:
   void setupDiag();
@@ -105,6 +130,8 @@ class PlanningValidator : public rclcpp::Node
 
   autoware::universe_utils::InterProcessPollingSubscriber<Odometry> sub_kinematics_{
     this, "~/input/kinematics"};
+  autoware::universe_utils::InterProcessPollingSubscriber<PredictedObjects> sub_obj_{
+    this, "~/input/objects"};
   rclcpp::Subscription<Trajectory>::SharedPtr sub_traj_;
   rclcpp::Publisher<Trajectory>::SharedPtr pub_traj_;
   rclcpp::Publisher<PlanningValidatorStatus>::SharedPtr pub_status_;
@@ -126,7 +153,7 @@ class PlanningValidator : public rclcpp::Node
   PlanningValidatorStatus validation_status_;
   ValidationParams validation_params_;  // for thresholds
 
-  autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
+  VehicleInfo vehicle_info_;
 
   bool isAllValid(const PlanningValidatorStatus & status) const;
 
@@ -134,6 +161,7 @@ class PlanningValidator : public rclcpp::Node
   Trajectory::ConstSharedPtr previous_published_trajectory_;
 
   Odometry::ConstSharedPtr current_kinematics_;
+  PredictedObjects::ConstSharedPtr current_objects_;
 
   std::shared_ptr<PlanningValidatorDebugMarkerPublisher> debug_pose_publisher_;
 
diff --git a/planning/autoware_planning_validator/launch/planning_validator.launch.xml b/planning/autoware_planning_validator/launch/planning_validator.launch.xml
index aeb9c37d67cda..d695267c3d287 100644
--- a/planning/autoware_planning_validator/launch/planning_validator.launch.xml
+++ b/planning/autoware_planning_validator/launch/planning_validator.launch.xml
@@ -10,6 +10,7 @@
     <!-- remap topic name -->
     <remap from="~/input/trajectory" to="$(var input_trajectory)"/>
     <remap from="~/input/kinematics" to="/localization/kinematic_state"/>
+    <remap from="~/input/objects" to="/perception/object_recognition/objects"/>
     <remap from="~/output/trajectory" to="$(var output_trajectory)"/>
     <remap from="~/output/validation_status" to="~/validation_status"/>
   </node>
diff --git a/planning/autoware_planning_validator/msg/PlanningValidatorStatus.msg b/planning/autoware_planning_validator/msg/PlanningValidatorStatus.msg
index cf127347a6021..a52ec3cdbcfda 100644
--- a/planning/autoware_planning_validator/msg/PlanningValidatorStatus.msg
+++ b/planning/autoware_planning_validator/msg/PlanningValidatorStatus.msg
@@ -15,6 +15,7 @@ bool is_valid_velocity_deviation
 bool is_valid_distance_deviation
 bool is_valid_longitudinal_distance_deviation
 bool is_valid_forward_trajectory_length
+bool is_valid_no_collision
 
 # values
 int64 trajectory_size
diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index 29e8932c87842..f1e7de3c4b7a2 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -15,10 +15,13 @@
 #include "autoware/planning_validator/planning_validator.hpp"
 
 #include "autoware/planning_validator/utils.hpp"
+#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
 
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
+#include <boost/geometry/algorithms/intersects.hpp>
+
 #include <memory>
 #include <string>
 #include <utility>
@@ -181,6 +184,9 @@ bool PlanningValidator::isDataReady()
   if (!current_kinematics_) {
     return waiting("current_kinematics_");
   }
+  if (!current_objects_) {
+    return waiting("current_objects_");
+  }
   if (!current_trajectory_) {
     return waiting("current_trajectory_");
   }
@@ -195,6 +201,7 @@ void PlanningValidator::onTrajectory(const Trajectory::ConstSharedPtr msg)
 
   // receive data
   current_kinematics_ = sub_kinematics_.takeData();
+  current_objects_ = sub_obj_.takeData();
 
   if (!isDataReady()) return;
 
@@ -320,6 +327,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
   s.is_valid_lateral_acc = checkValidLateralAcceleration(resampled);
   s.is_valid_steering = checkValidSteering(resampled);
   s.is_valid_steering_rate = checkValidSteeringRate(resampled);
+  s.is_valid_no_collision = checkValidNoCollision(resampled);
 
   s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
 }
@@ -545,8 +553,169 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra
   return forward_length > forward_length_required;
 }
 
+bool PlanningValidator::checkValidNoCollision(const Trajectory & trajectory)
+{
+  const bool collision_check_unnecessary = current_kinematics_->twist.twist.linear.x < 0.1;
+  if (collision_check_unnecessary) {
+    return true;
+  }
+  const bool is_collision = checkCollision(
+    *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_);
+  return is_collision;
+}
+
+// TODO(Sugahara): move to utils
+bool PlanningValidator::checkCollision(
+  const PredictedObjects & predicted_objects, const Trajectory & trajectory,
+  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
+  const double collision_check_distance_threshold)
+{
+  // TODO(Sugahara): Detect collision if collision is detected in consecutive frames
+
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> resampled_trajectory;
+  resampled_trajectory.reserve(trajectory.points.size());
+
+  if (!trajectory.points.empty()) {
+    resampled_trajectory.push_back(trajectory.points.front());
+
+    constexpr double min_interval = 0.5;
+    for (size_t i = 1; i < trajectory.points.size(); ++i) {
+      const auto & current_point = trajectory.points[i];
+      if (current_point.longitudinal_velocity_mps < 0.1) {
+        continue;
+      }
+      const double distance_to_previous_point =
+        universe_utils::calcDistance2d(resampled_trajectory.back(), current_point);
+      if (distance_to_previous_point > min_interval) {
+        resampled_trajectory.push_back(current_point);
+      }
+    }
+  }
+  motion_utils::calculate_time_from_start(resampled_trajectory, current_ego_position);
+
+  // generate vehicle footprint along the trajectory
+  std::vector<Polygon2d> vehicle_footprints;
+  vehicle_footprints.reserve(resampled_trajectory.size());
+
+  for (const auto & point : resampled_trajectory) {
+    vehicle_footprints.push_back(createVehicleFootprintPolygon(point.pose, vehicle_info));
+  }
+
+  const double time_tolerance = 0.1;  // time tolerance threshold
+  for (const auto & object : predicted_objects.objects) {
+    // Calculate distance between object position and nearest point on trajectory
+    const auto & object_position = object.kinematics.initial_pose_with_covariance.pose.position;
+    const size_t nearest_index =
+      autoware::motion_utils::findNearestIndex(resampled_trajectory, object_position);
+    const double object_distance_to_nearest_point = autoware::universe_utils::calcDistance2d(
+      resampled_trajectory[nearest_index], object_position);
+
+    // Skip collision check if object is too far from trajectory
+    if (object_distance_to_nearest_point > collision_check_distance_threshold) {
+      continue;
+    }
+
+    // Select path with highest confidence from object's predicted paths
+    const auto selected_predicted_path =
+      [&object]() -> const autoware_perception_msgs::msg::PredictedPath * {
+      const auto max_confidence_it = std::max_element(
+        object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(),
+        [](const auto & a, const auto & b) { return a.confidence < b.confidence; });
+
+      return max_confidence_it != object.kinematics.predicted_paths.end() ? &(*max_confidence_it)
+                                                                          : nullptr;
+    }();
+
+    if (!selected_predicted_path) {
+      continue;
+    }
+
+    // Generate polygons for predicted object positions
+    std::vector<Polygon2d> predicted_object_polygons;
+    predicted_object_polygons.reserve(selected_predicted_path->path.size());
+
+    const double predicted_time_step =
+      selected_predicted_path->time_step.sec + selected_predicted_path->time_step.nanosec * 1e-9;
+
+    for (const auto & pose : selected_predicted_path->path) {
+      predicted_object_polygons.push_back(
+        autoware::universe_utils::toPolygon2d(pose, object.shape));
+    }
+
+    // Check for collision (considering time)
+    for (size_t i = 0; i < resampled_trajectory.size(); ++i) {
+      const auto & trajectory_point = resampled_trajectory[i];
+      const double trajectory_time =
+        trajectory_point.time_from_start.sec + trajectory_point.time_from_start.nanosec * 1e-9;
+
+      for (size_t j = 0; j < selected_predicted_path->path.size(); ++j) {
+        const double predicted_time = j * predicted_time_step;
+
+        if (std::fabs(trajectory_time - predicted_time) > time_tolerance) {
+          continue;
+        }
+
+        const double distance = autoware::universe_utils::calcDistance2d(
+          trajectory_point.pose.position, selected_predicted_path->path[j].position);
+
+        if (distance >= 10.0) {
+          continue;
+        }
+
+        if (boost::geometry::intersects(vehicle_footprints[i], predicted_object_polygons[j])) {
+          // Collision detected
+          std::cerr << "Collision detected at " << std::endl;
+          std::cerr << "  trajectory_time: " << trajectory_time << std::endl;
+          std::cerr << "  predicted_time: " << predicted_time << std::endl;
+          std::cerr << "object velocity: "
+                    << object.kinematics.initial_twist_with_covariance.twist.linear.x << "m/s"
+                    << std::endl;
+          return false;
+        }
+      }
+    }
+  }
+
+  return true;
+}
+
+// TODO(Sugahara): move to utils
+Polygon2d PlanningValidator::createVehicleFootprintPolygon(
+  const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info)
+{
+  using autoware::universe_utils::Point2d;
+  const double length = vehicle_info.vehicle_length_m;
+  const double width = vehicle_info.vehicle_width_m;
+  const double rear_overhang = vehicle_info.rear_overhang_m;
+  const double yaw = tf2::getYaw(pose.orientation);
+
+  // Calculate relative positions of vehicle corners
+  std::vector<Point2d> footprint_points{
+    Point2d(length - rear_overhang, width / 2.0), Point2d(length - rear_overhang, -width / 2.0),
+    Point2d(-rear_overhang, -width / 2.0), Point2d(-rear_overhang, width / 2.0)};
+
+  Polygon2d footprint_polygon;
+  footprint_polygon.outer().reserve(footprint_points.size() + 1);
+
+  for (const auto & point : footprint_points) {
+    Point2d transformed_point;
+    transformed_point.x() = pose.position.x + point.x() * std::cos(yaw) - point.y() * std::sin(yaw);
+    transformed_point.y() = pose.position.y + point.x() * std::sin(yaw) + point.y() * std::cos(yaw);
+    footprint_polygon.outer().push_back(transformed_point);
+  }
+
+  footprint_polygon.outer().push_back(footprint_polygon.outer().front());
+
+  return footprint_polygon;
+}
+
 bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
 {
+  // TODO(Sugahara): Add s.is_valid_no_collision after verifying that:
+  // 1. The false value of is_valid_no_collision correctly identifies path problems
+  // 2. Adding this check won't incorrectly invalidate otherwise valid paths
+  // want to avoid false negatives where good paths are marked invalid just because
+  // isAllValid becomes false
   return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval &&
          s.is_valid_relative_angle && s.is_valid_curvature && s.is_valid_lateral_acc &&
          s.is_valid_longitudinal_max_acc && s.is_valid_longitudinal_min_acc &&
@@ -583,6 +752,10 @@ void PlanningValidator::displayStatus()
     s.is_valid_longitudinal_distance_deviation,
     "planning trajectory is too far from ego in longitudinal direction!!");
   warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!");
+  warn(
+    s.is_valid_no_collision,
+    "planning trajectory has collision!! but this validation is not utilized for trajectory "
+    "validation.");
 }
 
 }  // namespace autoware::planning_validator

From 785bd3c508fd6a458c777759218493b5d103a980 Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Wed, 30 Oct 2024 11:56:41 +0900
Subject: [PATCH 02/12] refactor(autoware_planning_validator): remove unused
 code and update collision checking function

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
---
 .../planning_validator/planning_validator.hpp |  22 +--
 .../autoware/planning_validator/utils.hpp     |  32 ++++
 .../src/planning_validator.cpp                | 162 +-----------------
 .../autoware_planning_validator/src/utils.cpp | 137 +++++++++++++++
 4 files changed, 179 insertions(+), 174 deletions(-)

diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
index 58aff7231c1f1..7137870c34028 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
@@ -38,7 +38,6 @@
 
 namespace autoware::planning_validator
 {
-using autoware::universe_utils::Polygon2d;
 using autoware::universe_utils::StopWatch;
 using autoware::vehicle_info_utils::VehicleInfo;
 using autoware_perception_msgs::msg::PredictedObjects;
@@ -91,26 +90,7 @@ class PlanningValidator : public rclcpp::Node
   bool checkValidDistanceDeviation(const Trajectory & trajectory);
   bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
   bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
-  bool checkValidNoCollision(const Trajectory & trajectory);
-  /**
-   * @brief Check for potential collisions between the ego vehicle's planned trajectory and the
-   * predicted paths of nearby objects.
-   * This function considers the highest confidence predicted path for each object within a
-   * specified distance threshold and performs collision detection over time.
-   * @param predicted_objects List of predicted objects with their predicted paths.
-   * @param trajectory Planned trajectory of the ego vehicle.
-   * @param current_ego_position Current position of the ego vehicle.
-   * @param vehicle_info Information about the ego vehicle (e.g., dimensions).
-   * @param collision_check_distance_threshold Maximum distance to consider objects for collision
-   * checking.
-   * @return True if a potential collision is detected; false otherwise.
-   */
-  bool checkCollision(
-    const PredictedObjects & objects, const Trajectory & trajectory,
-    const geometry_msgs::msg::Point & current_ego_point, const VehicleInfo & vehicle_info,
-    const double collision_check_distance_threshold = 10.0);
-  Polygon2d createVehicleFootprintPolygon(
-    const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info);
+  bool checkValidTrajectoryCollision(const Trajectory & trajectory);
 
 private:
   void setupDiag();
diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
index 7d8aca14aff77..feae6fbe714ae 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
@@ -15,8 +15,12 @@
 #ifndef AUTOWARE__PLANNING_VALIDATOR__UTILS_HPP_
 #define AUTOWARE__PLANNING_VALIDATOR__UTILS_HPP_
 
+#include "autoware/universe_utils/geometry/boost_geometry.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
+#include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 
 #include <string>
@@ -25,6 +29,9 @@
 
 namespace autoware::planning_validator
 {
+using autoware::universe_utils::Polygon2d;
+using autoware::vehicle_info_utils::VehicleInfo;
+using autoware_perception_msgs::msg::PredictedObjects;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
 
@@ -57,6 +64,31 @@ std::pair<double, size_t> calcMaxSteeringAngles(
 std::pair<double, size_t> calcMaxSteeringRates(
   const Trajectory & trajectory, const double wheelbase);
 
+/**
+ * @brief Validates trajectory for potential collisions with predicted objects
+ *
+ * This function checks if the planned trajectory will result in any collisions with
+ * predicted objects in the environment. It performs the following steps:
+ * 1. Resamples the trajectory for efficient checking
+ * 2. Generates vehicle footprints along the trajectory
+ * 3. Checks for intersections with predicted object paths
+ *
+ * @param predicted_objects List of predicted objects with their predicted paths.
+ * @param trajectory Planned trajectory of the ego vehicle.
+ * @param current_ego_point Current position of the ego vehicle.
+ * @param vehicle_info Information about the ego vehicle (e.g., dimensions).
+ * @param collision_check_distance_threshold Maximum distance to consider objects for collision
+ * checking.
+ * @return True if a potential collision is detected; false otherwise.
+ */
+bool checkCollision(
+  const PredictedObjects & objects, const Trajectory & trajectory,
+  const geometry_msgs::msg::Point & current_ego_point, const VehicleInfo & vehicle_info,
+  const double collision_check_distance_threshold = 10.0);
+
+Polygon2d createVehicleFootprintPolygon(
+  const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info);
+
 bool checkFinite(const TrajectoryPoint & point);
 
 void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal);
diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index f1e7de3c4b7a2..05ef9ef04bf8c 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -15,13 +15,10 @@
 #include "autoware/planning_validator/planning_validator.hpp"
 
 #include "autoware/planning_validator/utils.hpp"
-#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
 
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
-#include <boost/geometry/algorithms/intersects.hpp>
-
 #include <memory>
 #include <string>
 #include <utility>
@@ -172,6 +169,9 @@ void PlanningValidator::setupDiag()
       stat, validation_status_.is_valid_forward_trajectory_length,
       "trajectory length is too short");
   });
+  d->add(ns + "trajectory_collision", [&](auto & stat) {
+    setStatus(stat, validation_status_.is_valid_no_collision, "collision is detected");
+  });
 }
 
 bool PlanningValidator::isDataReady()
@@ -327,7 +327,7 @@ void PlanningValidator::validate(const Trajectory & trajectory)
   s.is_valid_lateral_acc = checkValidLateralAcceleration(resampled);
   s.is_valid_steering = checkValidSteering(resampled);
   s.is_valid_steering_rate = checkValidSteeringRate(resampled);
-  s.is_valid_no_collision = checkValidNoCollision(resampled);
+  s.is_valid_no_collision = checkValidTrajectoryCollision(resampled);
 
   s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
 }
@@ -553,162 +553,18 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra
   return forward_length > forward_length_required;
 }
 
-bool PlanningValidator::checkValidNoCollision(const Trajectory & trajectory)
+bool PlanningValidator::checkValidTrajectoryCollision(const Trajectory & trajectory)
 {
-  const bool collision_check_unnecessary = current_kinematics_->twist.twist.linear.x < 0.1;
-  if (collision_check_unnecessary) {
-    return true;
+  const auto ego_speed = std::abs(current_kinematics_->twist.twist.linear.x);
+  if (ego_speed < 1.0 / 3.6) {
+    return true;  // Ego is almost stopped.
   }
+
   const bool is_collision = checkCollision(
     *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_);
   return is_collision;
 }
 
-// TODO(Sugahara): move to utils
-bool PlanningValidator::checkCollision(
-  const PredictedObjects & predicted_objects, const Trajectory & trajectory,
-  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
-  const double collision_check_distance_threshold)
-{
-  // TODO(Sugahara): Detect collision if collision is detected in consecutive frames
-
-  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> resampled_trajectory;
-  resampled_trajectory.reserve(trajectory.points.size());
-
-  if (!trajectory.points.empty()) {
-    resampled_trajectory.push_back(trajectory.points.front());
-
-    constexpr double min_interval = 0.5;
-    for (size_t i = 1; i < trajectory.points.size(); ++i) {
-      const auto & current_point = trajectory.points[i];
-      if (current_point.longitudinal_velocity_mps < 0.1) {
-        continue;
-      }
-      const double distance_to_previous_point =
-        universe_utils::calcDistance2d(resampled_trajectory.back(), current_point);
-      if (distance_to_previous_point > min_interval) {
-        resampled_trajectory.push_back(current_point);
-      }
-    }
-  }
-  motion_utils::calculate_time_from_start(resampled_trajectory, current_ego_position);
-
-  // generate vehicle footprint along the trajectory
-  std::vector<Polygon2d> vehicle_footprints;
-  vehicle_footprints.reserve(resampled_trajectory.size());
-
-  for (const auto & point : resampled_trajectory) {
-    vehicle_footprints.push_back(createVehicleFootprintPolygon(point.pose, vehicle_info));
-  }
-
-  const double time_tolerance = 0.1;  // time tolerance threshold
-  for (const auto & object : predicted_objects.objects) {
-    // Calculate distance between object position and nearest point on trajectory
-    const auto & object_position = object.kinematics.initial_pose_with_covariance.pose.position;
-    const size_t nearest_index =
-      autoware::motion_utils::findNearestIndex(resampled_trajectory, object_position);
-    const double object_distance_to_nearest_point = autoware::universe_utils::calcDistance2d(
-      resampled_trajectory[nearest_index], object_position);
-
-    // Skip collision check if object is too far from trajectory
-    if (object_distance_to_nearest_point > collision_check_distance_threshold) {
-      continue;
-    }
-
-    // Select path with highest confidence from object's predicted paths
-    const auto selected_predicted_path =
-      [&object]() -> const autoware_perception_msgs::msg::PredictedPath * {
-      const auto max_confidence_it = std::max_element(
-        object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(),
-        [](const auto & a, const auto & b) { return a.confidence < b.confidence; });
-
-      return max_confidence_it != object.kinematics.predicted_paths.end() ? &(*max_confidence_it)
-                                                                          : nullptr;
-    }();
-
-    if (!selected_predicted_path) {
-      continue;
-    }
-
-    // Generate polygons for predicted object positions
-    std::vector<Polygon2d> predicted_object_polygons;
-    predicted_object_polygons.reserve(selected_predicted_path->path.size());
-
-    const double predicted_time_step =
-      selected_predicted_path->time_step.sec + selected_predicted_path->time_step.nanosec * 1e-9;
-
-    for (const auto & pose : selected_predicted_path->path) {
-      predicted_object_polygons.push_back(
-        autoware::universe_utils::toPolygon2d(pose, object.shape));
-    }
-
-    // Check for collision (considering time)
-    for (size_t i = 0; i < resampled_trajectory.size(); ++i) {
-      const auto & trajectory_point = resampled_trajectory[i];
-      const double trajectory_time =
-        trajectory_point.time_from_start.sec + trajectory_point.time_from_start.nanosec * 1e-9;
-
-      for (size_t j = 0; j < selected_predicted_path->path.size(); ++j) {
-        const double predicted_time = j * predicted_time_step;
-
-        if (std::fabs(trajectory_time - predicted_time) > time_tolerance) {
-          continue;
-        }
-
-        const double distance = autoware::universe_utils::calcDistance2d(
-          trajectory_point.pose.position, selected_predicted_path->path[j].position);
-
-        if (distance >= 10.0) {
-          continue;
-        }
-
-        if (boost::geometry::intersects(vehicle_footprints[i], predicted_object_polygons[j])) {
-          // Collision detected
-          std::cerr << "Collision detected at " << std::endl;
-          std::cerr << "  trajectory_time: " << trajectory_time << std::endl;
-          std::cerr << "  predicted_time: " << predicted_time << std::endl;
-          std::cerr << "object velocity: "
-                    << object.kinematics.initial_twist_with_covariance.twist.linear.x << "m/s"
-                    << std::endl;
-          return false;
-        }
-      }
-    }
-  }
-
-  return true;
-}
-
-// TODO(Sugahara): move to utils
-Polygon2d PlanningValidator::createVehicleFootprintPolygon(
-  const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info)
-{
-  using autoware::universe_utils::Point2d;
-  const double length = vehicle_info.vehicle_length_m;
-  const double width = vehicle_info.vehicle_width_m;
-  const double rear_overhang = vehicle_info.rear_overhang_m;
-  const double yaw = tf2::getYaw(pose.orientation);
-
-  // Calculate relative positions of vehicle corners
-  std::vector<Point2d> footprint_points{
-    Point2d(length - rear_overhang, width / 2.0), Point2d(length - rear_overhang, -width / 2.0),
-    Point2d(-rear_overhang, -width / 2.0), Point2d(-rear_overhang, width / 2.0)};
-
-  Polygon2d footprint_polygon;
-  footprint_polygon.outer().reserve(footprint_points.size() + 1);
-
-  for (const auto & point : footprint_points) {
-    Point2d transformed_point;
-    transformed_point.x() = pose.position.x + point.x() * std::cos(yaw) - point.y() * std::sin(yaw);
-    transformed_point.y() = pose.position.y + point.x() * std::sin(yaw) + point.y() * std::cos(yaw);
-    footprint_polygon.outer().push_back(transformed_point);
-  }
-
-  footprint_polygon.outer().push_back(footprint_polygon.outer().front());
-
-  return footprint_polygon;
-}
-
 bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
 {
   // TODO(Sugahara): Add s.is_valid_no_collision after verifying that:
diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp
index 0a70b4d415dcb..c3a2475f8e9de 100644
--- a/planning/autoware_planning_validator/src/utils.cpp
+++ b/planning/autoware_planning_validator/src/utils.cpp
@@ -14,9 +14,13 @@
 
 #include "autoware/planning_validator/utils.hpp"
 
+#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"
+
 #include <autoware/motion_utils/trajectory/trajectory.hpp>
 #include <autoware/universe_utils/geometry/geometry.hpp>
 
+#include <boost/geometry/algorithms/intersects.hpp>
+
 #include <memory>
 #include <string>
 #include <utility>
@@ -289,6 +293,139 @@ std::pair<double, size_t> calcMaxSteeringRates(
   return {max_steering_rate, max_index};
 }
 
+bool checkCollision(
+  const PredictedObjects & predicted_objects, const Trajectory & trajectory,
+  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
+  const double collision_check_distance_threshold)
+{
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> filtered_trajectory;
+  filtered_trajectory.reserve(trajectory.points.size());
+
+  for (size_t i = 0; i < trajectory.points.size(); ++i) {
+    const auto & point = trajectory.points[i];
+    const double dist_to_point = autoware::motion_utils::calcSignedArcLength(
+      trajectory.points, current_ego_position, size_t(i));
+
+    // Only include points that are ahead of current position (positive distance)
+    if (dist_to_point > 0.0) {
+      filtered_trajectory.push_back(point);
+    }
+  }
+
+  // Calculate timestamps for each trajectory point
+  motion_utils::calculate_time_from_start(filtered_trajectory, current_ego_position);
+
+  // Generate vehicle footprint polygons along the trajectory
+  std::vector<Polygon2d> vehicle_footprints;
+  vehicle_footprints.reserve(filtered_trajectory.size());
+  for (const auto & point : filtered_trajectory) {
+    vehicle_footprints.push_back(createVehicleFootprintPolygon(point.pose, vehicle_info));
+  }
+
+  const double time_tolerance = 0.1;
+  // Check each predicted object for potential collisions
+  for (const auto & object : predicted_objects.objects) {
+    // Calculate distance to object and skip if too far from trajectory
+    const auto & object_position = object.kinematics.initial_pose_with_covariance.pose.position;
+    const size_t nearest_index =
+      autoware::motion_utils::findNearestIndex(filtered_trajectory, object_position);
+    const double object_distance_to_nearest_point =
+      autoware::universe_utils::calcDistance2d(filtered_trajectory[nearest_index], object_position);
+
+    if (object_distance_to_nearest_point > collision_check_distance_threshold) {
+      continue;
+    }
+
+    // Find the predicted path with highest confidence for this object
+    const auto selected_predicted_path =
+      [&object]() -> const autoware_perception_msgs::msg::PredictedPath * {
+      const auto max_confidence_it = std::max_element(
+        object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(),
+        [](const auto & a, const auto & b) { return a.confidence < b.confidence; });
+
+      return max_confidence_it != object.kinematics.predicted_paths.end() ? &(*max_confidence_it)
+                                                                          : nullptr;
+    }();
+
+    // Skip if no valid predicted path exists
+    if (!selected_predicted_path) {
+      continue;
+    }
+
+    // Generate polygons for all predicted positions of the object
+    std::vector<Polygon2d> predicted_object_polygons;
+    predicted_object_polygons.reserve(selected_predicted_path->path.size());
+
+    const double predicted_time_step =
+      selected_predicted_path->time_step.sec + selected_predicted_path->time_step.nanosec * 1e-9;
+
+    for (const auto & pose : selected_predicted_path->path) {
+      predicted_object_polygons.push_back(
+        autoware::universe_utils::toPolygon2d(pose, object.shape));
+    }
+
+    // Check for collisions by comparing vehicle and object polygons at matching timestamps
+    for (size_t i = 0; i < filtered_trajectory.size(); ++i) {
+      const auto & trajectory_point = filtered_trajectory[i];
+      const double trajectory_time =
+        trajectory_point.time_from_start.sec + trajectory_point.time_from_start.nanosec * 1e-9;
+
+      for (size_t j = 0; j < selected_predicted_path->path.size(); ++j) {
+        const double predicted_time = j * predicted_time_step;
+
+        // Skip if timestamps don't match within tolerance
+        if (std::fabs(trajectory_time - predicted_time) > time_tolerance) {
+          continue;
+        }
+
+        // Skip collision check if object is too far away
+        const double distance = autoware::universe_utils::calcDistance2d(
+          trajectory_point.pose.position, selected_predicted_path->path[j].position);
+        if (distance >= 10.0) {
+          continue;
+        }
+
+        // Check for geometric intersection between vehicle and object polygons
+        if (boost::geometry::intersects(vehicle_footprints[i], predicted_object_polygons[j])) {
+          return false;
+        }
+      }
+    }
+  }
+
+  // No collisions detected
+  return true;
+}
+
+Polygon2d createVehicleFootprintPolygon(
+  const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info)
+{
+  using autoware::universe_utils::Point2d;
+  const double length = vehicle_info.vehicle_length_m;
+  const double width = vehicle_info.vehicle_width_m;
+  const double rear_overhang = vehicle_info.rear_overhang_m;
+  const double yaw = tf2::getYaw(pose.orientation);
+
+  // Calculate relative positions of vehicle corners
+  std::vector<Point2d> footprint_points{
+    Point2d(length - rear_overhang, width / 2.0), Point2d(length - rear_overhang, -width / 2.0),
+    Point2d(-rear_overhang, -width / 2.0), Point2d(-rear_overhang, width / 2.0)};
+
+  Polygon2d footprint_polygon;
+  footprint_polygon.outer().reserve(footprint_points.size() + 1);
+
+  for (const auto & point : footprint_points) {
+    Point2d transformed_point;
+    transformed_point.x() = pose.position.x + point.x() * std::cos(yaw) - point.y() * std::sin(yaw);
+    transformed_point.y() = pose.position.y + point.x() * std::sin(yaw) + point.y() * std::cos(yaw);
+    footprint_polygon.outer().push_back(transformed_point);
+  }
+
+  footprint_polygon.outer().push_back(footprint_polygon.outer().front());
+
+  return footprint_polygon;
+}
+
 bool checkFinite(const TrajectoryPoint & point)
 {
   const auto & p = point.pose.position;

From 97b48fa342ae127b6667e61a5b38c00ace5ece25 Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Thu, 31 Oct 2024 20:38:24 +0900
Subject: [PATCH 03/12] refactor(autoware_planning_validator): rename collision
 checking function and update includes

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../autoware/planning_validator/utils.hpp     |  33 ++-
 .../src/planning_validator.cpp                |   2 +-
 .../autoware_planning_validator/src/utils.cpp | 190 ++++++++++--------
 3 files changed, 135 insertions(+), 90 deletions(-)

diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
index feae6fbe714ae..31fbe253f40f1 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
@@ -18,12 +18,14 @@
 #include "autoware/universe_utils/geometry/boost_geometry.hpp"
 #include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
 
-#include <rclcpp/rclcpp.hpp>
-
 #include <autoware_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_planning_msgs/msg/trajectory.hpp>
 
-#include <string>
+#include <boost/geometry.hpp>
+#include <boost/geometry/geometries/box.hpp>
+#include <boost/geometry/geometries/point.hpp>
+#include <boost/geometry/index/rtree.hpp>
+
 #include <utility>
 #include <vector>
 
@@ -31,10 +33,17 @@ namespace autoware::planning_validator
 {
 using autoware::universe_utils::Polygon2d;
 using autoware::vehicle_info_utils::VehicleInfo;
+using autoware_perception_msgs::msg::PredictedObject;
 using autoware_perception_msgs::msg::PredictedObjects;
+using autoware_perception_msgs::msg::PredictedPath;
+using autoware_perception_msgs::msg::Shape;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
+using Point = boost::geometry::model::d2::point_xy<double>;
+using Box = boost::geometry::model::box<Point>;
+using BoxTimePair = std::pair<autoware::universe_utils::Box2d, double>;
 
+using Rtree = boost::geometry::index::rtree<BoxTimePair, boost::geometry::index::rstar<16, 4>>;
 std::pair<double, size_t> getAbsMaxValAndIdx(const std::vector<double> & v);
 
 Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval);
@@ -81,13 +90,25 @@ std::pair<double, size_t> calcMaxSteeringRates(
  * checking.
  * @return True if a potential collision is detected; false otherwise.
  */
-bool checkCollision(
+bool check_collision(
   const PredictedObjects & objects, const Trajectory & trajectory,
   const geometry_msgs::msg::Point & current_ego_point, const VehicleInfo & vehicle_info,
   const double collision_check_distance_threshold = 10.0);
 
-Polygon2d createVehicleFootprintPolygon(
-  const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info);
+Rtree make_ego_footprint_rtree(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const VehicleInfo & vehicle_info);
+
+std::optional<PredictedObjects> filter_objects(
+  const PredictedObjects & objects,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const double collision_check_distance_threshold);
+
+std::optional<PredictedPath> find_highest_confidence_path(const PredictedObject & object);
+
+void make_predicted_object_rtree(
+  const PredictedPath & highest_confidence_path, const Shape & object_shape,
+  const double predicted_time_step, std::vector<BoxTimePair> & predicted_object_rtree_nodes);
 
 bool checkFinite(const TrajectoryPoint & point);
 
diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index 05ef9ef04bf8c..7c2b523aa198f 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -560,7 +560,7 @@ bool PlanningValidator::checkValidTrajectoryCollision(const Trajectory & traject
     return true;  // Ego is almost stopped.
   }
 
-  const bool is_collision = checkCollision(
+  const bool is_collision = check_collision(
     *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_);
   return is_collision;
 }
diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp
index c3a2475f8e9de..779c485edb067 100644
--- a/planning/autoware_planning_validator/src/utils.cpp
+++ b/planning/autoware_planning_validator/src/utils.cpp
@@ -293,7 +293,7 @@ std::pair<double, size_t> calcMaxSteeringRates(
   return {max_steering_rate, max_index};
 }
 
-bool checkCollision(
+bool check_collision(
   const PredictedObjects & predicted_objects, const Trajectory & trajectory,
   const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
   const double collision_check_distance_threshold)
@@ -315,115 +315,139 @@ bool checkCollision(
   // Calculate timestamps for each trajectory point
   motion_utils::calculate_time_from_start(filtered_trajectory, current_ego_position);
 
-  // Generate vehicle footprint polygons along the trajectory
-  std::vector<Polygon2d> vehicle_footprints;
-  vehicle_footprints.reserve(filtered_trajectory.size());
-  for (const auto & point : filtered_trajectory) {
-    vehicle_footprints.push_back(createVehicleFootprintPolygon(point.pose, vehicle_info));
+  const auto & ego_rtree = make_ego_footprint_rtree(filtered_trajectory, vehicle_info);
+
+  const auto filtered_objects =
+    filter_objects(predicted_objects, filtered_trajectory, collision_check_distance_threshold);
+  if (!filtered_objects) {
+    return true;
   }
 
   const double time_tolerance = 0.1;
-  // Check each predicted object for potential collisions
-  for (const auto & object : predicted_objects.objects) {
-    // Calculate distance to object and skip if too far from trajectory
-    const auto & object_position = object.kinematics.initial_pose_with_covariance.pose.position;
-    const size_t nearest_index =
-      autoware::motion_utils::findNearestIndex(filtered_trajectory, object_position);
-    const double object_distance_to_nearest_point =
-      autoware::universe_utils::calcDistance2d(filtered_trajectory[nearest_index], object_position);
 
-    if (object_distance_to_nearest_point > collision_check_distance_threshold) {
+  std::vector<BoxTimePair> predicted_object_rtree_nodes;
+
+  // Check each predicted object for potential collisions
+  for (const auto & object : filtered_objects.value().objects) {
+    const auto & highest_confidence_path = find_highest_confidence_path(object);
+    if (!highest_confidence_path) {
       continue;
     }
 
-    // Find the predicted path with highest confidence for this object
-    const auto selected_predicted_path =
-      [&object]() -> const autoware_perception_msgs::msg::PredictedPath * {
-      const auto max_confidence_it = std::max_element(
-        object.kinematics.predicted_paths.begin(), object.kinematics.predicted_paths.end(),
-        [](const auto & a, const auto & b) { return a.confidence < b.confidence; });
+    const double predicted_time_step = highest_confidence_path.value().time_step.sec +
+                                       highest_confidence_path.value().time_step.nanosec * 1e-9;
 
-      return max_confidence_it != object.kinematics.predicted_paths.end() ? &(*max_confidence_it)
-                                                                          : nullptr;
-    }();
+    make_predicted_object_rtree(
+      highest_confidence_path.value(), object.shape, predicted_time_step,
+      predicted_object_rtree_nodes);
+  }
 
-    // Skip if no valid predicted path exists
-    if (!selected_predicted_path) {
-      continue;
-    }
+  Rtree predicted_object_rtree(
+    predicted_object_rtree_nodes.begin(), predicted_object_rtree_nodes.end());
 
-    // Generate polygons for all predicted positions of the object
-    std::vector<Polygon2d> predicted_object_polygons;
-    predicted_object_polygons.reserve(selected_predicted_path->path.size());
+  for (auto it = ego_rtree.begin(); it != ego_rtree.end(); ++it) {
+    const auto & ego_box_time = *it;
+    const auto & ego_box = ego_box_time.first;
+    const double ego_time = ego_box_time.second;
 
-    const double predicted_time_step =
-      selected_predicted_path->time_step.sec + selected_predicted_path->time_step.nanosec * 1e-9;
+    std::vector<BoxTimePair> potential_collisions;
+    predicted_object_rtree.query(
+      boost::geometry::index::intersects(ego_box) &&
+        boost::geometry::index::satisfies([&](const BoxTimePair & obj) {
+          return std::fabs(obj.second - ego_time) <= time_tolerance;
+        }),
+      std::back_inserter(potential_collisions));
 
-    for (const auto & pose : selected_predicted_path->path) {
-      predicted_object_polygons.push_back(
-        autoware::universe_utils::toPolygon2d(pose, object.shape));
+    for (const auto & obj_box_time : potential_collisions) {
+      const auto & obj_box = obj_box_time.first;
+
+      // Check for geometric intersection between vehicle and object polygons
+      if (boost::geometry::intersects(ego_box, obj_box)) {
+        return false;
+      }
     }
+  }
 
-    // Check for collisions by comparing vehicle and object polygons at matching timestamps
-    for (size_t i = 0; i < filtered_trajectory.size(); ++i) {
-      const auto & trajectory_point = filtered_trajectory[i];
-      const double trajectory_time =
-        trajectory_point.time_from_start.sec + trajectory_point.time_from_start.nanosec * 1e-9;
+  // No collisions detected
+  return true;
+}
 
-      for (size_t j = 0; j < selected_predicted_path->path.size(); ++j) {
-        const double predicted_time = j * predicted_time_step;
+Rtree make_ego_footprint_rtree(
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const VehicleInfo & vehicle_info)
+{
+  autoware::universe_utils::MultiPolygon2d trajectory_footprints;
+  const double base_to_front = vehicle_info.wheel_base_m - vehicle_info.rear_overhang_m;
+  const double base_to_rear = vehicle_info.rear_overhang_m;
+
+  for (const auto & p : trajectory)
+    trajectory_footprints.push_back(autoware::universe_utils::toFootprint(
+      p.pose, base_to_front, base_to_rear, vehicle_info.vehicle_width_m));
+  std::vector<BoxTimePair> rtree_nodes;
+
+  rtree_nodes.reserve(trajectory_footprints.size());
+  for (auto i = 0UL; i < trajectory_footprints.size(); ++i) {
+    const auto box =
+      boost::geometry::return_envelope<autoware::universe_utils::Box2d>(trajectory_footprints[i]);
+    const double time =
+      trajectory[i].time_from_start.sec + trajectory[i].time_from_start.nanosec * 1e-9;
+    rtree_nodes.emplace_back(box, time);
+  }
+  return Rtree(rtree_nodes);
+}
 
-        // Skip if timestamps don't match within tolerance
-        if (std::fabs(trajectory_time - predicted_time) > time_tolerance) {
-          continue;
-        }
+std::optional<PredictedObjects> filter_objects(
+  const PredictedObjects & objects,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const double collision_check_distance_threshold)
+{
+  PredictedObjects filtered_objects;
 
-        // Skip collision check if object is too far away
-        const double distance = autoware::universe_utils::calcDistance2d(
-          trajectory_point.pose.position, selected_predicted_path->path[j].position);
-        if (distance >= 10.0) {
-          continue;
-        }
+  for (const auto & object : objects.objects) {
+    const auto & object_position = object.kinematics.initial_pose_with_covariance.pose.position;
+    const size_t nearest_index =
+      autoware::motion_utils::findNearestIndex(trajectory, object_position);
+    const double object_distance_to_nearest_point =
+      autoware::universe_utils::calcDistance2d(trajectory[nearest_index], object_position);
 
-        // Check for geometric intersection between vehicle and object polygons
-        if (boost::geometry::intersects(vehicle_footprints[i], predicted_object_polygons[j])) {
-          return false;
-        }
-      }
+    if (object_distance_to_nearest_point < collision_check_distance_threshold) {
+      filtered_objects.objects.push_back(object);
     }
   }
 
-  // No collisions detected
-  return true;
+  return filtered_objects.objects.empty() ? std::nullopt : std::make_optional(filtered_objects);
 }
 
-Polygon2d createVehicleFootprintPolygon(
-  const geometry_msgs::msg::Pose & pose, const VehicleInfo & vehicle_info)
+std::optional<PredictedPath> find_highest_confidence_path(const PredictedObject & object)
 {
-  using autoware::universe_utils::Point2d;
-  const double length = vehicle_info.vehicle_length_m;
-  const double width = vehicle_info.vehicle_width_m;
-  const double rear_overhang = vehicle_info.rear_overhang_m;
-  const double yaw = tf2::getYaw(pose.orientation);
-
-  // Calculate relative positions of vehicle corners
-  std::vector<Point2d> footprint_points{
-    Point2d(length - rear_overhang, width / 2.0), Point2d(length - rear_overhang, -width / 2.0),
-    Point2d(-rear_overhang, -width / 2.0), Point2d(-rear_overhang, width / 2.0)};
-
-  Polygon2d footprint_polygon;
-  footprint_polygon.outer().reserve(footprint_points.size() + 1);
-
-  for (const auto & point : footprint_points) {
-    Point2d transformed_point;
-    transformed_point.x() = pose.position.x + point.x() * std::cos(yaw) - point.y() * std::sin(yaw);
-    transformed_point.y() = pose.position.y + point.x() * std::sin(yaw) + point.y() * std::cos(yaw);
-    footprint_polygon.outer().push_back(transformed_point);
+  const auto & paths = object.kinematics.predicted_paths;
+
+  if (paths.empty()) {
+    return std::nullopt;
   }
 
-  footprint_polygon.outer().push_back(footprint_polygon.outer().front());
+  const auto max_confidence_it = std::max_element(
+    paths.begin(), paths.end(),
+    [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });
 
-  return footprint_polygon;
+  return *max_confidence_it;
+}
+
+void make_predicted_object_rtree(
+  const PredictedPath & highest_confidence_path, const Shape & object_shape,
+  const double predicted_time_step, std::vector<BoxTimePair> & predicted_object_rtree_nodes)
+{
+  for (size_t j = 0; j < highest_confidence_path.path.size(); ++j) {
+    const auto & pose = highest_confidence_path.path[j];
+    const double predicted_time = j * predicted_time_step;
+
+    const auto predicted_polygon = autoware::universe_utils::toPolygon2d(pose, object_shape);
+
+    const auto box =
+      boost::geometry::return_envelope<autoware::universe_utils::Box2d>(predicted_polygon);
+
+    predicted_object_rtree_nodes.emplace_back(box, predicted_time);
+  }
 }
 
 bool checkFinite(const TrajectoryPoint & point)

From 7b60f2347267f2ccbf44fbe1e2e7879ecb3b8171 Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 1 Nov 2024 00:32:09 +0900
Subject: [PATCH 04/12] refactor(autoware_planning_validator): update collision
 checking logic and data structures

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../autoware/planning_validator/utils.hpp     | 19 ++--
 .../src/planning_validator.cpp                | 21 ++--
 .../autoware_planning_validator/src/utils.cpp | 96 +++++++++++--------
 3 files changed, 80 insertions(+), 56 deletions(-)

diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
index 31fbe253f40f1..9eb04af04aec6 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
@@ -39,11 +39,11 @@ using autoware_perception_msgs::msg::PredictedPath;
 using autoware_perception_msgs::msg::Shape;
 using autoware_planning_msgs::msg::Trajectory;
 using autoware_planning_msgs::msg::TrajectoryPoint;
-using Point = boost::geometry::model::d2::point_xy<double>;
+using Point = autoware::universe_utils::Point2d;
 using Box = boost::geometry::model::box<Point>;
-using BoxTimePair = std::pair<autoware::universe_utils::Box2d, double>;
+using BoxTimeIndexPair = std::pair<Box, std::pair<double, std::size_t>>;
+using Rtree = boost::geometry::index::rtree<BoxTimeIndexPair, boost::geometry::index::rstar<16, 4>>;
 
-using Rtree = boost::geometry::index::rtree<BoxTimePair, boost::geometry::index::rstar<16, 4>>;
 std::pair<double, size_t> getAbsMaxValAndIdx(const std::vector<double> & v);
 
 Trajectory resampleTrajectory(const Trajectory & trajectory, const double min_interval);
@@ -90,10 +90,9 @@ std::pair<double, size_t> calcMaxSteeringRates(
  * checking.
  * @return True if a potential collision is detected; false otherwise.
  */
-bool check_collision(
+std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_collision(
   const PredictedObjects & objects, const Trajectory & trajectory,
-  const geometry_msgs::msg::Point & current_ego_point, const VehicleInfo & vehicle_info,
-  const double collision_check_distance_threshold = 10.0);
+  const geometry_msgs::msg::Point & current_ego_point, const VehicleInfo & vehicle_info);
 
 Rtree make_ego_footprint_rtree(
   std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
@@ -101,14 +100,16 @@ Rtree make_ego_footprint_rtree(
 
 std::optional<PredictedObjects> filter_objects(
   const PredictedObjects & objects,
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
-  const double collision_check_distance_threshold);
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory);
 
 std::optional<PredictedPath> find_highest_confidence_path(const PredictedObject & object);
 
 void make_predicted_object_rtree(
   const PredictedPath & highest_confidence_path, const Shape & object_shape,
-  const double predicted_time_step, std::vector<BoxTimePair> & predicted_object_rtree_nodes);
+  const double predicted_time_step, std::vector<BoxTimeIndexPair> & predicted_object_rtree_nodes);
+
+std::vector<std::pair<size_t, size_t>> detect_collisions(
+  const Rtree & ego_rtree, const Rtree & predicted_object_rtree, double time_tolerance);
 
 bool checkFinite(const TrajectoryPoint & point);
 
diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index 7c2b523aa198f..9e7cc6281e111 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -560,16 +560,23 @@ bool PlanningValidator::checkValidTrajectoryCollision(const Trajectory & traject
     return true;  // Ego is almost stopped.
   }
 
-  const bool is_collision = check_collision(
+  const auto & collided_points = check_collision(
     *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_);
-  return is_collision;
+
+  if (collided_points) {
+    for (const auto & p : *collided_points) {
+      debug_pose_publisher_->pushPoseMarker(p.pose, "collision", 0);
+    }
+  }
+  return !collided_points;
 }
 
 bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
 {
   // TODO(Sugahara): Add s.is_valid_no_collision after verifying that:
-  // 1. The false value of is_valid_no_collision correctly identifies path problems
+  // 1. The value of is_valid_no_collision correctly identifies path problems
   // 2. Adding this check won't incorrectly invalidate otherwise valid paths
+  //
   // want to avoid false negatives where good paths are marked invalid just because
   // isAllValid becomes false
   return s.is_valid_size && s.is_valid_finite_value && s.is_valid_interval &&
@@ -608,10 +615,10 @@ void PlanningValidator::displayStatus()
     s.is_valid_longitudinal_distance_deviation,
     "planning trajectory is too far from ego in longitudinal direction!!");
   warn(s.is_valid_forward_trajectory_length, "planning trajectory forward length is not enough!!");
-  warn(
-    s.is_valid_no_collision,
-    "planning trajectory has collision!! but this validation is not utilized for trajectory "
-    "validation.");
+  // warn(
+  //   s.is_valid_no_collision,
+  //   "planning trajectory has collision!! but this validation is not utilized for trajectory "
+  //   "validation.");
 }
 
 }  // namespace autoware::planning_validator
diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp
index 779c485edb067..c2561d96a5d79 100644
--- a/planning/autoware_planning_validator/src/utils.cpp
+++ b/planning/autoware_planning_validator/src/utils.cpp
@@ -293,12 +293,12 @@ std::pair<double, size_t> calcMaxSteeringRates(
   return {max_steering_rate, max_index};
 }
 
-bool check_collision(
+std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_collision(
   const PredictedObjects & predicted_objects, const Trajectory & trajectory,
-  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
-  const double collision_check_distance_threshold)
+  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info)
 {
   std::vector<autoware_planning_msgs::msg::TrajectoryPoint> filtered_trajectory;
+
   filtered_trajectory.reserve(trajectory.points.size());
 
   for (size_t i = 0; i < trajectory.points.size(); ++i) {
@@ -311,21 +311,22 @@ bool check_collision(
       filtered_trajectory.push_back(point);
     }
   }
-
+  if (filtered_trajectory.size() == 0) {
+    return std::nullopt;
+  }
   // Calculate timestamps for each trajectory point
   motion_utils::calculate_time_from_start(filtered_trajectory, current_ego_position);
 
   const auto & ego_rtree = make_ego_footprint_rtree(filtered_trajectory, vehicle_info);
 
-  const auto filtered_objects =
-    filter_objects(predicted_objects, filtered_trajectory, collision_check_distance_threshold);
+  const auto filtered_objects = filter_objects(predicted_objects, filtered_trajectory);
   if (!filtered_objects) {
-    return true;
+    return std::nullopt;
   }
 
   const double time_tolerance = 0.1;
 
-  std::vector<BoxTimePair> predicted_object_rtree_nodes;
+  std::vector<BoxTimeIndexPair> predicted_object_rtree_nodes;
 
   // Check each predicted object for potential collisions
   for (const auto & object : filtered_objects.value().objects) {
@@ -345,31 +346,14 @@ bool check_collision(
   Rtree predicted_object_rtree(
     predicted_object_rtree_nodes.begin(), predicted_object_rtree_nodes.end());
 
-  for (auto it = ego_rtree.begin(); it != ego_rtree.end(); ++it) {
-    const auto & ego_box_time = *it;
-    const auto & ego_box = ego_box_time.first;
-    const double ego_time = ego_box_time.second;
-
-    std::vector<BoxTimePair> potential_collisions;
-    predicted_object_rtree.query(
-      boost::geometry::index::intersects(ego_box) &&
-        boost::geometry::index::satisfies([&](const BoxTimePair & obj) {
-          return std::fabs(obj.second - ego_time) <= time_tolerance;
-        }),
-      std::back_inserter(potential_collisions));
-
-    for (const auto & obj_box_time : potential_collisions) {
-      const auto & obj_box = obj_box_time.first;
-
-      // Check for geometric intersection between vehicle and object polygons
-      if (boost::geometry::intersects(ego_box, obj_box)) {
-        return false;
-      }
-    }
+  const auto & collision_index_set =
+    detect_collisions(ego_rtree, predicted_object_rtree, time_tolerance);
+  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> collision_points;
+  for (const auto & [ego_index, obj_index] : collision_index_set) {
+    collision_points.push_back(filtered_trajectory[ego_index]);
   }
 
-  // No collisions detected
-  return true;
+  return collision_points.empty() ? std::nullopt : std::make_optional(collision_points);
 }
 
 Rtree make_ego_footprint_rtree(
@@ -377,13 +361,13 @@ Rtree make_ego_footprint_rtree(
   const VehicleInfo & vehicle_info)
 {
   autoware::universe_utils::MultiPolygon2d trajectory_footprints;
-  const double base_to_front = vehicle_info.wheel_base_m - vehicle_info.rear_overhang_m;
+  const double base_to_front = vehicle_info.wheel_base_m + vehicle_info.front_overhang_m;
   const double base_to_rear = vehicle_info.rear_overhang_m;
 
   for (const auto & p : trajectory)
     trajectory_footprints.push_back(autoware::universe_utils::toFootprint(
       p.pose, base_to_front, base_to_rear, vehicle_info.vehicle_width_m));
-  std::vector<BoxTimePair> rtree_nodes;
+  std::vector<BoxTimeIndexPair> rtree_nodes;
 
   rtree_nodes.reserve(trajectory_footprints.size());
   for (auto i = 0UL; i < trajectory_footprints.size(); ++i) {
@@ -391,26 +375,31 @@ Rtree make_ego_footprint_rtree(
       boost::geometry::return_envelope<autoware::universe_utils::Box2d>(trajectory_footprints[i]);
     const double time =
       trajectory[i].time_from_start.sec + trajectory[i].time_from_start.nanosec * 1e-9;
-    rtree_nodes.emplace_back(box, time);
+    rtree_nodes.emplace_back(box, std::make_pair(time, i));
   }
   return Rtree(rtree_nodes);
 }
 
 std::optional<PredictedObjects> filter_objects(
   const PredictedObjects & objects,
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
-  const double collision_check_distance_threshold)
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory)
 {
   PredictedObjects filtered_objects;
+  constexpr double trajectory_to_object_distance_threshold = 20.0;
+  constexpr double ego_to_object_distance_threshold = 50.0;
 
   for (const auto & object : objects.objects) {
     const auto & object_position = object.kinematics.initial_pose_with_covariance.pose.position;
     const size_t nearest_index =
       autoware::motion_utils::findNearestIndex(trajectory, object_position);
-    const double object_distance_to_nearest_point =
+    const double trajectory_to_object_distance =
       autoware::universe_utils::calcDistance2d(trajectory[nearest_index], object_position);
+    const double ego_to_object_distance =
+      autoware::universe_utils::calcDistance2d(trajectory.front().pose.position, object_position);
 
-    if (object_distance_to_nearest_point < collision_check_distance_threshold) {
+    if (
+      trajectory_to_object_distance < trajectory_to_object_distance_threshold &&
+      ego_to_object_distance < ego_to_object_distance_threshold) {
       filtered_objects.objects.push_back(object);
     }
   }
@@ -435,7 +424,7 @@ std::optional<PredictedPath> find_highest_confidence_path(const PredictedObject
 
 void make_predicted_object_rtree(
   const PredictedPath & highest_confidence_path, const Shape & object_shape,
-  const double predicted_time_step, std::vector<BoxTimePair> & predicted_object_rtree_nodes)
+  const double predicted_time_step, std::vector<BoxTimeIndexPair> & predicted_object_rtree_nodes)
 {
   for (size_t j = 0; j < highest_confidence_path.path.size(); ++j) {
     const auto & pose = highest_confidence_path.path[j];
@@ -446,10 +435,37 @@ void make_predicted_object_rtree(
     const auto box =
       boost::geometry::return_envelope<autoware::universe_utils::Box2d>(predicted_polygon);
 
-    predicted_object_rtree_nodes.emplace_back(box, predicted_time);
+    predicted_object_rtree_nodes.emplace_back(box, std::make_pair(predicted_time, j));
   }
 }
 
+std::vector<std::pair<size_t, size_t>> detect_collisions(
+  const Rtree & ego_rtree, const Rtree & predicted_object_rtree, double time_tolerance)
+{
+  std::vector<std::pair<size_t, size_t>> collision_sets;
+
+  for (const auto & ego_value : ego_rtree) {
+    const auto & ego_box = ego_value.first;
+    const double ego_time = ego_value.second.first;
+
+    std::vector<BoxTimeIndexPair> potential_collisions;
+    predicted_object_rtree.query(
+      boost::geometry::index::intersects(ego_box) &&
+        boost::geometry::index::satisfies([&](const BoxTimeIndexPair & obj_value) {
+          return std::fabs(obj_value.second.first - ego_time) <= time_tolerance;
+        }),
+      std::back_inserter(potential_collisions));
+
+    for (const auto & obj_value : potential_collisions) {
+      if (boost::geometry::intersects(ego_box, obj_value.first)) {
+        collision_sets.emplace_back(ego_value.second.second, obj_value.second.second);
+      }
+    }
+  }
+
+  return collision_sets;
+}
+
 bool checkFinite(const TrajectoryPoint & point)
 {
   const auto & p = point.pose.position;

From c25d99ad228a52908f46ab0bef27a0592abbbe73 Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 1 Nov 2024 01:35:31 +0900
Subject: [PATCH 05/12] docs(autoware_planning_validator): update readme

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../autoware_planning_validator/README.md     |  10 +-
 .../image/planning_validator.drawio.svg       | 465 ++++++++++--------
 2 files changed, 257 insertions(+), 218 deletions(-)

diff --git a/planning/autoware_planning_validator/README.md b/planning/autoware_planning_validator/README.md
index 92c9b86c8b31c..78a8c95fe3772 100644
--- a/planning/autoware_planning_validator/README.md
+++ b/planning/autoware_planning_validator/README.md
@@ -20,6 +20,7 @@ The following features are supported for trajectory validation and can have thre
 - **Distance deviation** : invalid if the ego is too far from the trajectory
 - **Longitudinal distance deviation** : invalid if the trajectory is too far from ego in longitudinal direction
 - **Forward trajectory length** : invalid if the trajectory length is not enough to stop within a given deceleration
+- **Potential collision** : invalid if the planned trajectory is too close to the obstacle in the time horizon
 
 The following features are to be implemented.
 
@@ -31,10 +32,11 @@ The following features are to be implemented.
 
 The `autoware_planning_validator` takes in the following inputs:
 
-| Name                 | Type                              | Description                                    |
-| -------------------- | --------------------------------- | ---------------------------------------------- |
-| `~/input/kinematics` | nav_msgs/Odometry                 | ego pose and twist                             |
-| `~/input/trajectory` | autoware_planning_msgs/Trajectory | target trajectory to be validated in this node |
+| Name                 | Type                                      | Description                                    |
+| -------------------- | ----------------------------------------- | ---------------------------------------------- |
+| `~/input/kinematics` | nav_msgs/Odometry                         | ego pose and twist                             |
+| `~/input/trajectory` | autoware_planning_msgs/Trajectory         | target trajectory to be validated in this node |
+| `~/input/objects`    | autoware_perception_msgs/PredictedObjects | obstacles in the environment                   |
 
 ### Outputs
 
diff --git a/planning/autoware_planning_validator/image/planning_validator.drawio.svg b/planning/autoware_planning_validator/image/planning_validator.drawio.svg
index 0a2267cba159e..723c13dc1360e 100644
--- a/planning/autoware_planning_validator/image/planning_validator.drawio.svg
+++ b/planning/autoware_planning_validator/image/planning_validator.drawio.svg
@@ -1,215 +1,252 @@
-<svg
-  host="65bd71144e"
-  xmlns="http://www.w3.org/2000/svg"
-  xmlns:xlink="http://www.w3.org/1999/xlink"
-  version="1.1"
-  width="496px"
-  height="171px"
-  viewBox="-0.5 -0.5 496 171"
-  content="&lt;mxfile&gt;&lt;diagram id=&quot;U03Aw2_6lbGUJ5V6ckVM&quot; name=&quot;ページ1&quot;&gt;5Vhbb9owGP01PA45CQnksaXtJk1Tq7JL91R5iZt4TWJmOwX26/c5scnF0FHK2CaKVOLj+3eO/Z0w8Kb58i3H8/QDi0k2cFG8HHgXA9edeD78V8CqBsKRBhJO4xpyGmBGfxINIo2WNCai01Aylkk674IRKwoSyQ6GOWeLbrMHlnVnneOEWMAswpmNfqGxTPW2fNTg7whNUjOzg3TNNxw9JpyVhZ6vYAWpa3JshtFNRYpjtmhB3uXAm3LGZP2UL6ckU1E1Eav7XW2pXS+Zk0Lu0mFUd3jCWal3/ZHj7xBNxld6hXJlAkJiiI8uVnvyzlOZZ1By4JEsqbxrPX+FZzR0JhNdvlC6QKawahVuCKc5kYRrrJ5YzbZ1VxoSrOSRbuWuwweCJAwGVFtAnGRY0qfuUFhLI1m3W3e9YRQmcZFWcTDRTGkR+4Z+M4TEPCFS92oiDQ+tZTRQFf/NXLgWF/MMFwUtknvAaIyBFIuTSmYk1pFbpFSS2RxXMVnAqexSpCcgXJLl86G146g7eH4vHkbJi+aIOAZL28cjQNtj34naMyHyXiTXIj5T1wCUogwLQaOeYAuY/c7oUBU6klVAo9mqtGqX9lWtHdpW6PwNkTPYK5Xsoh5z494Q9WGylGwNZElg7AzROGz+/tQBGVvsk4TdP9KCwL1KI3EwBbj+bwTwv3EdeOgwXFsDHY7d0GL3guKkYAKonUksyw387pGQam635qP9mT1wognQbpG2BhoF4bB1GMNJ97CG4XDi7SSGPTg01q1F4o3OYZ9NCjtFLnc0DS/k0hr2gETaZmTgBhlE7fyBVStsqAt+lMxUvBGVjT+DBg6aL5vKyqdnjHd6DFxvgtSnDQWJ+m7ndbD9ylWTej1KRWB49GpgI/WC6m6WrMDpyK6UhOTskUz1YrTUHmiW9SCYKSlU5gCxqCR/rnwThdeDM12R0zhW02x0XV1fNni98XLRuJd1tXdvGy9vQ0rp39j7+C7HNl7HlcMnoSiHdziu5JC2pEBZcXpacPpZ+ZhasN8Zj6uFz80dgGTrmjgxEfiO370QfOeIIvD/dn5QKUGAwe/dBJC6RZlJkzTqq0KshCR5Pe31e9jb5e3t9e3JKWZkWUzPUsz6zeHgign+CcWk8DZoZLGWCq5kZEAsKiHVuYYBqr5zFpcZAdt6VQrCxQlqJ+x5z8C+bcbBBu2MXqwdKDa/gNbmtfmB2bv8BQ==&lt;/diagram&gt;&lt;/mxfile&gt;"
->
-  <defs/>
-  <g>
-    <path d="M 277 30.08 L 480.63 30" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 485.88 30 L 478.88 33.5 L 480.63 30 L 478.88 26.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 30px; margin-left: 382px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
-              >
-                Trajectory
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="382" y="33" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">Trajectory</text>
-      </switch>
+<svg host="65bd71144e" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="496px" height="172px" viewBox="-0.5 -0.5 496 172" content="&lt;mxfile&gt;&lt;diagram id=&quot;U03Aw2_6lbGUJ5V6ckVM&quot; name=&quot;ページ1&quot;&gt;5Vnbcts2EP0aPdYDkiJNPvqWdCbTscdO0/Qpg5AwiRgiFAC0pH59FwQgXiDFisOqyciasYiD+56D3QU1i64W67cCL6s/eEHYLETFehZdz8JwngTwXwMbC0RzA5SCFgYKOuCB/kMsiCza0ILIQUPFOVN0OQRzXtckVwMMC8FXw2aPnA1nXeKSeMBDjpmP/kULVRk0jVGH/05oWbmZA2RrPuP8qRS8qe18Na+JqVlgN4xtKitc8FUPim5m0ZXgXJmnxfqKMG1VZzHT782e2u2SBanVIR0sI8+YNXbX7wX+AtbkYmNXqDbOIKQA+9hiu6foslILBqUAHsmaqo+957/hGZ0FaWrL11oXyBU2vcIdEXRBFBEW8zdh9yV5I3K7mNBAek29NnaXbwmHAfUWkCAMK/o85BRbaZTbdtuud5zCrCGyKk5Sy5QVcezod0MoLEqibK/O0vDQW0YHtfbfzUXocbFkuK5pXX4CjBYYSPE4aWVGCmu5VUUVeVji1kgrOJVDivYa9pkIRdbftKOtjeKRPZySV90RCRxW9Y9HgvbbfmC1b5go+i651sWFdgNQyhmWkuYjwdYw+0enQ10YSFYDnWbb0qZfOli1L0q0Z7p4h+Uc9oNKDtGIuXk2HMKcLk/J3kCeBObZGTrPur//6oCce+yTkn96ojUBv0pzOZkCwvgFAfxqXCcITcO1N9B07GYeu9cUlzWXQO2DwqrZwe8rApLhdm88ej2zEweaQy3tDTRPsrPeYczS4WHNsrM0OkgMr+DQpW49Eu9sDPvgQtgpcnlg0vCdXHrDTkikn4zMwoSBGS8febvCjrrka8NdxW+yTeMvoEGAluuuss3TGReDHrMwSpH+9KGk1N/9uA5pv86qiVmPVhEkPHY1sBGzINPNkxWkNWooJakEfyJXdjFWao+UsREEM5W1jhygHh3kL3WSROF6cGErFrQo9DQ7s65hXrZXiYcnXiE6Hx7k2FLUT7yiHSElmiDvCvzE67hy+FNqyuEOJ7Qcqp4UKK9PTwvBOFbMj6gF/854XC186HwAUj03cWIiiIN45BCCI4og/r/jgw4JEhL8kSeA0C0bplzQMK5CbqQiCzPt7TvY2839/e39ySlm7qWYkaeY7c1hcsUkP4ViKrgNOllspYJbGTkQy1ZIJtZwQPX3ghcNI5C2vmkkEfIEtZONcs/E9zbnyQ7tzCfQTupp5/az9vvTXfl/6KXP6D6xw9Q/zYuAA68ML78IiNHgpU86HHa61wKB/17guH7jnuS8rGEw7SO4k50JLwVIIVdmREblL5qLel7gIAHvuZfsdAwTpSFQ7H4aMVrqfnmKbv4F&lt;/diagram&gt;&lt;/mxfile&gt;">
+    <defs/>
+    <g>
+        <path d="M 277 30.08 L 480.63 30" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+        <path d="M 485.88 30 L 478.88 33.5 L 480.63 30 L 478.88 26.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 30px; margin-left: 382px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
+                                Trajectory
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="382" y="33" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
+                    Trajectory
+                </text>
+            </switch>
+        </g>
+        <rect x="157" y="0" width="120" height="160" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 80px; margin-left: 158px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                                planning_validator
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="217" y="84" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
+                    planning_validator
+                </text>
+            </switch>
+        </g>
+        <path d="M 7 29 L 150.63 29.08" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+        <path d="M 155.88 29.08 L 148.88 32.58 L 150.63 29.08 L 148.88 25.58 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 29px; margin-left: 82px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
+                                Trajectory
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="82" y="32" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
+                    Trajectory
+                </text>
+            </switch>
+        </g>
+        <path d="M 7 80 L 150.63 80" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+        <path d="M 155.88 80 L 148.88 83.5 L 150.63 80 L 148.88 76.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 82px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
+                                ego_kinematics
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="82" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
+                    ego_kinema...
+                </text>
+            </switch>
+        </g>
+        <path d="M 277 79.83 L 480.63 79.99" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+        <path d="M 485.88 80 L 478.88 83.49 L 480.63 79.99 L 478.88 76.49 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 382px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
+                                DiagnosticStatus
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="382" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
+                    DiagnosticStatus
+                </text>
+            </switch>
+        </g>
+        <path d="M 277 130 L 480.63 130" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+        <path d="M 485.88 130 L 478.88 133.5 L 480.63 130 L 478.88 126.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 130px; margin-left: 382px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
+                                PlanningValidatorStatus
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="382" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
+                    PlanningValidatorStatus
+                </text>
+            </switch>
+        </g>
+        <rect x="14" y="32" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 47px; margin-left: 15px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                                <font color="#808080" style="font-size: 10px">
+                                    Trajectory to be validated
+                                </font>
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="79" y="51" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
+                    Trajectory to be vali...
+                </text>
+            </switch>
+        </g>
+        <rect x="17" y="84" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 99px; margin-left: 18px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                                <font color="#808080" style="font-size: 10px">
+                                    Used for the validation
+                                </font>
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="82" y="103" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
+                    Used for the validati...
+                </text>
+            </switch>
+        </g>
+        <rect x="322" y="31" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 46px; margin-left: 323px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                                <font color="#808080" style="font-size: 10px">
+                                    Validated trajectory
+                                </font>
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="387" y="50" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
+                    Validated trajectory
+                </text>
+            </switch>
+        </g>
+        <rect x="287" y="83" width="200" height="30" fill="none" stroke="none" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 98px; margin-left: 288px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                                <font color="#808080" style="font-size: 10px">
+                                    To send validation result to the system: OK/ERROR
+                                </font>
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="387" y="102" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
+                    To send validation result to the...
+                </text>
+            </switch>
+        </g>
+        <rect x="296" y="136" width="176" height="34" fill="none" stroke="none" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 174px; height: 1px; padding-top: 153px; margin-left: 297px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                                <font color="#808080" style="font-size: 10px">
+                                    To show the result and the reason for other modules/users
+                                </font>
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="384" y="157" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
+                    To show the result and the re...
+                </text>
+            </switch>
+        </g>
+        <path d="M 7 130 L 150.63 130.08" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+        <path d="M 155.88 130.08 L 148.88 133.58 L 150.63 130.08 L 148.88 126.58 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 130px; margin-left: 82px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
+                                Objects
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="82" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
+                    Objects
+                </text>
+            </switch>
+        </g>
+        <rect x="14" y="136" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+        <g transform="translate(-0.5 -0.5)">
+            <switch>
+                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 151px; margin-left: 15px;">
+                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                                <font color="#808080" style="font-size: 10px">
+                                    Recognized objects to detect collision
+                                </font>
+                            </div>
+                        </div>
+                    </div>
+                </foreignObject>
+                <text x="79" y="155" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
+                    Recognized objects to...
+                </text>
+            </switch>
+        </g>
     </g>
-    <rect x="157" y="0" width="120" height="160" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 80px; margin-left: 158px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                autoware_planning_validator
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="217" y="84" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">autoware_planning_validator</text>
-      </switch>
-    </g>
-    <path d="M 7 51 L 150.63 51.08" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 155.88 51.08 L 148.88 54.58 L 150.63 51.08 L 148.88 47.58 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 51px; margin-left: 82px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
-              >
-                Trajectory
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="82" y="54" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">Trajectory</text>
-      </switch>
-    </g>
-    <path d="M 7 110 L 150.63 110" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 155.88 110 L 148.88 113.5 L 150.63 110 L 148.88 106.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 110px; margin-left: 82px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
-              >
-                ego_kinematics
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="82" y="113" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">ego_kinema...</text>
-      </switch>
-    </g>
-    <path d="M 277 79.83 L 480.63 79.99" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 485.88 80 L 478.88 83.49 L 480.63 79.99 L 478.88 76.49 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 382px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
-              >
-                DiagnosticStatus
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="382" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">DiagnosticStatus</text>
-      </switch>
-    </g>
-    <path d="M 277 130 L 480.63 130" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-    <path d="M 485.88 130 L 478.88 133.5 L 480.63 130 L 478.88 126.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 130px; margin-left: 382px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div
-                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
-              >
-                PlanningValidatorStatus
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="382" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">PlanningValidatorStatus</text>
-      </switch>
-    </g>
-    <rect x="14" y="54" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 69px; margin-left: 15px;">
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <font style="font-size: 10px" color="#808080">Trajectory to be validated</font>
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="79" y="73" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Trajectory to be vali...</text>
-      </switch>
-    </g>
-    <rect x="17" y="114" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 129px; margin-left: 18px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <font style="font-size: 10px" color="#808080">Used for the validation</font>
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="82" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Used for the validati...</text>
-      </switch>
-    </g>
-    <rect x="322" y="31" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 46px; margin-left: 323px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <font style="font-size: 10px" color="#808080">Validated trajectory</font>
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="387" y="50" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Validated trajectory</text>
-      </switch>
-    </g>
-    <rect x="287" y="83" width="200" height="30" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 98px; margin-left: 288px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <font style="font-size: 10px" color="#808080">To send validation result to the system: OK/ERROR</font>
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="387" y="102" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">To send validation result to the...</text>
-      </switch>
-    </g>
-    <rect x="296" y="136" width="176" height="34" fill="none" stroke="none" pointer-events="all"/>
-    <g transform="translate(-0.5 -0.5)">
-      <switch>
-        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-          <div
-            xmlns="http://www.w3.org/1999/xhtml"
-            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 174px; height: 1px; padding-top: 153px; margin-left: 297px;"
-          >
-            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                <font style="font-size: 10px" color="#808080">To show the result and the reason for other modules/users</font>
-              </div>
-            </div>
-          </div>
-        </foreignObject>
-        <text x="384" y="157" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">To show the result and the re...</text>
-      </switch>
-    </g>
-  </g>
-  <switch>
-    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
-    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
-      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Viewer does not support full SVG 1.1</text>
-    </a>
-  </switch>
-</svg>
+    <switch>
+        <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+        <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+            <text text-anchor="middle" font-size="10px" x="50%" y="100%">
+                Text is not SVG - cannot display
+            </text>
+        </a>
+    </switch>
+</svg>
\ No newline at end of file

From c9b24ec41028c4db3387f5a3f318f9e95f9549e6 Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 1 Nov 2024 01:59:13 +0900
Subject: [PATCH 06/12] refactor(autoware_planning_validator): update collision
 checking logic and data structures

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../config/planning_validator.param.yaml      | 15 +++++++++++
 .../planning_validator/planning_validator.hpp |  3 +++
 .../autoware/planning_validator/utils.hpp     | 27 +++++--------------
 .../src/planning_validator.cpp                |  4 ++-
 .../autoware_planning_validator/src/utils.cpp | 18 +++++++------
 5 files changed, 38 insertions(+), 29 deletions(-)

diff --git a/planning/autoware_planning_validator/config/planning_validator.param.yaml b/planning/autoware_planning_validator/config/planning_validator.param.yaml
index da337d70b1078..27f485d7b1ec3 100644
--- a/planning/autoware_planning_validator/config/planning_validator.param.yaml
+++ b/planning/autoware_planning_validator/config/planning_validator.param.yaml
@@ -37,3 +37,18 @@
       # Setting it to 0 means an error will occur if even slightly exceeding the end of the path,
       # therefore, a certain margin is necessary.
       forward_trajectory_length_margin: 2.0
+
+      # Threshold to filter out objects that are far from the trajectory.
+      # Only objects within this distance [m] from their nearest point on the trajectory
+      # are considered for collision checking to improve computational efficiency.
+      trajectory_to_object_distance_threshold: 30
+
+      # Threshold to filter out objects that are far from the ego vehicle.
+      # Only objects within this distance [m] from the current ego position
+      # are considered for collision checking to improve computational efficiency.
+      ego_to_object_distance_threshold: 50
+      
+      # Time tolerance threshold [s] for collision detection between ego and object trajectories.
+      # Collision check is performed only when the time difference between ego and object
+      # predicted positions is within this threshold to detect temporal-spatial intersections.
+      time_tolerance_threshold: 0.1      
diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
index 7137870c34028..a8603db8f8951 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
@@ -63,6 +63,9 @@ struct ValidationParams
   double velocity_deviation_threshold;
   double distance_deviation_threshold;
   double longitudinal_distance_deviation_threshold;
+  double trajectory_to_object_distance_threshold;
+  double ego_to_object_distance_threshold;
+  double time_tolerance_threshold;
 
   // parameters
   double forward_trajectory_length_acceleration;
diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
index 9eb04af04aec6..864405c127a55 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
@@ -73,26 +73,11 @@ std::pair<double, size_t> calcMaxSteeringAngles(
 std::pair<double, size_t> calcMaxSteeringRates(
   const Trajectory & trajectory, const double wheelbase);
 
-/**
- * @brief Validates trajectory for potential collisions with predicted objects
- *
- * This function checks if the planned trajectory will result in any collisions with
- * predicted objects in the environment. It performs the following steps:
- * 1. Resamples the trajectory for efficient checking
- * 2. Generates vehicle footprints along the trajectory
- * 3. Checks for intersections with predicted object paths
- *
- * @param predicted_objects List of predicted objects with their predicted paths.
- * @param trajectory Planned trajectory of the ego vehicle.
- * @param current_ego_point Current position of the ego vehicle.
- * @param vehicle_info Information about the ego vehicle (e.g., dimensions).
- * @param collision_check_distance_threshold Maximum distance to consider objects for collision
- * checking.
- * @return True if a potential collision is detected; false otherwise.
- */
 std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_collision(
-  const PredictedObjects & objects, const Trajectory & trajectory,
-  const geometry_msgs::msg::Point & current_ego_point, const VehicleInfo & vehicle_info);
+  const PredictedObjects & predicted_objects, const Trajectory & trajectory,
+  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
+  const double trajectory_to_object_distance_threshold, ,
+  const double ego_to_object_distance_threshold, const double time_tolerance_threshold);
 
 Rtree make_ego_footprint_rtree(
   std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
@@ -100,7 +85,9 @@ Rtree make_ego_footprint_rtree(
 
 std::optional<PredictedObjects> filter_objects(
   const PredictedObjects & objects,
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory);
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const double trajectory_to_object_distance_threshold,
+  const double ego_to_object_distance_threshold);
 
 std::optional<PredictedPath> find_highest_confidence_path(const PredictedObject & object);
 
diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index 9e7cc6281e111..e015689d9299c 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -561,7 +561,9 @@ bool PlanningValidator::checkValidTrajectoryCollision(const Trajectory & traject
   }
 
   const auto & collided_points = check_collision(
-    *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_);
+    *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_,
+    validator_params_.trajectory_to_object_distance_threshold,
+    validator_params_.ego_to_object_distance_threshold, validator_params_.time_tolerance_threshold);
 
   if (collided_points) {
     for (const auto & p : *collided_points) {
diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp
index c2561d96a5d79..25c6f0d9d66ab 100644
--- a/planning/autoware_planning_validator/src/utils.cpp
+++ b/planning/autoware_planning_validator/src/utils.cpp
@@ -295,7 +295,9 @@ std::pair<double, size_t> calcMaxSteeringRates(
 
 std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_collision(
   const PredictedObjects & predicted_objects, const Trajectory & trajectory,
-  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info)
+  const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
+  const double trajectory_to_object_distance_threshold, ,
+  const double ego_to_object_distance_threshold, const double time_tolerance_threshold)
 {
   std::vector<autoware_planning_msgs::msg::TrajectoryPoint> filtered_trajectory;
 
@@ -319,13 +321,13 @@ std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_c
 
   const auto & ego_rtree = make_ego_footprint_rtree(filtered_trajectory, vehicle_info);
 
-  const auto filtered_objects = filter_objects(predicted_objects, filtered_trajectory);
+  const auto filtered_objects = filter_objects(
+    predicted_objects, filtered_trajectory, trajectory_to_object_distance_threshold,
+    ego_to_object_distance_threshold);
   if (!filtered_objects) {
     return std::nullopt;
   }
 
-  const double time_tolerance = 0.1;
-
   std::vector<BoxTimeIndexPair> predicted_object_rtree_nodes;
 
   // Check each predicted object for potential collisions
@@ -347,7 +349,7 @@ std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_c
     predicted_object_rtree_nodes.begin(), predicted_object_rtree_nodes.end());
 
   const auto & collision_index_set =
-    detect_collisions(ego_rtree, predicted_object_rtree, time_tolerance);
+    detect_collisions(ego_rtree, predicted_object_rtree, time_tolerance_threshold);
   std::vector<autoware_planning_msgs::msg::TrajectoryPoint> collision_points;
   for (const auto & [ego_index, obj_index] : collision_index_set) {
     collision_points.push_back(filtered_trajectory[ego_index]);
@@ -382,11 +384,11 @@ Rtree make_ego_footprint_rtree(
 
 std::optional<PredictedObjects> filter_objects(
   const PredictedObjects & objects,
-  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory)
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const double trajectory_to_object_distance_threshold,
+  const double ego_to_object_distance_threshold)
 {
   PredictedObjects filtered_objects;
-  constexpr double trajectory_to_object_distance_threshold = 20.0;
-  constexpr double ego_to_object_distance_threshold = 50.0;
 
   for (const auto & object : objects.objects) {
     const auto & object_position = object.kinematics.initial_pose_with_covariance.pose.position;

From ff29a48a60b676bd09164d0ff452c67d971d2b99 Mon Sep 17 00:00:00 2001
From: "pre-commit-ci[bot]"
 <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Date: Thu, 31 Oct 2024 17:15:50 +0000
Subject: [PATCH 07/12] style(pre-commit): autofix

---
 .../config/planning_validator.param.yaml      |   4 +-
 .../image/planning_validator.drawio.svg       | 498 +++++++++---------
 2 files changed, 249 insertions(+), 253 deletions(-)

diff --git a/planning/autoware_planning_validator/config/planning_validator.param.yaml b/planning/autoware_planning_validator/config/planning_validator.param.yaml
index 27f485d7b1ec3..742b09973ae9f 100644
--- a/planning/autoware_planning_validator/config/planning_validator.param.yaml
+++ b/planning/autoware_planning_validator/config/planning_validator.param.yaml
@@ -47,8 +47,8 @@
       # Only objects within this distance [m] from the current ego position
       # are considered for collision checking to improve computational efficiency.
       ego_to_object_distance_threshold: 50
-      
+
       # Time tolerance threshold [s] for collision detection between ego and object trajectories.
       # Collision check is performed only when the time difference between ego and object
       # predicted positions is within this threshold to detect temporal-spatial intersections.
-      time_tolerance_threshold: 0.1      
+      time_tolerance_threshold: 0.1
diff --git a/planning/autoware_planning_validator/image/planning_validator.drawio.svg b/planning/autoware_planning_validator/image/planning_validator.drawio.svg
index 723c13dc1360e..9069f5c84b7eb 100644
--- a/planning/autoware_planning_validator/image/planning_validator.drawio.svg
+++ b/planning/autoware_planning_validator/image/planning_validator.drawio.svg
@@ -1,252 +1,248 @@
-<svg host="65bd71144e" xmlns="http://www.w3.org/2000/svg" xmlns:xlink="http://www.w3.org/1999/xlink" version="1.1" width="496px" height="172px" viewBox="-0.5 -0.5 496 172" content="&lt;mxfile&gt;&lt;diagram id=&quot;U03Aw2_6lbGUJ5V6ckVM&quot; name=&quot;ページ1&quot;&gt;5Vnbcts2EP0aPdYDkiJNPvqWdCbTscdO0/Qpg5AwiRgiFAC0pH59FwQgXiDFisOqyciasYiD+56D3QU1i64W67cCL6s/eEHYLETFehZdz8JwngTwXwMbC0RzA5SCFgYKOuCB/kMsiCza0ILIQUPFOVN0OQRzXtckVwMMC8FXw2aPnA1nXeKSeMBDjpmP/kULVRk0jVGH/05oWbmZA2RrPuP8qRS8qe18Na+JqVlgN4xtKitc8FUPim5m0ZXgXJmnxfqKMG1VZzHT782e2u2SBanVIR0sI8+YNXbX7wX+AtbkYmNXqDbOIKQA+9hiu6foslILBqUAHsmaqo+957/hGZ0FaWrL11oXyBU2vcIdEXRBFBEW8zdh9yV5I3K7mNBAek29NnaXbwmHAfUWkCAMK/o85BRbaZTbdtuud5zCrCGyKk5Sy5QVcezod0MoLEqibK/O0vDQW0YHtfbfzUXocbFkuK5pXX4CjBYYSPE4aWVGCmu5VUUVeVji1kgrOJVDivYa9pkIRdbftKOtjeKRPZySV90RCRxW9Y9HgvbbfmC1b5go+i651sWFdgNQyhmWkuYjwdYw+0enQ10YSFYDnWbb0qZfOli1L0q0Z7p4h+Uc9oNKDtGIuXk2HMKcLk/J3kCeBObZGTrPur//6oCce+yTkn96ojUBv0pzOZkCwvgFAfxqXCcITcO1N9B07GYeu9cUlzWXQO2DwqrZwe8rApLhdm88ej2zEweaQy3tDTRPsrPeYczS4WHNsrM0OkgMr+DQpW49Eu9sDPvgQtgpcnlg0vCdXHrDTkikn4zMwoSBGS8febvCjrrka8NdxW+yTeMvoEGAluuuss3TGReDHrMwSpH+9KGk1N/9uA5pv86qiVmPVhEkPHY1sBGzINPNkxWkNWooJakEfyJXdjFWao+UsREEM5W1jhygHh3kL3WSROF6cGErFrQo9DQ7s65hXrZXiYcnXiE6Hx7k2FLUT7yiHSElmiDvCvzE67hy+FNqyuEOJ7Qcqp4UKK9PTwvBOFbMj6gF/854XC186HwAUj03cWIiiIN45BCCI4og/r/jgw4JEhL8kSeA0C0bplzQMK5CbqQiCzPt7TvY2839/e39ySlm7qWYkaeY7c1hcsUkP4ViKrgNOllspYJbGTkQy1ZIJtZwQPX3ghcNI5C2vmkkEfIEtZONcs/E9zbnyQ7tzCfQTupp5/az9vvTXfl/6KXP6D6xw9Q/zYuAA68ML78IiNHgpU86HHa61wKB/17guH7jnuS8rGEw7SO4k50JLwVIIVdmREblL5qLel7gIAHvuZfsdAwTpSFQ7H4aMVrqfnmKbv4F&lt;/diagram&gt;&lt;/mxfile&gt;">
-    <defs/>
-    <g>
-        <path d="M 277 30.08 L 480.63 30" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-        <path d="M 485.88 30 L 478.88 33.5 L 480.63 30 L 478.88 26.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 30px; margin-left: 382px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
-                                Trajectory
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="382" y="33" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
-                    Trajectory
-                </text>
-            </switch>
-        </g>
-        <rect x="157" y="0" width="120" height="160" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 80px; margin-left: 158px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                                planning_validator
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="217" y="84" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
-                    planning_validator
-                </text>
-            </switch>
-        </g>
-        <path d="M 7 29 L 150.63 29.08" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-        <path d="M 155.88 29.08 L 148.88 32.58 L 150.63 29.08 L 148.88 25.58 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 29px; margin-left: 82px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
-                                Trajectory
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="82" y="32" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
-                    Trajectory
-                </text>
-            </switch>
-        </g>
-        <path d="M 7 80 L 150.63 80" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-        <path d="M 155.88 80 L 148.88 83.5 L 150.63 80 L 148.88 76.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 82px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
-                                ego_kinematics
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="82" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
-                    ego_kinema...
-                </text>
-            </switch>
-        </g>
-        <path d="M 277 79.83 L 480.63 79.99" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-        <path d="M 485.88 80 L 478.88 83.49 L 480.63 79.99 L 478.88 76.49 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 382px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
-                                DiagnosticStatus
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="382" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
-                    DiagnosticStatus
-                </text>
-            </switch>
-        </g>
-        <path d="M 277 130 L 480.63 130" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-        <path d="M 485.88 130 L 478.88 133.5 L 480.63 130 L 478.88 126.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 130px; margin-left: 382px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
-                                PlanningValidatorStatus
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="382" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
-                    PlanningValidatorStatus
-                </text>
-            </switch>
-        </g>
-        <rect x="14" y="32" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 47px; margin-left: 15px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                                <font color="#808080" style="font-size: 10px">
-                                    Trajectory to be validated
-                                </font>
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="79" y="51" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
-                    Trajectory to be vali...
-                </text>
-            </switch>
-        </g>
-        <rect x="17" y="84" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 99px; margin-left: 18px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                                <font color="#808080" style="font-size: 10px">
-                                    Used for the validation
-                                </font>
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="82" y="103" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
-                    Used for the validati...
-                </text>
-            </switch>
-        </g>
-        <rect x="322" y="31" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 46px; margin-left: 323px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                                <font color="#808080" style="font-size: 10px">
-                                    Validated trajectory
-                                </font>
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="387" y="50" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
-                    Validated trajectory
-                </text>
-            </switch>
-        </g>
-        <rect x="287" y="83" width="200" height="30" fill="none" stroke="none" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 98px; margin-left: 288px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                                <font color="#808080" style="font-size: 10px">
-                                    To send validation result to the system: OK/ERROR
-                                </font>
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="387" y="102" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
-                    To send validation result to the...
-                </text>
-            </switch>
-        </g>
-        <rect x="296" y="136" width="176" height="34" fill="none" stroke="none" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 174px; height: 1px; padding-top: 153px; margin-left: 297px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                                <font color="#808080" style="font-size: 10px">
-                                    To show the result and the reason for other modules/users
-                                </font>
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="384" y="157" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
-                    To show the result and the re...
-                </text>
-            </switch>
-        </g>
-        <path d="M 7 130 L 150.63 130.08" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
-        <path d="M 155.88 130.08 L 148.88 133.58 L 150.63 130.08 L 148.88 126.58 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 130px; margin-left: 82px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;">
-                                Objects
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="82" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">
-                    Objects
-                </text>
-            </switch>
-        </g>
-        <rect x="14" y="136" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
-        <g transform="translate(-0.5 -0.5)">
-            <switch>
-                <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
-                    <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 151px; margin-left: 15px;">
-                        <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
-                            <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
-                                <font color="#808080" style="font-size: 10px">
-                                    Recognized objects to detect collision
-                                </font>
-                            </div>
-                        </div>
-                    </div>
-                </foreignObject>
-                <text x="79" y="155" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">
-                    Recognized objects to...
-                </text>
-            </switch>
-        </g>
+<svg
+  host="65bd71144e"
+  xmlns="http://www.w3.org/2000/svg"
+  xmlns:xlink="http://www.w3.org/1999/xlink"
+  version="1.1"
+  width="496px"
+  height="172px"
+  viewBox="-0.5 -0.5 496 172"
+  content="&lt;mxfile&gt;&lt;diagram id=&quot;U03Aw2_6lbGUJ5V6ckVM&quot; name=&quot;ページ1&quot;&gt;5Vnbcts2EP0aPdYDkiJNPvqWdCbTscdO0/Qpg5AwiRgiFAC0pH59FwQgXiDFisOqyciasYiD+56D3QU1i64W67cCL6s/eEHYLETFehZdz8JwngTwXwMbC0RzA5SCFgYKOuCB/kMsiCza0ILIQUPFOVN0OQRzXtckVwMMC8FXw2aPnA1nXeKSeMBDjpmP/kULVRk0jVGH/05oWbmZA2RrPuP8qRS8qe18Na+JqVlgN4xtKitc8FUPim5m0ZXgXJmnxfqKMG1VZzHT782e2u2SBanVIR0sI8+YNXbX7wX+AtbkYmNXqDbOIKQA+9hiu6foslILBqUAHsmaqo+957/hGZ0FaWrL11oXyBU2vcIdEXRBFBEW8zdh9yV5I3K7mNBAek29NnaXbwmHAfUWkCAMK/o85BRbaZTbdtuud5zCrCGyKk5Sy5QVcezod0MoLEqibK/O0vDQW0YHtfbfzUXocbFkuK5pXX4CjBYYSPE4aWVGCmu5VUUVeVji1kgrOJVDivYa9pkIRdbftKOtjeKRPZySV90RCRxW9Y9HgvbbfmC1b5go+i651sWFdgNQyhmWkuYjwdYw+0enQ10YSFYDnWbb0qZfOli1L0q0Z7p4h+Uc9oNKDtGIuXk2HMKcLk/J3kCeBObZGTrPur//6oCce+yTkn96ojUBv0pzOZkCwvgFAfxqXCcITcO1N9B07GYeu9cUlzWXQO2DwqrZwe8rApLhdm88ej2zEweaQy3tDTRPsrPeYczS4WHNsrM0OkgMr+DQpW49Eu9sDPvgQtgpcnlg0vCdXHrDTkikn4zMwoSBGS8febvCjrrka8NdxW+yTeMvoEGAluuuss3TGReDHrMwSpH+9KGk1N/9uA5pv86qiVmPVhEkPHY1sBGzINPNkxWkNWooJakEfyJXdjFWao+UsREEM5W1jhygHh3kL3WSROF6cGErFrQo9DQ7s65hXrZXiYcnXiE6Hx7k2FLUT7yiHSElmiDvCvzE67hy+FNqyuEOJ7Qcqp4UKK9PTwvBOFbMj6gF/854XC186HwAUj03cWIiiIN45BCCI4og/r/jgw4JEhL8kSeA0C0bplzQMK5CbqQiCzPt7TvY2839/e39ySlm7qWYkaeY7c1hcsUkP4ViKrgNOllspYJbGTkQy1ZIJtZwQPX3ghcNI5C2vmkkEfIEtZONcs/E9zbnyQ7tzCfQTupp5/az9vvTXfl/6KXP6D6xw9Q/zYuAA68ML78IiNHgpU86HHa61wKB/17guH7jnuS8rGEw7SO4k50JLwVIIVdmREblL5qLel7gIAHvuZfsdAwTpSFQ7H4aMVrqfnmKbv4F&lt;/diagram&gt;&lt;/mxfile&gt;"
+>
+  <defs/>
+  <g>
+    <path d="M 277 30.08 L 480.63 30" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 485.88 30 L 478.88 33.5 L 480.63 30 L 478.88 26.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 30px; margin-left: 382px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
+              >
+                Trajectory
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="382" y="33" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">Trajectory</text>
+      </switch>
     </g>
-    <switch>
-        <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
-        <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
-            <text text-anchor="middle" font-size="10px" x="50%" y="100%">
-                Text is not SVG - cannot display
-            </text>
-        </a>
-    </switch>
-</svg>
\ No newline at end of file
+    <rect x="157" y="0" width="120" height="160" fill="rgb(255, 255, 255)" stroke="rgb(0, 0, 0)" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 118px; height: 1px; padding-top: 80px; margin-left: 158px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                planning_validator
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="217" y="84" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">planning_validator</text>
+      </switch>
+    </g>
+    <path d="M 7 29 L 150.63 29.08" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 155.88 29.08 L 148.88 32.58 L 150.63 29.08 L 148.88 25.58 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 29px; margin-left: 82px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
+              >
+                Trajectory
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="82" y="32" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">Trajectory</text>
+      </switch>
+    </g>
+    <path d="M 7 80 L 150.63 80" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 155.88 80 L 148.88 83.5 L 150.63 80 L 148.88 76.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 82px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
+              >
+                ego_kinematics
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="82" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">ego_kinema...</text>
+      </switch>
+    </g>
+    <path d="M 277 79.83 L 480.63 79.99" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 485.88 80 L 478.88 83.49 L 480.63 79.99 L 478.88 76.49 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 80px; margin-left: 382px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
+              >
+                DiagnosticStatus
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="382" y="83" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">DiagnosticStatus</text>
+      </switch>
+    </g>
+    <path d="M 277 130 L 480.63 130" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 485.88 130 L 478.88 133.5 L 480.63 130 L 478.88 126.5 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 130px; margin-left: 382px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
+              >
+                PlanningValidatorStatus
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="382" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">PlanningValidatorStatus</text>
+      </switch>
+    </g>
+    <rect x="14" y="32" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 47px; margin-left: 15px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <font color="#808080" style="font-size: 10px">Trajectory to be validated</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="79" y="51" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Trajectory to be vali...</text>
+      </switch>
+    </g>
+    <rect x="17" y="84" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 99px; margin-left: 18px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <font color="#808080" style="font-size: 10px">Used for the validation</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="82" y="103" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Used for the validati...</text>
+      </switch>
+    </g>
+    <rect x="322" y="31" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 46px; margin-left: 323px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <font color="#808080" style="font-size: 10px">Validated trajectory</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="387" y="50" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Validated trajectory</text>
+      </switch>
+    </g>
+    <rect x="287" y="83" width="200" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 198px; height: 1px; padding-top: 98px; margin-left: 288px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <font color="#808080" style="font-size: 10px">To send validation result to the system: OK/ERROR</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="387" y="102" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">To send validation result to the...</text>
+      </switch>
+    </g>
+    <rect x="296" y="136" width="176" height="34" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 174px; height: 1px; padding-top: 153px; margin-left: 297px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <font color="#808080" style="font-size: 10px">To show the result and the reason for other modules/users</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="384" y="157" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">To show the result and the re...</text>
+      </switch>
+    </g>
+    <path d="M 7 130 L 150.63 130.08" fill="none" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="stroke"/>
+    <path d="M 155.88 130.08 L 148.88 133.58 L 150.63 130.08 L 148.88 126.58 Z" fill="rgb(0, 0, 0)" stroke="rgb(0, 0, 0)" stroke-miterlimit="10" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div xmlns="http://www.w3.org/1999/xhtml" style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 1px; height: 1px; padding-top: 130px; margin-left: 82px;">
+            <div data-drawio-colors="color: rgb(0, 0, 0); background-color: rgb(255, 255, 255); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div
+                style="display: inline-block; font-size: 11px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; background-color: rgb(255, 255, 255); white-space: nowrap;"
+              >
+                Objects
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="82" y="133" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="11px" text-anchor="middle">Objects</text>
+      </switch>
+    </g>
+    <rect x="14" y="136" width="130" height="30" fill="none" stroke="none" pointer-events="all"/>
+    <g transform="translate(-0.5 -0.5)">
+      <switch>
+        <foreignObject pointer-events="none" width="100%" height="100%" requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility" style="overflow: visible; text-align: left;">
+          <div
+            xmlns="http://www.w3.org/1999/xhtml"
+            style="display: flex; align-items: unsafe center; justify-content: unsafe center; width: 128px; height: 1px; padding-top: 151px; margin-left: 15px;"
+          >
+            <div data-drawio-colors="color: rgb(0, 0, 0); " style="box-sizing: border-box; font-size: 0px; text-align: center;">
+              <div style="display: inline-block; font-size: 12px; font-family: Helvetica; color: rgb(0, 0, 0); line-height: 1.2; pointer-events: all; white-space: normal; overflow-wrap: normal;">
+                <font color="#808080" style="font-size: 10px">Recognized objects to detect collision</font>
+              </div>
+            </div>
+          </div>
+        </foreignObject>
+        <text x="79" y="155" fill="rgb(0, 0, 0)" font-family="Helvetica" font-size="12px" text-anchor="middle">Recognized objects to...</text>
+      </switch>
+    </g>
+  </g>
+  <switch>
+    <g requiredFeatures="http://www.w3.org/TR/SVG11/feature#Extensibility"/>
+    <a transform="translate(0,-5)" xlink:href="https://www.diagrams.net/doc/faq/svg-export-text-problems" target="_blank">
+      <text text-anchor="middle" font-size="10px" x="50%" y="100%">Text is not SVG - cannot display</text>
+    </a>
+  </switch>
+</svg>

From 1a68910d1be4f614a5bf40f6904da60b0ddeb7a8 Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 1 Nov 2024 02:29:22 +0900
Subject: [PATCH 08/12] fix(planning_validator): remove syntax error in
 check_collision function parameters

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../include/autoware/planning_validator/utils.hpp            | 2 +-
 .../autoware_planning_validator/src/planning_validator.cpp   | 5 +++--
 planning/autoware_planning_validator/src/utils.cpp           | 2 +-
 3 files changed, 5 insertions(+), 4 deletions(-)

diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
index 864405c127a55..0d3e3bff7add3 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
@@ -76,7 +76,7 @@ std::pair<double, size_t> calcMaxSteeringRates(
 std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_collision(
   const PredictedObjects & predicted_objects, const Trajectory & trajectory,
   const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
-  const double trajectory_to_object_distance_threshold, ,
+  const double trajectory_to_object_distance_threshold,
   const double ego_to_object_distance_threshold, const double time_tolerance_threshold);
 
 Rtree make_ego_footprint_rtree(
diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index e015689d9299c..0b9b028ced4ea 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -562,8 +562,9 @@ bool PlanningValidator::checkValidTrajectoryCollision(const Trajectory & traject
 
   const auto & collided_points = check_collision(
     *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_,
-    validator_params_.trajectory_to_object_distance_threshold,
-    validator_params_.ego_to_object_distance_threshold, validator_params_.time_tolerance_threshold);
+    validation_params_.trajectory_to_object_distance_threshold,
+    validation_params_.ego_to_object_distance_threshold,
+    validation_params_.time_tolerance_threshold);
 
   if (collided_points) {
     for (const auto & p : *collided_points) {
diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp
index 25c6f0d9d66ab..b12d104ed29d9 100644
--- a/planning/autoware_planning_validator/src/utils.cpp
+++ b/planning/autoware_planning_validator/src/utils.cpp
@@ -296,7 +296,7 @@ std::pair<double, size_t> calcMaxSteeringRates(
 std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_collision(
   const PredictedObjects & predicted_objects, const Trajectory & trajectory,
   const geometry_msgs::msg::Point & current_ego_position, const VehicleInfo & vehicle_info,
-  const double trajectory_to_object_distance_threshold, ,
+  const double trajectory_to_object_distance_threshold,
   const double ego_to_object_distance_threshold, const double time_tolerance_threshold)
 {
   std::vector<autoware_planning_msgs::msg::TrajectoryPoint> filtered_trajectory;

From b7d09cb483fbb69ae729b3c0af132b4634e14fb9 Mon Sep 17 00:00:00 2001
From: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
Date: Fri, 1 Nov 2024 09:38:58 +0900
Subject: [PATCH 09/12] refactor(autoware_planning_validator): add new
 parameters for trajectory and ego-object distance thresholds

Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp>
---
 .../autoware_planning_validator/src/planning_validator.cpp   | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index 0b9b028ced4ea..8585c156c6bad 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -87,6 +87,11 @@ void PlanningValidator::setupParameters()
       declare_parameter<double>(ps + "forward_trajectory_length_acceleration");
     p.forward_trajectory_length_margin =
       declare_parameter<double>(ps + "forward_trajectory_length_margin");
+    p.trajectory_to_object_distance_threshold =
+      declare_parameter<double>(ps + "trajectory_to_object_distance_threshold");
+    p.ego_to_object_distance_threshold =
+      declare_parameter<double>(ps + "ego_to_object_distance_threshold");
+    p.time_tolerance_threshold = declare_parameter<double>(ps + "time_tolerance_threshold");
   }
 
   try {

From 94820da8ea814b18b42c4aee461e7a7737f21f37 Mon Sep 17 00:00:00 2001
From: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Date: Mon, 18 Nov 2024 14:02:08 +0900
Subject: [PATCH 10/12] Update
 planning/autoware_planning_validator/src/utils.cpp

---
 planning/autoware_planning_validator/src/utils.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp
index b12d104ed29d9..8b8b32cf289da 100644
--- a/planning/autoware_planning_validator/src/utils.cpp
+++ b/planning/autoware_planning_validator/src/utils.cpp
@@ -359,7 +359,7 @@ std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_c
 }
 
 Rtree make_ego_footprint_rtree(
-  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
   const VehicleInfo & vehicle_info)
 {
   autoware::universe_utils::MultiPolygon2d trajectory_footprints;

From cb93cf72c5a11af35cdc13fbd22296e84c2d24da Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Tue, 19 Nov 2024 11:41:03 +0900
Subject: [PATCH 11/12] add README

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
---
 planning/autoware_planning_validator/README.md | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/planning/autoware_planning_validator/README.md b/planning/autoware_planning_validator/README.md
index 78a8c95fe3772..c7429134616f4 100644
--- a/planning/autoware_planning_validator/README.md
+++ b/planning/autoware_planning_validator/README.md
@@ -85,5 +85,10 @@ The input trajectory is detected as invalid if the index exceeds the following t
 
 For parameters used e.g. to calculate threshold.
 
-| `parameters.forward_trajectory_length_acceleration` | double | This value is used to calculate required trajectory length. | -5.0 |
-| `parameters.forward_trajectory_length_margin` | double | A margin of the required trajectory length not to raise an error when the ego slightly exceeds the trajectory end point. | 2.0 |
+| Parameter Name                                       | Type   | Description                                                                                                                                                                                                                                                        | Default Value |
+| ---------------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------ | ------------- |
+| `parameters.forward_trajectory_length_acceleration`  | double | This value is used to calculate required trajectory length.                                                                                                                                                                                                        | -5.0          |
+| `parameters.forward_trajectory_length_margin`        | double | A margin of the required trajectory length not to raise an error when the ego slightly exceeds the trajectory end point.                                                                                                                                           | 2.0           |
+| `parameters.trajectory_to_object_distance_threshold` | double | A threshold for filtering objects to improve computational efficiency of collision checking. Only objects within this distance [m] from their nearest point on the trajectory are considered for collision checking.                                               | 30.0          |
+| `parameters.ego_to_object_distance_threshold`        | double | A threshold for filtering objects to improve computational efficiency of collision checking. Only objects within this distance [m] from the current ego position are considered for collision checking.                                                            | 50.0          |
+| `parameters.time_tolerance_threshold`                | double | Time tolerance threshold [s] for collision detection between ego and object trajectories. Collision check is performed only when the time difference between ego and object predicted positions is within this threshold to detect temporal-spatial intersections. | 0.1           |

From 6b487bc1804e3c28665446434992f8da62ad46b0 Mon Sep 17 00:00:00 2001
From: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
Date: Tue, 19 Nov 2024 11:43:38 +0900
Subject: [PATCH 12/12] refactor(autoware_planning_validator): add new
 parameters for trajectory and ego-object distance thresholds

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>
---
 .../config/planning_validator.param.yaml      |  4 +--
 .../planning_validator/debug_marker.hpp       |  5 ++++
 .../autoware/planning_validator/utils.hpp     |  2 +-
 .../src/debug_marker.cpp                      | 28 +++++++++++++++++++
 .../src/planning_validator.cpp                |  1 +
 5 files changed, 37 insertions(+), 3 deletions(-)

diff --git a/planning/autoware_planning_validator/config/planning_validator.param.yaml b/planning/autoware_planning_validator/config/planning_validator.param.yaml
index 742b09973ae9f..930e254625d25 100644
--- a/planning/autoware_planning_validator/config/planning_validator.param.yaml
+++ b/planning/autoware_planning_validator/config/planning_validator.param.yaml
@@ -41,12 +41,12 @@
       # Threshold to filter out objects that are far from the trajectory.
       # Only objects within this distance [m] from their nearest point on the trajectory
       # are considered for collision checking to improve computational efficiency.
-      trajectory_to_object_distance_threshold: 30
+      trajectory_to_object_distance_threshold: 30.0
 
       # Threshold to filter out objects that are far from the ego vehicle.
       # Only objects within this distance [m] from the current ego position
       # are considered for collision checking to improve computational efficiency.
-      ego_to_object_distance_threshold: 50
+      ego_to_object_distance_threshold: 50.0
 
       # Time tolerance threshold [s] for collision detection between ego and object trajectories.
       # Collision check is performed only when the time difference between ego and object
diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/debug_marker.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/debug_marker.hpp
index 5e8fd31734121..38d20eeb3f4fa 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/debug_marker.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/debug_marker.hpp
@@ -15,6 +15,8 @@
 #ifndef AUTOWARE__PLANNING_VALIDATOR__DEBUG_MARKER_HPP_
 #define AUTOWARE__PLANNING_VALIDATOR__DEBUG_MARKER_HPP_
 
+#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
+
 #include <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
@@ -34,6 +36,9 @@ class PlanningValidatorDebugMarkerPublisher
   void pushPoseMarker(
     const autoware_planning_msgs::msg::TrajectoryPoint & p, const std::string & ns, int id = 0);
   void pushPoseMarker(const geometry_msgs::msg::Pose & pose, const std::string & ns, int id = 0);
+  void pushFootprintMarker(
+    const geometry_msgs::msg::Pose & pose,
+    const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const std::string & ns);
   void pushVirtualWall(const geometry_msgs::msg::Pose & pose);
   void pushWarningMsg(const geometry_msgs::msg::Pose & pose, const std::string & msg);
   void publish();
diff --git a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
index 0d3e3bff7add3..b3a9be06322f6 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
@@ -80,7 +80,7 @@ std::optional<std::vector<autoware_planning_msgs::msg::TrajectoryPoint>> check_c
   const double ego_to_object_distance_threshold, const double time_tolerance_threshold);
 
 Rtree make_ego_footprint_rtree(
-  std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
+  const std::vector<autoware_planning_msgs::msg::TrajectoryPoint> & trajectory,
   const VehicleInfo & vehicle_info);
 
 std::optional<PredictedObjects> filter_objects(
diff --git a/planning/autoware_planning_validator/src/debug_marker.cpp b/planning/autoware_planning_validator/src/debug_marker.cpp
index 7de23de8eda01..44dedaff69b38 100644
--- a/planning/autoware_planning_validator/src/debug_marker.cpp
+++ b/planning/autoware_planning_validator/src/debug_marker.cpp
@@ -14,6 +14,8 @@
 
 #include "autoware/planning_validator/debug_marker.hpp"
 
+#include "autoware/universe_utils/geometry/geometry.hpp"
+
 #include <autoware/motion_utils/marker/marker_helper.hpp>
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 
@@ -73,6 +75,32 @@ void PlanningValidatorDebugMarkerPublisher::pushPoseMarker(
   marker_array_.markers.push_back(marker);
 }
 
+void PlanningValidatorDebugMarkerPublisher::pushFootprintMarker(
+  const geometry_msgs::msg::Pose & pose,
+  const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const std::string & ns)
+{
+  using autoware::universe_utils::createMarkerColor;
+  Marker marker = autoware::universe_utils::createDefaultMarker(
+    "map", node_->get_clock()->now(), ns, getMarkerId(ns), Marker::LINE_STRIP,
+    autoware::universe_utils::createMarkerScale(0.1, 0.1, 0.1),
+    createMarkerColor(1.0, 0.0, 0.0, 0.999));
+  const double half_width = vehicle_info.vehicle_width_m / 2.0;
+  const double base_to_front = vehicle_info.vehicle_length_m - vehicle_info.rear_overhang_m;
+  const double base_to_rear = vehicle_info.rear_overhang_m;
+
+  marker.points.push_back(
+    autoware::universe_utils::calcOffsetPose(pose, base_to_front, -half_width, 0.0).position);
+  marker.points.push_back(
+    autoware::universe_utils::calcOffsetPose(pose, base_to_front, half_width, 0.0).position);
+  marker.points.push_back(
+    autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, half_width, 0.0).position);
+  marker.points.push_back(
+    autoware::universe_utils::calcOffsetPose(pose, -base_to_rear, -half_width, 0.0).position);
+  marker.points.push_back(marker.points.front());
+  marker.lifetime = rclcpp::Duration::from_seconds(0.2);
+  marker_array_.markers.push_back(marker);
+}
+
 void PlanningValidatorDebugMarkerPublisher::pushWarningMsg(
   const geometry_msgs::msg::Pose & pose, const std::string & msg)
 {
diff --git a/planning/autoware_planning_validator/src/planning_validator.cpp b/planning/autoware_planning_validator/src/planning_validator.cpp
index 8585c156c6bad..fea015a6aec08 100644
--- a/planning/autoware_planning_validator/src/planning_validator.cpp
+++ b/planning/autoware_planning_validator/src/planning_validator.cpp
@@ -574,6 +574,7 @@ bool PlanningValidator::checkValidTrajectoryCollision(const Trajectory & traject
   if (collided_points) {
     for (const auto & p : *collided_points) {
       debug_pose_publisher_->pushPoseMarker(p.pose, "collision", 0);
+      debug_pose_publisher_->pushFootprintMarker(p.pose, vehicle_info_, "collision");
     }
   }
   return !collided_points;