-
Notifications
You must be signed in to change notification settings - Fork 631
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.
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
fix(goal_planner): fix parking_path curvature and DecidingState transition #9022
Changes from all commits
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -1,4 +1,4 @@ | ||
// Copyright 2021 Tier IV, Inc. | ||
Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)ℹ Getting worse: Lines of Code in a Single File
|
||
// | ||
// Licensed under the Apache License, Version 2.0 (the "License"); | ||
// you may not use this file except in compliance with the License. | ||
|
@@ -2643,7 +2643,6 @@ | |
found_pull_over_path, now, static_target_objects, dynamic_target_objects, modified_goal_opt, | ||
planner_data, occupancy_grid_map, is_current_safe, parameters, goal_searcher, is_activated, | ||
pull_over_path, ego_polygons_expanded); | ||
prev_state_ = current_state_; | ||
current_state_ = next_state; | ||
} | ||
|
||
|
@@ -2658,10 +2657,7 @@ | |
const std::optional<PullOverPath> & pull_over_path_opt, | ||
std::vector<Polygon2d> & ego_polygons_expanded) const | ||
{ | ||
auto next_state = prev_state_; | ||
|
||
// update timestamp | ||
next_state.stamp = now; | ||
auto next_state = current_state_; | ||
Check warning on line 2660 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp Codecov / codecov/patchplanning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp#L2660
|
||
|
||
// update safety | ||
if (!parameters.safety_check_params.enable_safety_check) { | ||
|
@@ -2684,7 +2680,6 @@ | |
|
||
// Once this function returns true, it will continue to return true thereafter | ||
if (next_state.state == PathDecisionState::DecisionKind::DECIDED) { | ||
next_state.state = PathDecisionState::DecisionKind::DECIDED; | ||
return next_state; | ||
} | ||
|
||
|
@@ -2699,20 +2694,26 @@ | |
// If it is dangerous against dynamic objects before approval, do not determine the path. | ||
// This eliminates a unsafe path to be approved | ||
if (enable_safety_check && !next_state.is_stable_safe && !is_activated) { | ||
RCLCPP_DEBUG( | ||
logger_, | ||
"[DecidingPathStatus]: NOT_DECIDED. path is not safe against dynamic objects before " | ||
"approval"); | ||
next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; | ||
return next_state; | ||
} | ||
|
||
const auto & current_path = pull_over_path.getCurrentPath(); | ||
if (prev_state_.state == PathDecisionState::DecisionKind::DECIDING) { | ||
if (current_state_.state == PathDecisionState::DecisionKind::DECIDING) { | ||
const double hysteresis_factor = 0.9; | ||
|
||
// check goal pose collision | ||
if ( | ||
modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor( | ||
modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, | ||
planner_data, static_target_objects)) { | ||
RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe"); | ||
next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; | ||
next_state.deciding_start_time = std::nullopt; | ||
return next_state; | ||
} | ||
|
||
|
@@ -2727,24 +2728,36 @@ | |
/*extract_static_objects=*/false, parameters.maximum_deceleration, | ||
parameters.object_recognition_collision_check_max_extra_stopping_margin, | ||
ego_polygons_expanded, true)) { | ||
RCLCPP_DEBUG( | ||
logger_, "[DecidingPathStatus]: DECIDING->NOT_DECIDED. path has collision with objects"); | ||
next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; | ||
next_state.deciding_start_time = std::nullopt; | ||
return next_state; | ||
} | ||
|
||
if (enable_safety_check && !next_state.is_stable_safe) { | ||
RCLCPP_DEBUG( | ||
logger_, | ||
"[DecidingPathStatus]: DECIDING->NOT_DECIDED. path is not safe against dynamic objects"); | ||
next_state.state = PathDecisionState::DecisionKind::NOT_DECIDED; | ||
return next_state; | ||
} | ||
|
||
// if enough time has passed since deciding status starts, transition to DECIDED | ||
constexpr double check_collision_duration = 1.0; | ||
const double elapsed_time_from_deciding = (now - prev_state_.stamp).seconds(); | ||
const double elapsed_time_from_deciding = | ||
(now - current_state_.deciding_start_time.value()).seconds(); | ||
if (elapsed_time_from_deciding > check_collision_duration) { | ||
RCLCPP_DEBUG(logger_, "[DecidingPathStatus]: DECIDING->DECIDED. has enough safe time passed"); | ||
next_state.state = PathDecisionState::DecisionKind::DECIDED; | ||
next_state.deciding_start_time = std::nullopt; | ||
return next_state; | ||
} | ||
|
||
// if enough time has NOT passed since deciding status starts, keep DECIDING | ||
RCLCPP_DEBUG( | ||
logger_, "[DecidingPathStatus]: keep DECIDING. elapsed_time_from_deciding: %f", | ||
elapsed_time_from_deciding); | ||
return next_state; | ||
} | ||
|
||
|
@@ -2766,10 +2779,15 @@ | |
// if object recognition for path collision check is enabled, transition to DECIDING to check | ||
// collision for a certain period of time. Otherwise, transition to DECIDED directly. | ||
if (parameters.use_object_recognition) { | ||
next_state.state = PathDecisionState::DecisionKind::DECIDED; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. In prior PR, I mistakenly changed the next state to DECIDED. DECIDING is correct for this case. |
||
RCLCPP_DEBUG( | ||
logger_, | ||
"[DecidingPathStatus]: NOT_DECIDED->DECIDING. start checking collision for certain " | ||
"period of time"); | ||
next_state.state = PathDecisionState::DecisionKind::DECIDING; | ||
Check warning on line 2786 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp Codecov / codecov/patchplanning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp#L2786
|
||
next_state.deciding_start_time = now; | ||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. fixedto return DECIDING as the next state. |
||
return next_state; | ||
} | ||
return {PathDecisionState::DecisionKind::DECIDED, now}; | ||
return {PathDecisionState::DecisionKind::DECIDED, std::nullopt}; | ||
Check warning on line 2790 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)❌ Getting worse: Complex Method
Check warning on line 2790 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp Codecov / codecov/patchplanning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp#L2790
|
||
} | ||
|
||
} // namespace autoware::behavior_path_planner |
Original file line number | Diff line number | Diff line change |
---|---|---|
|
@@ -73,7 +73,7 @@ std::optional<PullOverPath> PullOverPath::create( | |
double parking_path_max_curvature{}; | ||
std::tie(full_path_curvatures, full_path_max_curvature) = calculateCurvaturesAndMax(full_path); | ||
std::tie(parking_path_curvatures, parking_path_max_curvature) = | ||
calculateCurvaturesAndMax(full_path); | ||
calculateCurvaturesAndMax(parking_path); | ||
|
||
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I was using |
||
return PullOverPath( | ||
type, goal_id, id, start_pose, end_pose, partial_paths, full_path, parking_path, | ||
|
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Only
prev_state_.stamp
was used for saving the time when DECIDING state started.So I deleted
prev_state_
and savedeciding_start_time
instead.