diff --git a/control/predicted_path_checker/README.md b/control/predicted_path_checker/README.md index ee904472958ab..91315e4b713c5 100644 --- a/control/predicted_path_checker/README.md +++ b/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 | diff --git a/control/predicted_path_checker/config/predicted_path_checker.param.yaml b/control/predicted_path_checker/config/predicted_path_checker.param.yaml index cf2621b333491..0aa577de3098d 100644 --- a/control/predicted_path_checker/config/predicted_path_checker.param.yaml +++ b/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 diff --git a/control/predicted_path_checker/images/FlowChart.png b/control/predicted_path_checker/images/FlowChart.png new file mode 100644 index 0000000000000..9d93e37390c6b Binary files /dev/null and b/control/predicted_path_checker/images/FlowChart.png differ diff --git a/control/predicted_path_checker/images/Z_axis_filtering.png b/control/predicted_path_checker/images/Z_axis_filtering.png new file mode 100644 index 0000000000000..0c064aecae42e Binary files /dev/null and b/control/predicted_path_checker/images/Z_axis_filtering.png differ diff --git a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp index cee982bdcba8b..187d6b866337a 100644 --- a/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp +++ b/control/predicted_path_checker/include/predicted_path_checker/predicted_path_checker_node.hpp @@ -23,9 +23,7 @@ #include #include #include -#include #include -#include #include #include @@ -88,8 +86,6 @@ class PredictedPathCheckerNode : public rclcpp::Node // Subscriber std::shared_ptr self_pose_listener_; - std::shared_ptr transform_listener_; - tf2_ros::Buffer tf_buffer_{get_clock()}; rclcpp::Subscription::SharedPtr sub_dynamic_objects_; rclcpp::Subscription::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); diff --git a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml index f4c7f5dbb27a4..a35c11b80c1f7 100755 --- a/control/predicted_path_checker/launch/predicted_path_checker.launch.xml +++ b/control/predicted_path_checker/launch/predicted_path_checker.launch.xml @@ -12,10 +12,10 @@ - - - - - + + + + + diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp index 24d3d3d3bb4f0..b3c5b8a8e8707 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/debug_marker.cpp +++ b/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("~/predicted/virtual_wall", 1); + node->create_publisher("~/debug/virtual_wall", 1); debug_viz_pub_ = - node_->create_publisher("~/predicted/debug/marker", 1); + node_->create_publisher("~/debug/marker", 1); } bool PredictedPathCheckerDebugNode::pushPolygon( diff --git a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp b/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp index d1faf7bb473a6..f387abd6773f8 100644 --- a/control/predicted_path_checker/src/predicted_path_checker_node/predicted_path_checker_node.cpp +++ b/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,9 +58,9 @@ 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); @@ -68,18 +68,18 @@ PredictedPathCheckerNode::PredictedPathCheckerNode(const rclcpp::NodeOptions & n self_pose_listener_ = std::make_shared(this); sub_dynamic_objects_ = create_subscription( - "input/objects", rclcpp::SensorDataQoS(), + "~/input/objects", rclcpp::SensorDataQoS(), std::bind(&PredictedPathCheckerNode::onDynamicObjects, this, _1)); sub_reference_trajectory_ = create_subscription( - "input/reference_trajectory", 1, + "~/input/reference_trajectory", 1, std::bind(&PredictedPathCheckerNode::onReferenceTrajectory, this, _1)); sub_predicted_trajectory_ = create_subscription( - "input/predicted_trajectory", 1, + "~/input/predicted_trajectory", 1, std::bind(&PredictedPathCheckerNode::onPredictedTrajectory, this, _1)); sub_odom_ = create_subscription( - "input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1)); + "~/input/odometry", 1, std::bind(&PredictedPathCheckerNode::onOdom, this, _1)); sub_accel_ = create_subscription( - "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(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 =