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 #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/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 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;