readme

control/predicted_path_checker/README.md

@@ -1,23 +1,96 @@
-# predicted_path_checker
+# Predicted Path Checker
 
 ## Purpose
 
-## Inner-workings / Algorithms
+The Predicted Path Checker package is designed for autonomous vehicles to check the predicted path generated by control
+modules. It handles potential collisions that the planning module might not be able to handle and that in the brake
+distance. In case of collision in brake distance, the package will send a diagnostic message labeled "ERROR" to alert
+the system to send emergency and in the case of collisions in outside reference trajectory, it sends pause request to
+pause interface to make the vehicle stop.
 
-### Flow chart
+## Algorithm
 
-### Algorithms
+The package algorithm evaluates the predicted trajectory against the reference trajectory and the predicted objects in
+the environment. It checks for potential collisions and, if necessary, generates an appropriate response to avoid them (
+emergency or pause request).
 
-### Check data
+### Inner Algorithm
 
-### Diagnostic update
+![FlowChart.png](images%2FFlowChart.png)
 
-## Inputs / Outputs
+**cutTrajectory() ->** It cuts the predicted trajectory with input length. Length is calculated by multiplying the
+velocity
+of ego vehicle with "trajectory_check_time" parameter and "min_trajectory_length".
 
-### Input
+**checkTrajectoryForCollision() ->** It checks the predicted trajectory for collision with the predicted objects. It
+calculates both polygon of trajectory points and predicted objects and checks intersection of both polygons. If there is
+an intersection, it calculates the nearest collision point. It returns the nearest collision point of polygon and the
+predicted object. It also checks predicted objects history which are intersect with the footprint before to avoid
+unexpected behaviors. Predicted objects history stores the objects if it was detected below the "chattering_threshold"
+seconds ago.
 
-### Output
+If the "enable_z_axis_obstacle_filtering" parameter is set to true, it filters the predicted objects in the Z-axis by
+using "z_axis_filtering_buffer". If the object does not intersect with the Z-axis, it is filtered out.
+
+![Z_axis_filtering.png](images%2FZ_axis_filtering.png)
+
+**calculateProjectedVelAndAcc() ->** It calculates the projected velocity and acceleration of the predicted object on
+predicted trajectory's collision point's axes.
+
+**isInBrakeDistance() ->** It checks if the stop point is in brake distance. It gets relative velocity and
+acceleration of ego vehicle with respect to the predicted object. It calculates the brake distance, if the point in
+brake distance, it returns true.
+
+**isItDiscretePoint() ->** It checks if the stop point on predicted trajectory is discrete point or not. If it is not
+discrete point, planning should handle the stop.
+
+**isThereStopPointOnRefTrajectory() ->** It checks if there is a stop point on reference trajectory. If there is a stop
+point before the stop index, it returns true. Otherwise, it returns false, and node is going to call pause interface to
+make the vehicle stop.
+
+## Inputs
+
+| Name                                  | Type                                                  | Description                                         |
+|---------------------------------------|-------------------------------------------------------|-----------------------------------------------------|
+| `~/input/reference_trajectory`        | `autoware_auto_planning_msgs::msg::Trajectory`        | Reference trajectory                                |
+| `~/input/predicted_trajectory`        | `autoware_auto_planning_msgs::msg::Trajectory`        | Predicted trajectory                                |
+| `~/input/objects`                     | `autoware_auto_perception_msgs::msg::PredictedObject` | Dynamic objects in the environment                  |
+| `~/input/odometry`                    | `nav_msgs::msg::Odometry`                             | Odometry message of vehicle to get current velocity |
+| `~/input/current_accel`               | `geometry_msgs::msg::AccelWithCovarianceStamped`      | Current acceleration                                |
+| `/control/vehicle_cmd_gate/is_paused` | `tier4_control_msgs::msg::IsPaused`                   | Current pause state of the vehicle                  |
+
+## Outputs
+
+| Name                                  | Type                                     | Description                            |
+|---------------------------------------|------------------------------------------|----------------------------------------|
+| `~/debug/marker`                      | `visualization_msgs::msg::MarkerArray`   | Marker for visualization               |
+| `~/debug/virtual_wall`                | `visualization_msgs::msg::MarkerArray`   | Virtual wall marker for visualization  |
+| `/control/vehicle_cmd_gate/set_pause` | `tier4_control_msgs::srv::SetPause`      | Pause service to make the vehicle stop |
+| `/diagnostics`                        | `diagnostic_msgs::msg::DiagnosticStatus` | Diagnostic status of vehicle           |
 
 ## Parameters
 
-## Assumptions / Known limits
+**Node Parameters**
+
+| Name                                | Type     | Description                                             | Default value |
+|:------------------------------------|:---------|:--------------------------------------------------------|:--------------|
+| `update_rate`                       | `double` | The update rate [Hz]                                    | 10.0          |
+| `delay_time`                        | `double` | he time delay considered for the emergency response [s] | 0.17          |
+| `max_deceleration`                  | `double` | Max deceleration for ego vehicle to stop [m/s^2]        | 1.5           |
+| `resample_interval`                 | `double` | Interval for resampling trajectory [m]                  | 0.5           |
+| `stop_margin`                       | `double` | The stopping margin [m]                                 | 0.5           |
+| `ego_nearest_dist_threshold`        | `double` | The nearest distance threshold for ego vehicle  [m]     | 3.0           |
+| `ego_nearest_yaw_threshold`         | `double` | The nearest yaw threshold for ego vehicle [rad]         | 1.046         |
+| `min_trajectory_check_length`       | `double` | The minimum trajectory check length in meters [m]       | 1.5           |
+| `trajectory_check_time`             | `double` | The trajectory check time in seconds. [s]               | 3.0           |
+| `distinct_point_distance_threshold` | `double` | The distinct point distance threshold [m]               | 0.3           |
+| `distinct_point_yaw_threshold`      | `double` | The distinct point yaw threshold [deg]                  | 5.0           |
+
+**Collision Checker Parameters**
+
+| Name                               | Type     | Description                                                       | Default value |
+|:-----------------------------------|:---------|:------------------------------------------------------------------|:--------------|
+| `width_margin`                     | `double` | The width margin for collision checking [Hz]                      | 0.2           |
+| `chattering_threshold`             | `double` | The chattering threshold for collision detection [s]              | 0.2           |
+| `z_axis_filtering_buffer`          | `double` | The Z-axis filtering buffer [m]                                   | 0.3           |
+| `enable_z_axis_obstacle_filtering` | `bool`   | A boolean flag indicating if Z-axis obstacle filtering is enabled | false         |

control/predicted_path_checker/config/predicted_path_checker.param.yaml

@@ -2,20 +2,19 @@
   ros__parameters:
     # Node
     update_rate: 10.0
-    max_deceleration: 1.5
     delay_time: 0.17
+    max_deceleration: 1.5
+    resample_interval: 0.5
+    stop_margin: 0.5 # [m]
     ego_nearest_dist_threshold: 3.0 # [m]
     ego_nearest_yaw_threshold: 1.046 # [rad] = 60 [deg]
-    stop_margin: 0.5 # [m]
     min_trajectory_check_length: 1.5
     trajectory_check_time: 3.0
-    resample_interval: 0.5
     distinct_point_distance_threshold: 0.3
     distinct_point_yaw_threshold: 5.0 # [deg]
 
     collision_checker_params:
-      width_margin: 0.1
-      longitudinal_margin: 0.15
-      enable_z_axis_obstacle_filtering: false
-      z_axis_filtering_buffer: 0.3
+      width_margin: 0.2
       chattering_threshold: 0.2
+      z_axis_filtering_buffer: 0.3
+      enable_z_axis_obstacle_filtering: false

control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp

@@ -23,9 +23,7 @@
 #include <predicted_path_checker/collision_checker.hpp>
 #include <predicted_path_checker/utils.hpp>
 #include <tier4_autoware_utils/geometry/geometry.hpp>
-#include <tier4_autoware_utils/ros/debug_publisher.hpp>
 #include <tier4_autoware_utils/ros/self_pose_listener.hpp>
-#include <tier4_autoware_utils/ros/transform_listener.hpp>
 #include <vehicle_info_util/vehicle_info.hpp>
 #include <vehicle_info_util/vehicle_info_util.hpp>
 
@@ -88,8 +86,6 @@ class PredictedPathCheckerNode : public rclcpp::Node
 
   // Subscriber
   std::shared_ptr<tier4_autoware_utils::SelfPoseListener> self_pose_listener_;
-  std::shared_ptr<tier4_autoware_utils::TransformListener> transform_listener_;
-  tf2_ros::Buffer tf_buffer_{get_clock()};
   rclcpp::Subscription<PredictedObjects>::SharedPtr sub_dynamic_objects_;
   rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
     sub_reference_trajectory_;
@@ -150,7 +146,7 @@ class PredictedPathCheckerNode : public rclcpp::Node
 
   void sendRequest(bool make_stop_vehicle);
 
-  bool checkDiscretePoint(
+  bool isItDiscretePoint(
     const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const;
 
   static Trajectory cutTrajectory(const Trajectory & trajectory, const double length);

control/predicted_path_checker/launch/predicted_path_checker.launch.xml

@@ -12,10 +12,10 @@
   <node pkg="predicted_path_checker" exec="predicted_path_checker_node" name="predicted_path_checker_node" output="screen">
     <param from="$(var config_file)"/>
     <param from="$(var vehicle_info_param_file)"/>
-    <remap from="input/objects" to="$(var input/objects)"/>
-    <remap from="input/reference_trajectory" to="$(var input/reference_trajectory)"/>
-    <remap from="input/predicted_trajectory" to="$(var input/predicted_trajectory)"/>
-    <remap from="input/odometry" to="$(var input/odometry)"/>
-    <remap from="input/current_accel" to="$(var input/current_accel)"/>
+    <remap from="~/input/objects" to="$(var input/objects)"/>
+    <remap from="~/input/reference_trajectory" to="$(var input/reference_trajectory)"/>
+    <remap from="~/input/predicted_trajectory" to="$(var input/predicted_trajectory)"/>
+    <remap from="~/input/odometry" to="$(var input/odometry)"/>
+    <remap from="~/input/current_accel" to="$(var input/current_accel)"/>
   </node>
 </launch>

control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp

@@ -45,9 +45,9 @@ PredictedPathCheckerDebugNode::PredictedPathCheckerDebugNode(
 : node_(node), base_link2front_(base_link2front)
 {
   virtual_wall_pub_ =
-    node->create_publisher<visualization_msgs::msg::MarkerArray>("~/predicted/virtual_wall", 1);
+    node->create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/virtual_wall", 1);
   debug_viz_pub_ =
-    node_->create_publisher<visualization_msgs::msg::MarkerArray>("~/predicted/debug/marker", 1);
+    node_->create_publisher<visualization_msgs::msg::MarkerArray>("~/debug/marker", 1);
 }
 
 bool PredictedPathCheckerDebugNode::pushPolygon(

control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp

@@ -45,8 +45,8 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n
   node_param_.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold", 3.0);
   node_param_.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold", 1.046);
   node_param_.max_deceleration = declare_parameter("max_deceleration", 1.5);
-  node_param_.delay_time = declare_parameter("delay_time", 0.5);
-  node_param_.stop_margin = declare_parameter("stop_margin", 1.0);
+  node_param_.delay_time = declare_parameter("delay_time", 0.17);
+  node_param_.stop_margin = declare_parameter("stop_margin", 0.5);
   node_param_.min_trajectory_check_length = declare_parameter("min_trajectory_check_length", 1.5);
   node_param_.trajectory_check_time = declare_parameter("trajectory_check_time", 3.0);
   node_param_.resample_interval = declare_parameter("resample_interval", 0.5);
@@ -58,28 +58,28 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n
   collision_checker_param_.width_margin =
     declare_parameter("collision_checker_params.width_margin", 0.2);
   collision_checker_param_.enable_z_axis_obstacle_filtering =
-    declare_parameter("collision_checker_params.enable_z_axis_obstacle_filtering", true);
+    declare_parameter("collision_checker_params.enable_z_axis_obstacle_filtering", false);
   collision_checker_param_.z_axis_filtering_buffer =
-    declare_parameter("collision_checker_params.z_axis_filtering_buffer", 1.0);
+    declare_parameter("collision_checker_params.z_axis_filtering_buffer", 0.3);
   collision_checker_param_.chattering_threshold =
     declare_parameter("collision_checker_params.chattering_threshold", 0.2);
 
   // Subscriber
   self_pose_listener_ = std::make_shared<tier4_autoware_utils::SelfPoseListener>(this);
 
   sub_dynamic_objects_ = create_subscription<PredictedObjects>(
-    "input/objects", rclcpp::SensorDataQoS(),
+    "~/input/objects", rclcpp::SensorDataQoS(),
     std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1));
   sub_reference_trajectory_ = create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
-    "input/reference_trajectory", 1,
+    "~/input/reference_trajectory", 1,
     std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1));
   sub_predicted_trajectory_ = create_subscription<autoware_auto_planning_msgs::msg::Trajectory>(
-    "input/predicted_trajectory", 1,
+    "~/input/predicted_trajectory", 1,
     std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1));
   sub_odom_ = create_subscription<nav_msgs::msg::Odometry>(
-    "input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1));
+    "~/input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1));
   sub_accel_ = create_subscription<geometry_msgs::msg::AccelWithCovarianceStamped>(
-    "input/current_accel", rclcpp::QoS{1}, std::bind(&PredictedPathCheckerNode::onAccel, this, _1));
+    "~/input/current_accel", rclcpp::QoS{1}, std::bind(&PredictedPathCheckerNode::onAccel, this, _1));
 
   debug_ptr_ =
     std::make_shared<PredictedPathCheckerDebugNode>(this, vehicle_info_.max_longitudinal_offset_m);
@@ -311,7 +311,7 @@ void PredictedPathCheckerNode::onTimer()
     motion_utils::convertToTrajectoryPointArray(*reference_trajectory_);
 
   const auto is_discrete_point =
-    checkDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx));
+    isItDiscretePoint(reference_trajectory_array, predicted_trajectory_array.at(stop_idx));
 
   if (is_discrete_point) {
     // Check reference trajectory has stop point or not
@@ -321,11 +321,11 @@ void PredictedPathCheckerNode::onTimer()
 
     if (!is_there_stop_in_ref_trajectory) {
       // Send pause
-      current_state_ = State::PAUSE;
-      updater_.force_update();
       if (!is_paused_by_node_) {
         sendRequest(true);
       }
+      current_state_ = State::PAUSE;
+      updater_.force_update();
 
       RCLCPP_WARN_THROTTLE(
         this->get_logger(), *this->get_clock(), 5000 /* ms */,
@@ -406,7 +406,7 @@ void PredictedPathCheckerNode::sendRequest(bool make_stop_vehicle)
     cli_set_pause_->async_send_request(req, [this](auto) { is_calling_set_pause_ = false; });
   }
 }
-bool PredictedPathCheckerNode::checkDiscretePoint(
+bool PredictedPathCheckerNode::isItDiscretePoint(
   const TrajectoryPoints & reference_trajectory, const TrajectoryPoint & collision_point) const
 {
   const auto nearest_segment =