From 52360d3fb5a2b85880c5b2ae4e219f60ff2e00c0 Mon Sep 17 00:00:00 2001 From: yoshiri Date: Thu, 12 Oct 2023 16:26:24 +0900 Subject: [PATCH 1/3] enable to control lateral path convergence speed by setting control time horizon Signed-off-by: yoshiri --- .../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 ++++- .../map_based_prediction/src/path_generator.cpp | 15 ++++++++++----- 5 files changed, 19 insertions(+), 8 deletions(-) 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 3d01ab96e9b62..a8a54c8f28be1 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 @@ -135,6 +135,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 745bccf6dbabe..62e5284042f40 100644 --- a/perception/map_based_prediction/src/map_based_prediction_node.cpp +++ b/perception/map_based_prediction/src/map_based_prediction_node.cpp @@ -694,6 +694,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"); @@ -740,7 +742,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( "~/input/objects", 1, diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 547132410badf..73ae523a4f63d 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,16 +214,20 @@ 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 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); + // 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 * std::pow(t, 2) + lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4); if (s_next > max_length) { From 06267fca318caa335367fce9b154111ee340497a Mon Sep 17 00:00:00 2001 From: yoshiri Date: Mon, 16 Oct 2023 19:29:45 +0900 Subject: [PATCH 2/3] update readme Signed-off-by: yoshiri --- perception/map_based_prediction/README.md | 16 ++++++++++++++++ 1 file changed, 16 insertions(+) diff --git a/perception/map_based_prediction/README.md b/perception/map_based_prediction/README.md index 7631b47772862..6d3d7f5e035a9 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: @@ -155,6 +170,7 @@ If the target object is inside the road or crosswalk, this module outputs one or | ---------------------------------------------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------------------- | | `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 | From ae216521d97c137cdad22a05d6a86f13ffd9c01b Mon Sep 17 00:00:00 2001 From: yoshiri Date: Fri, 20 Oct 2023 11:05:47 +0900 Subject: [PATCH 3/3] add comment in generate path function Signed-off-by: yoshiri --- .../map_based_prediction/src/path_generator.cpp | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/perception/map_based_prediction/src/path_generator.cpp b/perception/map_based_prediction/src/path_generator.cpp index 73ae523a4f63d..5cb7e186b7cc4 100644 --- a/perception/map_based_prediction/src/path_generator.cpp +++ b/perception/map_based_prediction/src/path_generator.cpp @@ -223,13 +223,16 @@ FrenetPath PathGenerator::generateFrenetPath( 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 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 * std::pow(t, 2) + - lon_coeff(0) * std::pow(t, 3) + lon_coeff(1) * std::pow(t, 4); + 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; }