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(behavior_path_planner): add processOnEntry in start_planner #4953

Merged
merged 4 commits into from
Sep 12, 2023
Merged

feat(behavior_path_planner): add processOnEntry in start_planner #4953

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
48c92ae
add process on entry in start_planner
kyoichi-sugahara Sep 12, 2023
f269697
delete description
kyoichi-sugahara Sep 12, 2023
5a9e4d8
Merge branch 'main' into feature/add_processOnEntry_start_planner
kyoichi-sugahara Sep 12, 2023
f434e0c
Merge branch 'main' into feature/add_processOnEntry_start_planner
kyoichi-sugahara Sep 12, 2023
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
13 changes: 7 additions & 6 deletions ...h_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -81,12 +81,12 @@ struct PullOverStatus
size_t current_path_idx{0};
bool require_increment_{true}; // if false, keep current path idx.
std::shared_ptr<PathWithLaneId> prev_stop_path{nullptr};
lanelet::ConstLanelets current_lanes{};
lanelet::ConstLanelets pull_over_lanes{};
std::vector<DrivableLanes> lanes{}; // current + pull_over
bool has_decided_path{false};
bool is_safe_static_objects{false}; // current path is safe against static objects
bool is_safe_dynamic_objects{false}; // current path is safe against dynamic objects
lanelet::ConstLanelets current_lanes{}; // TODO(someone): explain
lanelet::ConstLanelets pull_over_lanes{}; // TODO(someone): explain
std::vector<DrivableLanes> lanes{}; // current + pull_over
bool has_decided_path{false}; // if true, the path is decided and safe against static objects
bool is_safe_static_objects{false}; // current path is safe against *static* objects
bool is_safe_dynamic_objects{false}; // current path is safe against *dynamic* objects
bool prev_is_safe{false};
bool has_decided_velocity{false};
bool has_requested_approval{false};
Expand Down Expand Up @@ -299,6 +299,7 @@ class GoalPlannerModule : public SceneModuleInterface
std::pair<double, double> calcDistanceToPathChange() const;

// safety check
void initializeSafetyCheckParameters();
SafetyCheckParams createSafetyCheckParams() const;
void updateSafetyCheckTargetObjectsData(
const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
Expand Down
4 changes: 3 additions & 1 deletion ...planner/include/behavior_path_planner/scene_module/start_planner/start_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -93,7 +93,7 @@ class StartPlannerModule : public SceneModuleInterface
BehaviorModuleOutput plan() override;
BehaviorModuleOutput planWaitingApproval() override;
CandidateOutput planCandidate() const override;

void processOnEntry() override;
void processOnExit() override;

void setParameters(const std::shared_ptr<StartPlannerParameters> & parameters)
Expand Down Expand Up @@ -125,6 +125,8 @@ class StartPlannerModule : public SceneModuleInterface

bool canTransitIdleToRunningState() override { return false; }

void initializeSafetyCheckParameters();

std::shared_ptr<StartPlannerParameters> parameters_;
mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
Expand Down
29 changes: 20 additions & 9 deletions planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -252,12 +252,27 @@ void GoalPlannerModule::initializeOccupancyGridMap()
occupancy_grid_map_->setParam(occupancy_grid_map_param);
}

void GoalPlannerModule::initializeSafetyCheckParameters()
{
utils::start_goal_planner_common::updateEgoPredictedPathParams(
ego_predicted_path_params_, parameters_);
utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
utils::start_goal_planner_common::updateObjectsFilteringParams(
objects_filtering_params_, parameters_);
}

void GoalPlannerModule::processOnEntry()
{
// Initialize occupancy grid map
if (parameters_->use_occupancy_grid) {
initializeOccupancyGridMap();
}
// Initialize safety checker
if (parameters_->safety_check_params.enable_safety_check) {
initializeSafetyCheckParameters();
utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
goal_planner_data_.collision_check);
}
}

void GoalPlannerModule::processOnExit()
Expand Down Expand Up @@ -354,11 +369,6 @@ bool GoalPlannerModule::isExecutionReady() const
}

if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) {
utils::start_goal_planner_common::updateEgoPredictedPathParams(
ego_predicted_path_params_, parameters_);
utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
utils::start_goal_planner_common::updateObjectsFilteringParams(
objects_filtering_params_, parameters_);
if (!isSafePath()) {
RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
return false;
Expand Down Expand Up @@ -690,7 +700,6 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output)
if (status_.prev_is_safe || status_.prev_stop_path == nullptr) {
// safe -> not_safe or no prev_stop_path: generate new stop_path
output.path = std::make_shared<PathWithLaneId>(generateStopPath());
output.reference_path = getPreviousModuleOutput().reference_path;
status_.prev_stop_path = output.path;
// set stop path as pull over path
mutex_.lock();
Expand All @@ -705,10 +714,10 @@ void GoalPlannerModule::setStopPath(BehaviorModuleOutput & output)
} else {
// not_safe -> not_safe: use previous stop path
output.path = status_.prev_stop_path;
output.reference_path = getPreviousModuleOutput().reference_path;
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, "Not found safe pull_over path, use previous stop path");
}
output.reference_path = getPreviousModuleOutput().reference_path;
}

void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
Expand Down Expand Up @@ -1097,6 +1106,10 @@ bool GoalPlannerModule::incrementPathIndex()

PathWithLaneId GoalPlannerModule::getCurrentPath() const
{
if (status_.pull_over_path == nullptr) {
return PathWithLaneId{};
}

if (status_.pull_over_path->partial_paths.size() <= status_.current_path_idx) {
return PathWithLaneId{};
}
Expand Down Expand Up @@ -1521,8 +1534,6 @@ bool GoalPlannerModule::isSafePath() const

utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
goal_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
goal_planner_data_.collision_check);

bool is_safe_dynamic_objects = true;
// Check for collisions with each predicted path of the object
Expand Down
26 changes: 19 additions & 7 deletions planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,16 @@ BehaviorModuleOutput StartPlannerModule::run()
return plan();
}

void StartPlannerModule::processOnEntry()
{
// Initialize safety checker
if (parameters_->safety_check_params.enable_safety_check) {
initializeSafetyCheckParameters();
utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
start_planner_data_.collision_check);
}
}

void StartPlannerModule::processOnExit()
{
resetPathCandidate();
Expand Down Expand Up @@ -149,11 +159,6 @@ bool StartPlannerModule::isExecutionReady() const
}

if (status_.is_safe_static_objects && parameters_->safety_check_params.enable_safety_check) {
utils::start_goal_planner_common::updateEgoPredictedPathParams(
ego_predicted_path_params_, parameters_);
utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
utils::start_goal_planner_common::updateObjectsFilteringParams(
objects_filtering_params_, parameters_);
if (!isSafePath()) {
RCLCPP_ERROR_THROTTLE(getLogger(), *clock_, 5000, "Path is not safe against dynamic objects");
return false;
Expand Down Expand Up @@ -272,6 +277,15 @@ CandidateOutput StartPlannerModule::planCandidate() const
return CandidateOutput{};
}

void StartPlannerModule::initializeSafetyCheckParameters()
{
utils::start_goal_planner_common::updateEgoPredictedPathParams(
ego_predicted_path_params_, parameters_);
utils::start_goal_planner_common::updateSafetyCheckParams(safety_check_params_, parameters_);
utils::start_goal_planner_common::updateObjectsFilteringParams(
objects_filtering_params_, parameters_);
}

PathWithLaneId StartPlannerModule::getFullPath() const
{
// combine partial pull out path
Expand Down Expand Up @@ -982,8 +996,6 @@ bool StartPlannerModule::isSafePath() const

utils::start_goal_planner_common::updateSafetyCheckTargetObjectsData(
start_planner_data_, filtered_objects, target_objects_on_lane, ego_predicted_path);
utils::start_goal_planner_common::initializeCollisionCheckDebugMap(
start_planner_data_.collision_check);

bool is_safe_dynamic_objects = true;
// Check for collisions with each predicted path of the object
Expand Down