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

Jump to bottom

fix(behavior_path_planner): suppress reseting root lanelet #9129

Open
wants to merge 1 commit into
base: main
Choose a base branch
from
Open

fix(behavior_path_planner): suppress reseting root lanelet #9129

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
4 changes: 3 additions & 1 deletion ...autoware_behavior_path_planner/include/autoware/behavior_path_planner/planner_manager.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -551,8 +551,10 @@ class PlannerManager
/**
* @brief find and set the closest lanelet within the route to current route lanelet
* @param planner data.
* @param is any approved module running.
*/
void updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data);
void updateCurrentRouteLanelet(
const std::shared_ptr<PlannerData> & data, const bool is_any_approved_module_running);

void generateCombinedDrivableArea(
BehaviorModuleOutput & output, const std::shared_ptr<PlannerData> & data) const;
Expand Down
2 changes: 1 addition & 1 deletion ...g/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -351,9 +351,9 @@ void BehaviorPathPlannerNode::run()
if (!is_first_time && !has_same_route_id) {
RCLCPP_INFO(get_logger(), "New uuid route is received. Resetting modules.");
planner_manager_->reset();
planner_manager_->resetCurrentRouteLanelet(planner_data_);
planner_data_->prev_modified_goal.reset();
}
planner_manager_->resetCurrentRouteLanelet(planner_data_);
}
const auto controlled_by_autoware_autonomously =
planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS &&
Expand Down
10 changes: 6 additions & 4 deletions planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2023 TIER IV, Inc.

Check notice on line 1 in planning/behavior_path_planner/autoware_behavior_path_planner/src/planner_manager.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.50 to 5.55, 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 @@ -135,7 +135,7 @@
const bool is_any_module_running =
is_any_approved_module_running || is_any_candidate_module_running_or_idle;

updateCurrentRouteLanelet(data);
updateCurrentRouteLanelet(data, is_any_approved_module_running);
Copy link
Contributor

Choose a reason for hiding this comment

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

if is_any_approved_module_running, then when the ego path is on the middle of two lanes while avoiding, ReferencePath can suddenly switch to the next lane. To prevent it, while modules are running, do not call this (resetCurrentRouteLanelet is called when lane_change successfully finished.)


const bool is_out_of_route = utils::isEgoOutOfRoute(
data->self_odometry->pose.pose, current_route_lanelet_->value(), data->prev_modified_goal,
Expand Down Expand Up @@ -228,7 +228,8 @@
utils::extractObstaclesFromDrivableArea(output.path, di.obstacles);
}

void PlannerManager::updateCurrentRouteLanelet(const std::shared_ptr<PlannerData> & data)
void PlannerManager::updateCurrentRouteLanelet(
const std::shared_ptr<PlannerData> & data, const bool is_any_approved_module_running)
{
const auto & route_handler = data->route_handler;
const auto & pose = data->self_odometry->pose.pose;
Expand Down Expand Up @@ -256,10 +257,11 @@
p.ego_nearest_yaw_threshold) ||
lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lane);

if (could_calculate_closest_lanelet)
if (could_calculate_closest_lanelet) {
*current_route_lanelet_ = closest_lane;
else
} else if (!is_any_approved_module_running) {
resetCurrentRouteLanelet(data);
}
}

BehaviorModuleOutput PlannerManager::getReferencePath(
Expand Down
Loading