From d8943b935bef03e2ece41af49c6a06c08b96e4be Mon Sep 17 00:00:00 2001 From: Kyoichi Sugahara Date: Wed, 23 Oct 2024 16:19:38 +0900 Subject: [PATCH] refactor(autoware_planning_validator): add collision checking for predicted objects Signed-off-by: Kyoichi Sugahara --- .../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 #include +#include #include #include #include @@ -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 sub_kinematics_{ this, "~/input/kinematics"}; + autoware::universe_utils::InterProcessPollingSubscriber sub_obj_{ + this, "~/input/objects"}; rclcpp::Subscription::SharedPtr sub_traj_; rclcpp::Publisher::SharedPtr pub_traj_; rclcpp::Publisher::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 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 @@ + 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 #include +#include + #include #include #include @@ -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 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: + // 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