Skip to content

Commit

Permalink
feat(map_based_prediction): enable to control lateral path convergenc…
Browse files Browse the repository at this point in the history 
…e speed by setting control time horizon (autowarefoundation#5285)

* enable to control lateral path convergence speed by setting control time horizon

Signed-off-by: yoshiri <[email protected]>

* update readme

Signed-off-by: yoshiri <[email protected]>

* add comment  in generate path function

Signed-off-by: yoshiri <[email protected]>

---------

Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
@YoshiRi
YoshiRi committed Oct 31, 2023
1 parent cf64cca commit 77c6eac
Show file tree
Hide file tree
Showing 6 changed files with 55 additions and 27 deletions.
48 changes: 31 additions & 17 deletions perception/map_based_prediction/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand Down Expand Up @@ -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

Expand Down
1 change: 1 addition & 0 deletions perception/map_based_prediction/config/map_based_prediction.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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]
Expand Down
1 change: 1 addition & 0 deletions perception/map_based_prediction/include/map_based_prediction/map_based_prediction_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,6 +139,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_;
Expand Down
5 changes: 3 additions & 2 deletions perception/map_based_prediction/include/map_based_prediction/path_generator.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,8 +57,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);

Expand All @@ -78,6 +78,7 @@ class PathGenerator
private:
// Parameters
double time_horizon_;
double lateral_time_horizon_;
double sampling_time_interval_;
double min_crosswalk_user_velocity_;

Expand Down
5 changes: 4 additions & 1 deletion perception/map_based_prediction/src/map_based_prediction_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -591,6 +591,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
{
enable_delay_compensation_ = declare_parameter<bool>("enable_delay_compensation");
prediction_time_horizon_ = declare_parameter<double>("prediction_time_horizon");
lateral_control_time_horizon_ =
declare_parameter<double>("lateral_control_time_horizon"); // [s] for lateral control point
prediction_sampling_time_interval_ = declare_parameter<double>("prediction_sampling_delta_time");
min_velocity_for_map_based_prediction_ =
declare_parameter<double>("min_velocity_for_map_based_prediction");
Expand Down Expand Up @@ -635,7 +637,8 @@ MapBasedPredictionNode::MapBasedPredictionNode(const rclcpp::NodeOptions & node_
declare_parameter<double>("prediction_time_horizon_rate_for_validate_shoulder_lane_length");

path_generator_ = std::make_shared<PathGenerator>(
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<TrackedObjects>(
"/perception/object_recognition/tracking/objects", 1,
Expand Down
22 changes: 15 additions & 7 deletions perception/map_based_prediction/src/path_generator.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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)
{
Expand Down Expand Up @@ -212,18 +213,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<size_t>(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;
}
Expand Down

0 comments on commit 77c6eac

Please sign in to comment.