refactor(autoware_planning_validator): add new parameters for traject…

…ory and ego-object distance thresholds

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

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

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 <rclcpp/rclcpp.hpp>
 
 #include <autoware_planning_msgs/msg/trajectory.hpp>
@@ -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();

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 <autoware/motion_utils/marker/marker_helper.hpp>
 #include <autoware/universe_utils/ros/marker_helper.hpp>
 
@@ -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)
 {

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;