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.

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_common): add safety target object located on shoulder lane #6839

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -330,48 +330,62 @@

lanelet::ConstLanelets all_left_lanelets;
lanelet::ConstLanelets all_right_lanelets;
// TODO(sugahara): consider right side parking and define right shoulder lanelets
lanelet::ConstLanelets all_left_shoulder_lanelets;

// Define lambda functions to update left and right lanelets
const auto update_left_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
const auto left_lanelets = route_handler->getAllLeftSharedLinestringLanelets(
target_lane, include_opposite, invert_opposite);
all_left_lanelets.insert(all_left_lanelets.end(), left_lanelets.begin(), left_lanelets.end());
};

const auto update_right_lanelets = [&](const lanelet::ConstLanelet & target_lane) {
const auto right_lanelets = route_handler->getAllRightSharedLinestringLanelets(
target_lane, include_opposite, invert_opposite);
all_right_lanelets.insert(
all_right_lanelets.end(), right_lanelets.begin(), right_lanelets.end());
};

// Update left and right lanelets for each current lane
const auto update_left_shoulder_lanelet = [&](const lanelet::ConstLanelet & target_lane) {
lanelet::ConstLanelet neighbor_shoulder_lane{};
const bool shoulder_lane_is_found =
route_handler->getLeftShoulderLanelet(target_lane, &neighbor_shoulder_lane);
if (shoulder_lane_is_found) {
all_left_shoulder_lanelets.insert(all_left_shoulder_lanelets.end(), neighbor_shoulder_lane);
}
};

for (const auto & current_lane : current_lanes) {
update_left_lanelets(current_lane);
update_right_lanelets(current_lane);
update_left_shoulder_lanelet(current_lane);
}

TargetObjectsOnLane target_objects_on_lane{};

const auto append_objects_on_lane = [&](auto & lane_objects, const auto & check_lanes) {
std::for_each(
filtered_objects.objects.begin(), filtered_objects.objects.end(), [&](const auto & object) {
if (isCentroidWithinLanelets(object, check_lanes)) {
lane_objects.push_back(
transform(object, safety_check_time_horizon, safety_check_time_resolution));
}
});
};

// TODO(Sugahara): Consider shoulder and other lane objects
// TODO(Sugahara): Consider other lane objects
if (object_lane_configuration.check_current_lane && !current_lanes.empty()) {
append_objects_on_lane(target_objects_on_lane.on_current_lane, current_lanes);
}
if (object_lane_configuration.check_left_lane && !all_left_lanelets.empty()) {
append_objects_on_lane(target_objects_on_lane.on_left_lane, all_left_lanelets);
}
if (object_lane_configuration.check_right_lane && !all_right_lanelets.empty()) {
append_objects_on_lane(target_objects_on_lane.on_right_lane, all_right_lanelets);
}
if (object_lane_configuration.check_shoulder_lane && !all_left_shoulder_lanelets.empty()) {
append_objects_on_lane(target_objects_on_lane.on_shoulder_lane, all_left_shoulder_lanelets);
}

Check warning on line 388 in planning/behavior_path_planner_common/src/utils/path_safety_checker/objects_filtering.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

createTargetObjectsOnLane increases in cyclomatic complexity from 9 to 12, 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.

return target_objects_on_lane;
}
Expand Down
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/behavior_path_start_planner_module/src/start_planner_module.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Lines of Code in a Single File

The lines of code increases from 1325 to 1334, improve code health by reducing it to 1000. The number of Lines of Code in a single file. More Lines of Code lowers the code health.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -16,6 +16,7 @@

#include "behavior_path_planner_common/utils/parking_departure/utils.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
#include "behavior_path_planner_common/utils/path_utils.hpp"
#include "behavior_path_start_planner_module/debug.hpp"
#include "behavior_path_start_planner_module/util.hpp"
Expand All @@ -39,6 +40,7 @@
#include <vector>

using behavior_path_planner::utils::parking_departure::initializeCollisionCheckDebugMap;
using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObject;
using motion_utils::calcLateralOffset;
using motion_utils::calcLongitudinalOffsetPose;
using tier4_autoware_utils::calcOffsetPose;
Expand Down Expand Up @@ -1324,10 +1326,18 @@
debug_data_.target_objects_on_lane = target_objects_on_lane;
debug_data_.ego_predicted_path = ego_predicted_path;
}

std::vector<ExtendedPredictedObject> merged_target_object;
merged_target_object.reserve(
target_objects_on_lane.on_current_lane.size() + target_objects_on_lane.on_shoulder_lane.size());
merged_target_object.insert(
merged_target_object.end(), target_objects_on_lane.on_current_lane.begin(),
target_objects_on_lane.on_current_lane.end());
merged_target_object.insert(
merged_target_object.end(), target_objects_on_lane.on_shoulder_lane.begin(),
target_objects_on_lane.on_shoulder_lane.end());
return behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS(
pull_out_path, ego_predicted_path, target_objects_on_lane.on_current_lane,
debug_data_.collision_check, planner_data_->parameters, safety_check_params_->rss_params,
pull_out_path, ego_predicted_path, merged_target_object, debug_data_.collision_check,
planner_data_->parameters, safety_check_params_->rss_params,
objects_filtering_params_->use_all_predicted_path, hysteresis_factor);
}

Expand Down
Loading