Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(time-based-safe-dist): improve obstacle_cruise_planner safe distance for dence urban ODD scenarios #7987

Closed
wants to merge 6 commits into from
Closed

feat(time-based-safe-dist): improve obstacle_cruise_planner safe distance for dence urban ODD scenarios #7987

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
5eec2eb
feat(time-based-safe-dist): initial implementation
Jul 11, 2024
586ed10
style(pre-commit): autofix
pre-commit-ci[bot] Jul 11, 2024
350999a
feat(time-based-safe-dist): update desired margin in stop trajectory …
Jul 15, 2024
4cb3960
feat(time-based-safe-dist): add parameter and implementation to selec…
Jul 15, 2024
9d9f497
feat(time-based-safe-dist): update readme file
Jul 15, 2024
0438644
style(pre-commit): autofix
pre-commit-ci[bot] Jul 15, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
28 changes: 24 additions & 4 deletions planning/autoware_obstacle_cruise_planner/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -198,14 +198,20 @@ If the acceleration is less than `common.min_strong_accel`, the stop planning wi

### Cruise planning

| Parameter | Type | Description |
| ----------------------------- | ------ | ---------------------------------------------- |
| `common.safe_distance_margin` | double | minimum distance with obstacles for cruise [m] |
| Parameter | Type | Description |
| ---------------------------------- | ------ | ------------------------------------------------------------------------------------------ |
| `common.safe_distance_method` | string | safe distance method selection from time_based and RSS |
| `common.safe_distance_margin` | double | minimum distance with obstacles for cruise [m] |
| `common.safe_distance_time_margin` | double | time values used in time-based safe distance rule, e.g. 2-seconds rule, 3-seconds rule [s] |

The role of the cruise planning is keeping a safe distance with dynamic vehicle objects with smoothed velocity transition.
This includes not only cruising a front vehicle, but also reacting a cut-in and cut-out vehicle.

The safe distance is calculated dynamically based on the Responsibility-Sensitive Safety (RSS) by the following equation.
The parameter `common.safe_distance_method` is used to select the safe distnace method to be either `RSS` or `time-based`.

#### RSS

In Responsibility-Sensitive Safety (RSS) method, the safe distance is calculated dynamically based on the following equation.

$$
d_{rss} = v_{ego} t_{idling} + \frac{1}{2} a_{ego} t_{idling}^2 + \frac{v_{ego}^2}{2 a_{ego}} - \frac{v_{obstacle}^2}{2 a_{obstacle}},
Expand Down Expand Up @@ -242,6 +248,20 @@ $$
| `lpf(val)` | apply low-pass filter to `val` |
| `pid(val)` | apply pid to `val` |

#### Time-based

In time-based method, the safe distance is calculated dynamically based on the following equation.

$$
d_{time,based} = v_{ego} * t_{safe,distance}
$$

assuming that

- $d_{time,based}$ is the calculated safe distance,
- $v_{ego}$ is the ego's current velocity, and
- $t_{safe,distance}$ is the selected time value time-based safe distance rule, e.g. 2-seconds rule, 3-seconds rule. Parameter: `common.safe_distance_time_margin`

### Slow down planning

| Parameter | Type | Description |
Expand Down
2 changes: 2 additions & 0 deletions planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -9,11 +9,13 @@
enable_slow_down_planning: true

# longitudinal info
safe_distance_method: "time_based" # currently supported safe distance calculation methods are "rss" and "time-based"
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you change the default one as rss_based since this new logic is not well tested, especially in the real environment.

idling_time: 2.0 # idling time to detect front vehicle starting deceleration [s]
min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss]
min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss]
safe_distance_margin : 6.0 # This is also used as a stop margin [m]
terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m]
safe_distance_time_margin : 2.0 # This is used for time-based safe distance rule, e.g. 2-seconds rule, 3-seconds rule [s]
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you separate the block depending on the logic as follows?

rss_based:
    idling_time: 2.0
    min_ego_accel_for_rs: -1.0
    ...
time_based:
    safe_distance_time_margin: 2.0

hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s]
hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m]
slow_down_min_acc: -1.0 # slow down min deceleration [m/ss]
Expand Down
8 changes: 8 additions & 0 deletions ...oware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -208,9 +208,11 @@
min_ego_accel_for_rss = node.declare_parameter<double>("common.min_ego_accel_for_rss");
min_object_accel_for_rss = node.declare_parameter<double>("common.min_object_accel_for_rss");

safe_distance_method = node.declare_parameter<std::string>("common.safe_distance_method");
safe_distance_margin = node.declare_parameter<double>("common.safe_distance_margin");
terminal_safe_distance_margin =
node.declare_parameter<double>("common.terminal_safe_distance_margin");
safe_distance_time_margin = node.declare_parameter<double>("common.safe_distance_time_margin");

hold_stop_velocity_threshold =
node.declare_parameter<double>("common.hold_stop_velocity_threshold");
Expand Down Expand Up @@ -239,10 +241,14 @@
autoware::universe_utils::updateParam<double>(
parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss);

autoware::universe_utils::updateParam<std::string>(
parameters, "common.safe_distance_method", safe_distance_method);

Check warning on line 245 in planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp#L245 

Added line #L245 was not covered by tests
autoware::universe_utils::updateParam<double>(
parameters, "common.safe_distance_margin", safe_distance_margin);
autoware::universe_utils::updateParam<double>(
parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin);
autoware::universe_utils::updateParam<double>(
parameters, "common.safe_distance_time_margin", safe_distance_time_margin);

Check warning on line 251 in planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp#L251 

Added line #L251 was not covered by tests

autoware::universe_utils::updateParam<double>(
parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold);
Expand All @@ -268,8 +274,10 @@
double min_object_accel_for_rss;

// distance margin
std::string safe_distance_method;
double safe_distance_margin;
double terminal_safe_distance_margin;
double safe_distance_time_margin;

// hold stop
double hold_stop_velocity_threshold;
Expand Down
6 changes: 6 additions & 0 deletions ...re_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -174,6 +174,12 @@
return rss_dist_with_margin;
}

double calcTimeBasedSafeDistance(const double ego_vel, const double time) const
{
double time_based_safe_distance = ego_vel * time;

Check warning on line 179 in planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp#L179 

Added line #L179 was not covered by tests
return time_based_safe_distance;
}

void updateCommonParam(const std::vector<rclcpp::Parameter> & parameters)
{
longitudinal_info_.onParam(parameters);
Expand Down
14 changes: 11 additions & 3 deletions planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2022 TIER IV, Inc.

Check notice on line 1 in planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Overall Code Complexity 

The mean cyclomatic complexity increases from 5.10 to 5.20, threshold = 4. This file has many conditional statements (e.g. if, for, while) across its implementation, leading to lower code health. Avoid adding more conditionals.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand Down Expand Up @@ -200,9 +200,17 @@
calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point) +
(obstacle.velocity - planner_data.ego_vel) * time_to_evaluate_rss_;

// calculate distance between ego and obstacle based on RSS
const double target_dist_to_obstacle = calcRSSDistance(
planner_data.ego_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin);
const double target_dist_to_obstacle = [&]() -> double {

Check warning on line 203 in planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp#L203 

Added line #L203 was not covered by tests
if (longitudinal_info_.safe_distance_method == "time_based") {
// calculate distance between ego and obstacle based on time-based rule
return calcTimeBasedSafeDistance(
planner_data.ego_vel, longitudinal_info_.safe_distance_time_margin);

Check warning on line 207 in planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp#L206-L207 

Added lines #L206 - L207 were not covered by tests
} else {
// calculate distance between ego and obstacle based on RSS
return calcRSSDistance(
planner_data.ego_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin);

Check warning on line 211 in planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp#L210-L211 

Added lines #L210 - L211 were not covered by tests
}
}();

Check warning on line 213 in planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

PIDBasedPlanner::calcObstacleToCruise increases in cyclomatic complexity from 9 to 10, threshold = 9. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 213 in planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp#L213 

Added line #L213 was not covered by tests
Comment on lines +203 to +213
Copy link
Contributor

@takayuki5168 takayuki5168 Jul 18, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can you create a member function of calcSafeDistance (you can change the name) which uses calcTimeBasedSafeDistance and calcRSSDistance internally, and make calcTimeBasedSafeDistance and calcRSSDistance functions private so that other people won't use calcTimeBasedSafeDistance and calcRSSDistance directly by mistake?


// calculate error distance and normalized one
const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle;
Expand Down
4 changes: 3 additions & 1 deletion planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -444,7 +444,9 @@
const PlannerData & planner_data, const StopObstacle & stop_obstacle) const
{
if (!enable_approaching_on_curve_ || use_pointcloud_) {
return longitudinal_info_.safe_distance_margin;
double margin_from_obstacle =
calcTimeBasedSafeDistance(planner_data.ego_vel, longitudinal_info_.safe_distance_time_margin);
return margin_from_obstacle;

Check warning on line 449 in planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method 

PlannerInterface::calculateMarginFromObstacleOnCurve already has high cyclomatic complexity, and now it increases in Lines of Code from 74 to 76. This function has many conditional statements (e.g. if, for, while), leading to lower code health. Avoid adding more conditionals and code to it without refactoring.

Check warning on line 449 in planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp

View check run for this annotation

Codecov / codecov/patch

planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp#L448-L449 

Added lines #L448 - L449 were not covered by tests
Comment on lines +447 to +449
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do we have to change this part as well?
Like the existing code, the distance for the STOP should be used I guess.

}

const double abs_ego_offset = planner_data.is_driving_forward
Expand Down
Loading