-
ego_kinema...
+
ego_kinema...
@@ -118,37 +118,34 @@
PlanningValidatorStatus
-
+
-
+
- Trajectory to be validated
+ Trajectory to be validated
-
Trajectory to be vali...
+
Trajectory to be vali...
-
+
-
+
- Used for the validation
+ Used for the validation
-
Used for the validati...
+
Used for the validati...
@@ -161,7 +158,7 @@
>
- Validated trajectory
+ Validated trajectory
- To send validation result to the system: OK/ERROR
+ To send validation result to the system: OK/ERROR
- To show the result and the reason for other modules/users
+ To show the result and the reason for other modules/users
@@ -205,11 +202,47 @@
To show the result and the re...
+
+
+
+
+
+
+
+ Objects
+
+
+
+
+
+
+
+
+
+ Recognized objects to detect collision
+
+
+
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
#include
@@ -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/planning_validator.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/planning_validator.hpp
index e996855b9b4da..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
@@ -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
@@ -37,6 +39,8 @@
namespace autoware::planning_validator
{
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;
@@ -59,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;
@@ -86,6 +93,7 @@ class PlanningValidator : public rclcpp::Node
bool checkValidDistanceDeviation(const Trajectory & trajectory);
bool checkValidLongitudinalDistanceDeviation(const Trajectory & trajectory);
bool checkValidForwardTrajectoryLength(const Trajectory & trajectory);
+ bool checkValidTrajectoryCollision(const Trajectory & trajectory);
private:
void setupDiag();
@@ -105,6 +113,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 +136,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 +144,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/include/autoware/planning_validator/utils.hpp b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
index 7d8aca14aff77..b3a9be06322f6 100644
--- a/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
+++ b/planning/autoware_planning_validator/include/autoware/planning_validator/utils.hpp
@@ -15,18 +15,34 @@
#ifndef AUTOWARE__PLANNING_VALIDATOR__UTILS_HPP_
#define AUTOWARE__PLANNING_VALIDATOR__UTILS_HPP_
-#include
+#include "autoware/universe_utils/geometry/boost_geometry.hpp"
+#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
+#include
#include
-#include
+#include
+#include
+#include
+#include
+
#include
#include
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 = autoware::universe_utils::Point2d;
+using Box = boost::geometry::model::box;
+using BoxTimeIndexPair = std::pair>;
+using Rtree = boost::geometry::index::rtree>;
std::pair getAbsMaxValAndIdx(const std::vector & v);
@@ -57,6 +73,31 @@ std::pair calcMaxSteeringAngles(
std::pair calcMaxSteeringRates(
const Trajectory & trajectory, const double wheelbase);
+std::optional> 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 ego_to_object_distance_threshold, const double time_tolerance_threshold);
+
+Rtree make_ego_footprint_rtree(
+ const std::vector & trajectory,
+ const VehicleInfo & vehicle_info);
+
+std::optional filter_objects(
+ const PredictedObjects & objects,
+ const std::vector & trajectory,
+ const double trajectory_to_object_distance_threshold,
+ const double ego_to_object_distance_threshold);
+
+std::optional 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 & predicted_object_rtree_nodes);
+
+std::vector> detect_collisions(
+ const Rtree & ego_rtree, const Rtree & predicted_object_rtree, double time_tolerance);
+
bool checkFinite(const TrajectoryPoint & point);
void shiftPose(geometry_msgs::msg::Pose & pose, double longitudinal);
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/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
#include
@@ -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 29e8932c87842..fea015a6aec08 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(ps + "forward_trajectory_length_acceleration");
p.forward_trajectory_length_margin =
declare_parameter(ps + "forward_trajectory_length_margin");
+ p.trajectory_to_object_distance_threshold =
+ declare_parameter(ps + "trajectory_to_object_distance_threshold");
+ p.ego_to_object_distance_threshold =
+ declare_parameter(ps + "ego_to_object_distance_threshold");
+ p.time_tolerance_threshold = declare_parameter(ps + "time_tolerance_threshold");
}
try {
@@ -169,6 +174,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()
@@ -181,6 +189,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 +206,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 +332,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 = checkValidTrajectoryCollision(resampled);
s.invalid_count = isAllValid(s) ? 0 : s.invalid_count + 1;
}
@@ -545,8 +558,36 @@ bool PlanningValidator::checkValidForwardTrajectoryLength(const Trajectory & tra
return forward_length > forward_length_required;
}
+bool PlanningValidator::checkValidTrajectoryCollision(const Trajectory & trajectory)
+{
+ 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 auto & collided_points = check_collision(
+ *current_objects_, trajectory, current_kinematics_->pose.pose.position, vehicle_info_,
+ 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) {
+ debug_pose_publisher_->pushPoseMarker(p.pose, "collision", 0);
+ debug_pose_publisher_->pushFootprintMarker(p.pose, vehicle_info_, "collision");
+ }
+ }
+ return !collided_points;
+}
+
bool PlanningValidator::isAllValid(const PlanningValidatorStatus & s) const
{
+ // TODO(Sugahara): Add s.is_valid_no_collision after verifying that:
+ // 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 &&
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 +624,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
diff --git a/planning/autoware_planning_validator/src/utils.cpp b/planning/autoware_planning_validator/src/utils.cpp
index 0a70b4d415dcb..8b8b32cf289da 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,181 @@ std::pair calcMaxSteeringRates(
return {max_steering_rate, max_index};
}
+std::optional> 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 ego_to_object_distance_threshold, const double time_tolerance_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);
+ }
+ }
+ 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, trajectory_to_object_distance_threshold,
+ ego_to_object_distance_threshold);
+ if (!filtered_objects) {
+ return std::nullopt;
+ }
+
+ std::vector 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;
+ }
+
+ const double predicted_time_step = highest_confidence_path.value().time_step.sec +
+ highest_confidence_path.value().time_step.nanosec * 1e-9;
+
+ make_predicted_object_rtree(
+ highest_confidence_path.value(), object.shape, predicted_time_step,
+ predicted_object_rtree_nodes);
+ }
+
+ Rtree predicted_object_rtree(
+ predicted_object_rtree_nodes.begin(), predicted_object_rtree_nodes.end());
+
+ const auto & collision_index_set =
+ detect_collisions(ego_rtree, predicted_object_rtree, time_tolerance_threshold);
+ std::vector collision_points;
+ for (const auto & [ego_index, obj_index] : collision_index_set) {
+ collision_points.push_back(filtered_trajectory[ego_index]);
+ }
+
+ return collision_points.empty() ? std::nullopt : std::make_optional(collision_points);
+}
+
+Rtree make_ego_footprint_rtree(
+ const std::vector & trajectory,
+ const VehicleInfo & vehicle_info)
+{
+ autoware::universe_utils::MultiPolygon2d trajectory_footprints;
+ 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 rtree_nodes;
+
+ rtree_nodes.reserve(trajectory_footprints.size());
+ for (auto i = 0UL; i < trajectory_footprints.size(); ++i) {
+ const auto box =
+ boost::geometry::return_envelope(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, std::make_pair(time, i));
+ }
+ return Rtree(rtree_nodes);
+}
+
+std::optional filter_objects(
+ const PredictedObjects & objects,
+ const std::vector & trajectory,
+ const double trajectory_to_object_distance_threshold,
+ const double ego_to_object_distance_threshold)
+{
+ PredictedObjects filtered_objects;
+
+ 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 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 (
+ trajectory_to_object_distance < trajectory_to_object_distance_threshold &&
+ ego_to_object_distance < ego_to_object_distance_threshold) {
+ filtered_objects.objects.push_back(object);
+ }
+ }
+
+ return filtered_objects.objects.empty() ? std::nullopt : std::make_optional(filtered_objects);
+}
+
+std::optional find_highest_confidence_path(const PredictedObject & object)
+{
+ const auto & paths = object.kinematics.predicted_paths;
+
+ if (paths.empty()) {
+ return std::nullopt;
+ }
+
+ const auto max_confidence_it = std::max_element(
+ paths.begin(), paths.end(),
+ [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; });
+
+ 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 & 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(predicted_polygon);
+
+ predicted_object_rtree_nodes.emplace_back(box, std::make_pair(predicted_time, j));
+ }
+}
+
+std::vector> detect_collisions(
+ const Rtree & ego_rtree, const Rtree & predicted_object_rtree, double time_tolerance)
+{
+ std::vector> 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 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;