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 +#include #include #include @@ -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 calcMaxSteeringAngles( std::pair 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 #include -#include - #include #include #include @@ -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 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 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 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 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 #include +#include + #include #include #include @@ -289,6 +293,139 @@ std::pair 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 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 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 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 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;