Skip to content

Commit

Permalink
fix goal_candidates
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Jan 30, 2025
1 parent 40b0f94 commit 46aaaee
Show file tree
Hide file tree
Showing 2 changed files with 15 additions and 35 deletions.
2 changes: 1 addition & 1 deletion ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -336,7 +336,7 @@ class GoalPlannerModule : public SceneModuleInterface

// goal searcher
std::optional<GoalSearcher> goal_searcher_{};
std::optional<GoalCandidates> goal_candidates_{};
GoalCandidates goal_candidates_{};

bool use_bus_stop_area_{false};

Expand Down
48 changes: 14 additions & 34 deletions ...avior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -637,9 +637,6 @@ void FreespaceParkingPlanner::onTimer()

std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::syncWithThreads()
{
assert(goal_candidates_);
const auto & goal_candidates = goal_candidates_.value();

// In PlannerManager::run(), it calls SceneModuleInterface::setData and
// SceneModuleInterface::setPreviousModuleOutput before module_ptr->run().
// Then module_ptr->run() invokes GoalPlannerModule::updateData and then
Expand All @@ -659,7 +656,7 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
std::lock_guard<std::mutex> guard(lane_parking_mutex_);
if (!lane_parking_request_) {
lane_parking_request_.emplace(
vehicle_footprint_, goal_candidates, getPreviousModuleOutput(), use_bus_stop_area_);
vehicle_footprint_, goal_candidates_, getPreviousModuleOutput(), use_bus_stop_area_);
}
// NOTE: for the above reasons, PlannerManager/behavior_path_planner_node ensure that
// planner_data_ is not nullptr, so it is OK to copy as value
Expand Down Expand Up @@ -690,7 +687,7 @@ std::pair<LaneParkingResponse, FreespaceParkingResponse> GoalPlannerModule::sync
std::lock_guard<std::mutex> guard(freespace_parking_mutex_);
if (!freespace_parking_request_) {
freespace_parking_request_.emplace(
parameters_, vehicle_footprint_, goal_candidates, *planner_data_);
parameters_, vehicle_footprint_, goal_candidates_, *planner_data_);
}
constexpr double stuck_time = 5.0;
freespace_parking_request_.value().update(
Expand All @@ -717,7 +714,7 @@ void GoalPlannerModule::updateData()
}
const auto & goal_searcher = goal_searcher_.value();

if (!goal_candidates_) {
if (goal_candidates_.empty()) {
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
*(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
parameters_.forward_goal_search_length);
Expand All @@ -732,7 +729,6 @@ void GoalPlannerModule::updateData()
});
goal_candidates_ = generateGoalCandidates(goal_searcher_.value(), use_bus_stop_area_);
}
auto & goal_candidates_mut = goal_candidates_.value();

const lanelet::ConstLanelets current_lanes =
utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_);
Expand Down Expand Up @@ -815,7 +811,7 @@ void GoalPlannerModule::updateData()
}
const auto & ctx_data = context_data_.value();
goal_searcher.update(
goal_candidates_mut, occupancy_grid_map_, planner_data_, ctx_data.static_target_objects);
goal_candidates_, occupancy_grid_map_, planner_data_, ctx_data.static_target_objects);
auto & ctx_data_mut = context_data_.value();

if (!isActivated()) {
Expand Down Expand Up @@ -1031,7 +1027,7 @@ BehaviorModuleOutput GoalPlannerModule::plan()
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (utils::isAllowedGoalModification(planner_data_->route_handler)) {
if (!context_data_ || !goal_candidates_) {
if (!context_data_) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000, " [pull_over] plan() is called without valid context_data");
} else {
Expand Down Expand Up @@ -1225,9 +1221,6 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

assert(goal_candidates_);
const auto & goal_candidates = goal_candidates_.value();

const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose();
const double backward_length =
parameters_.backward_goal_search_length + parameters_.decide_path_distance;
Expand All @@ -1244,7 +1237,7 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
// STEP1-1: Extract paths which have safe goal
// Create a map of goal_id to GoalCandidate for quick access
std::unordered_map<int, GoalCandidate> goal_candidate_map;
for (const auto & goal_candidate : goal_candidates) {
for (const auto & goal_candidate : goal_candidates_) {
goal_candidate_map[goal_candidate.id] = goal_candidate;
}
for (size_t i = 0; i < pull_over_path_candidates.size(); ++i) {
Expand Down Expand Up @@ -1281,7 +1274,7 @@ std::optional<PullOverPath> GoalPlannerModule::selectPullOverPath(
sorted_path_indices.end());

sortPullOverPaths(
planner_data_, parameters_, pull_over_path_candidates, goal_candidates,
planner_data_, parameters_, pull_over_path_candidates, goal_candidates_,

Check warning on line 1277 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp#L1277 

Added line #L1277 was not covered by tests
context_data.static_target_objects, getLogger(), sorted_path_indices);

// STEP3: Select the final pull over path by checking collision to make it as high priority as
Expand Down Expand Up @@ -1331,15 +1324,13 @@ void GoalPlannerModule::setOutput(
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

assert(!goal_candidates_);
const auto & goal_candidates = goal_candidates_.value();

output.reference_path = getPreviousModuleOutput().reference_path;

if (!selected_pull_over_path_with_velocity_opt) {
// situation : not safe against static objects use stop_path
// TODO(soblin): goal_candidates_.empty() is impossible
output.path = generateStopPath(
context_data, (goal_candidates.empty() ? "no goal candidate" : "no static safe path"));
context_data, (goal_candidates_.empty() ? "no goal candidate" : "no static safe path"));
RCLCPP_INFO_THROTTLE(
getLogger(), *clock_, 5000, "Not found safe pull_over path, generate stop path");
setDrivableAreaInfo(context_data, output);
Expand Down Expand Up @@ -1463,14 +1454,11 @@ BehaviorModuleOutput GoalPlannerModule::planPullOver(PullOverContextData & conte
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

assert(goal_candidates_);
const auto & goal_candidates = goal_candidates_.value();

const auto current_state = path_decision_controller_.get_current_state();
if (current_state.state != PathDecisionState::DecisionKind::DECIDED) {
const bool is_stable_safe = current_state.is_stable_safe;
const std::string detail =
goal_candidates.empty() ? "no goal candidate"
goal_candidates_.empty() ? "no goal candidate"
: context_data.lane_parking_response.pull_over_path_candidates.empty() ? "no path candidate"
: !context_data.pull_over_path_opt ? "no static safe path"
: !is_stable_safe ? "unsafe against dynamic objects"
Expand Down Expand Up @@ -1662,7 +1650,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApproval()
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

if (utils::isAllowedGoalModification(planner_data_->route_handler)) {
if (!context_data_ || !goal_candidates_) {
if (!context_data_) {
RCLCPP_WARN_THROTTLE(
getLogger(), *clock_, 5000,
" [pull_over] planWaitingApproval() is called without valid context_data. use fixed goal "
Expand Down Expand Up @@ -1715,9 +1703,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath(
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);
assert(!goal_searcher_);
assert(!goal_candidates_);
const auto & goal_searcher = goal_searcher_.value();

Check warning on line 1706 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp#L1706 

Added line #L1706 was not covered by tests
const auto & goal_candidates = goal_candidates_.value();

const auto & route_handler = planner_data_->route_handler;
const auto & current_pose = planner_data_->self_odometry->pose.pose;
Expand Down Expand Up @@ -1762,7 +1748,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath(
// approximate_pull_over_distance_ ego vehicle decelerates to this position. or if no feasible
// stop point is found, stop at this position.
const auto closest_goal_candidate =
goal_searcher.getClosetGoalCandidateAlongLanes(goal_candidates, planner_data_);
goal_searcher.getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_);

Check notice on line 1751 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Complex Method 

GoalPlannerModule::generateStopPath already has high cyclomatic complexity, and now it increases in Lines of Code from 79 to 81. 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.
const auto decel_pose = calcLongitudinalOffsetPose(
extended_prev_path.points, closest_goal_candidate.goal_pose.position,
-approximate_pull_over_distance_);
Expand Down Expand Up @@ -2093,12 +2079,9 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

assert(goal_candidates_);
const auto & goal_candidates = goal_candidates_.value();

// decelerate before the search area start
const auto closest_goal_candidate =
goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates, planner_data_);
goal_searcher_->getClosetGoalCandidateAlongLanes(goal_candidates_, planner_data_);
const auto decel_pose = calcLongitudinalOffsetPose(
pull_over_path.full_path().points, closest_goal_candidate.goal_pose.position,
-approximate_pull_over_distance_);
Expand Down Expand Up @@ -2449,9 +2432,6 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data)
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

assert(goal_candidates_);
const auto & goal_candidates = goal_candidates_.value();

debug_marker_.markers.clear();

using autoware::motion_utils::createStopVirtualWallMarker;
Expand Down Expand Up @@ -2485,7 +2465,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data)
goal_searcher_->getAreaPolygons(), header, color, z));

// Visualize goal candidates
add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color));
add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates_, color));

// Visualize objects extraction polygon
auto marker = autoware::universe_utils::createDefaultMarker(
Expand Down

0 comments on commit 46aaaee

Please sign in to comment.