From b6e77b987be200776f183ddf15875af8116eb33e Mon Sep 17 00:00:00 2001 From: Yoshi Ri Date: Fri, 20 Oct 2023 11:14:21 +0900 Subject: [PATCH] feat(map_based_prediction): enable to control lateral path convergence speed by setting control time horizon (#5285) * enable to control lateral path convergence speed by setting control time horizon Signed-off-by: yoshiri * update readme Signed-off-by: yoshiri * add comment in generate path function Signed-off-by: yoshiri --------- Signed-off-by: yoshiri --- perception/map_based_prediction/README.md | 48 ++++++++++++------- .../config/map_based_prediction.param.yaml | 1 + .../map_based_prediction_node.hpp | 1 + .../map_based_prediction/path_generator.hpp | 5 +- .../src/map_based_prediction_node.cpp | 5 +- .../src/path_generator.cpp | 22 ++++++--- 6 files changed, 55 insertions(+), 27 deletions(-) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 256a1d0ae899f..5da41de7cd3b7 100644 --- a/perception/map_based_prediction/README.md +++ b/perception/map_based_prediction/README.md @@ -109,6 +109,21 @@ For the additional information, here we show how we calculate lateral velocity. Currently, we use the upper method with a low-pass filter to calculate lateral velocity. +### Path generation + +Path generation is generated on the frenet frame. The path is generated by the following steps: + +1. Get the frenet frame of the reference path. +2. Generate the frenet frame of the current position of the object and the finite position of the object. +3. Optimize the path in each longitudinal and lateral coordinate of the frenet frame. (Use the quintic polynomial to fit start and end conditions.) +4. Convert the path to the global coordinate. + +See paper [2] for more details. + +#### Tuning lateral path shape + +`lateral_control_time_horizon` parameter supports the tuning of the lateral path shape. This parameter is used to calculate the time to reach the reference path. The smaller the value, the more the path will be generated to reach the reference path quickly. (Mostly the center of the lane.) + ### Path prediction for crosswalk users This module treats **Pedestrians** and **Bicycles** as objects using the crosswalk, and outputs prediction path based on map and estimated object's velocity, assuming the object has intention to cross the crosswalk, if the objects satisfies at least one of the following conditions: @@ -150,23 +165,22 @@ If the target object is inside the road or crosswalk, this module outputs one or ## Parameters -| Parameter | Type | Description | -| ------------------------------------------- | ------ | ------------------------------------------------------------------------------------------------------------ | -| `enable_delay_compensation` | bool | flag to enable the time delay compensation for the position of the object | -| `prediction_time_horizon` | double | predict time duration for predicted path [s] | -| `prediction_sampling_delta_time` | double | sampling time for points in predicted path [s] | -| `min_velocity_for_map_based_prediction` | double | apply map-based prediction to the objects with higher velocity than this value | -| `min_crosswalk_user_velocity` | double | minimum velocity use in path prediction for crosswalk users | -| `dist_threshold_for_searching_lanelet` | double | The threshold of the angle used when searching for the lane to which the object belongs [rad] | -| `delta_yaw_threshold_for_searching_lanelet` | double | The threshold of the distance used when searching for the lane to which the object belongs [m] | -| `sigma_lateral_offset` | double | Standard deviation for lateral position of objects [m] | -| `sigma_yaw_angle` | double | Standard deviation yaw angle of objects [rad] | -| `object_buffer_time_length` | double | Time span of object history to store the information [s] | -| `history_time_length` | double | Time span of object information used for prediction [s] | -| `dist_ratio_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Distance to the left bound of lanelet. | -| `dist_ratio_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Distance to the right bound of lanelet. | -| `diff_dist_threshold_to_left_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | -| `diff_dist_threshold_to_right_bound` | double | Conditions for using lane change detection of objects. Differential value of horizontal position of objects. | +| Parameter | Unit | Type | Description | +| ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | +| `enable_delay_compensation` | [-] | bool | flag to enable the time delay compensation for the position of the object | +| `prediction_time_horizon` | [s] | double | predict time duration for predicted path | +| `lateral_control_time_horizon` | [s] | double | time duration for predicted path will reach the reference path (mostly center of the lane) | +| `prediction_sampling_delta_time` | [s] | double | sampling time for points in predicted path | +| `min_velocity_for_map_based_prediction` | [m/s] | double | apply map-based prediction to the objects with higher velocity than this value | +| `min_crosswalk_user_velocity` | [m/s] | double | minimum velocity used when crosswalk user's velocity is calculated | +| `max_crosswalk_user_delta_yaw_threshold_for_lanelet` | [rad] | double | maximum yaw difference between crosswalk user and lanelet to use in path prediction for crosswalk users | +| `dist_threshold_for_searching_lanelet` | [m] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `delta_yaw_threshold_for_searching_lanelet` | [rad] | double | The threshold of the angle used when searching for the lane to which the object belongs | +| `sigma_lateral_offset` | [m] | double | Standard deviation for lateral position of objects | +| `sigma_yaw_angle_deg` | [deg] | double | Standard deviation yaw angle of objects | +| `object_buffer_time_length` | [s] | double | Time span of object history to store the information | +| `history_time_length` | [s] | double | Time span of object information used for prediction | +| `prediction_time_horizon_rate_for_validate_shoulder_lane_length` | [-] | double | prediction path will disabled when the estimated path length exceeds lanelet length. This parameter control the estimated path length | ## Assumptions / Known limits diff --git a/perception/map_based_prediction/config/map_based_prediction.param.yaml b/perception/map_based_prediction/config/map_based_prediction.param.yaml index 31fae9c811092..260ae45da2e05 100644 --- a/perception/map_based_prediction/config/map_based_prediction.param.yaml +++ b/perception/map_based_prediction/config/map_based_prediction.param.yaml @@ -2,6 +2,7 @@ ros__parameters: enable_delay_compensation: true prediction_time_horizon: 10.0 #[s] + lateral_control_time_horizon: 5.0 #[s] prediction_sampling_delta_time: 0.5 #[s] min_velocity_for_map_based_prediction: 1.39 #[m/s] min_crosswalk_user_velocity: 1.39 #[m/s] diff --git a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp index 0e87559ca28a5..38075766ea417 100644 --- a/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp @@ -143,6 +143,7 @@ class MapBasedPredictionNode : public rclcpp::Node // Parameters bool enable_delay_compensation_; double prediction_time_horizon_; + double lateral_control_time_horizon_; double prediction_time_horizon_rate_for_validate_lane_length_; double prediction_sampling_time_interval_; double min_velocity_for_map_based_prediction_; diff --git a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp index 7bb1542557d2c..4da4f62be2ede 100644 --- a/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp +++ b/perception/map_based_prediction/include/map_based_prediction/path_generator.hpp @@ -81,8 +81,8 @@ class PathGenerator { public: PathGenerator( - const double time_horizon, const double sampling_time_interval, - const double min_crosswalk_user_velocity); + const double time_horizon, const double lateral_time_horizon, + const double sampling_time_interval, const double min_crosswalk_user_velocity); PredictedPath generatePathForNonVehicleObject(const TrackedObject & object); @@ -102,6 +102,7 @@ class PathGenerator private: // Parameters double time_horizon_; + double lateral_time_horizon_; double sampling_time_interval_; double min_crosswalk_user_velocity_; diff --git a/perception/map_based_prediction/src/map_based_prediction_node.cpp b/perception/map_based_prediction/src/map_based_prediction_node.cpp index 801e7400b724d..3a0ab6ded96f8 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -686,6 +686,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ { enable_delay_compensation_ = declare_parameter("enable_delay_compensation"); prediction_time_horizon_ = declare_parameter("prediction_time_horizon"); + lateral_control_time_horizon_ = + declare_parameter("lateral_control_time_horizon"); // [s] for lateral control point prediction_sampling_time_interval_ = declare_parameter("prediction_sampling_delta_time"); min_velocity_for_map_based_prediction_ = declare_parameter("min_velocity_for_map_based_prediction"); @@ -732,7 +734,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_ declare_parameter("prediction_time_horizon_rate_for_validate_shoulder_lane_length"); path_generator_ = std::make_shared( - prediction_time_horizon_, prediction_sampling_time_interval_, min_crosswalk_user_velocity_); + prediction_time_horizon_, lateral_control_time_horizon_, prediction_sampling_time_interval_, + min_crosswalk_user_velocity_); sub_objects_ = this->create_subscription( "/perception/object_recognition/tracking/objects", 1, diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index b1f6319801d4a..c7d7a258f8de3 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -23,9 +23,10 @@ namespace map_based_prediction { PathGenerator::PathGenerator( - const double time_horizon, const double sampling_time_interval, + const double time_horizon, const double lateral_time_horizon, const double sampling_time_interval, const double min_crosswalk_user_velocity) : time_horizon_(time_horizon), + lateral_time_horizon_(lateral_time_horizon), sampling_time_interval_(sampling_time_interval), min_crosswalk_user_velocity_(min_crosswalk_user_velocity) { @@ -213,18 +214,25 @@ FrenetPath PathGenerator::generateFrenetPath( { FrenetPath path; const double duration = time_horizon_; + const double lateral_duration = lateral_time_horizon_; // Compute Lateral and Longitudinal Coefficients to generate the trajectory - const Eigen::Vector3d lat_coeff = calcLatCoefficients(current_point, target_point, duration); + const Eigen::Vector3d lat_coeff = + calcLatCoefficients(current_point, target_point, lateral_duration); const Eigen::Vector2d lon_coeff = calcLonCoefficients(current_point, target_point, duration); path.reserve(static_cast(duration / sampling_time_interval_)); for (double t = 0.0; t <= duration; t += sampling_time_interval_) { - const double d_next = current_point.d + current_point.d_vel * t + 0 * 2 * std::pow(t, 2) + - lat_coeff(0) * std::pow(t, 3) + lat_coeff(1) * std::pow(t, 4) + - lat_coeff(2) * std::pow(t, 5); - const double s_next = current_point.s + current_point.s_vel * t + 2 * 0 * std::pow(t, 2) + - lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4); + const double current_acc = + 0.0; // Currently we assume the object is traveling at a constant speed + const double d_next_ = current_point.d + current_point.d_vel * t + + current_acc * 2.0 * std::pow(t, 2) + lat_coeff(0) * std::pow(t, 3) + + lat_coeff(1) * std::pow(t, 4) + lat_coeff(2) * std::pow(t, 5); + // t > lateral_duration: 0.0, else d_next_ + const double d_next = t > lateral_duration ? 0.0 : d_next_; + const double s_next = current_point.s + current_point.s_vel * t + + 2.0 * current_acc * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) + + lon_coeff(1) * std::pow(t, 4); if (s_next > max_length) { break; }