refactor(autoware_planning_validator): add collision checking for pre…

…dicted objects

Signed-off-by: Kyoichi Sugahara <[email protected]>

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,14 +153,15 @@ 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;
 
   Trajectory::ConstSharedPtr current_trajectory_;
   Trajectory::ConstSharedPtr previous_published_trajectory_;
 
   Odometry::ConstSharedPtr current_kinematics_;
+  PredictedObjects::ConstSharedPtr current_objects_;
 
   std::shared_ptr<PlanningValidatorDebugMarkerPublisher> debug_pose_publisher_;
 

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>

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

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