Skip to content

Commit

Permalink
feat(avoidance): make detection area dynamically
Browse files Browse the repository at this point in the history
Signed-off-by: satoshi-ota <[email protected]>
  • Loading branch information
satoshi-ota committed Oct 16, 2023
1 parent dad4722 commit a11c9db
Show file tree
Hide file tree
Showing 8 changed files with 56 additions and 25 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -118,8 +118,6 @@
object_ignore_section_crosswalk_in_front_distance: 30.0 # [m]
object_ignore_section_crosswalk_behind_distance: 30.0 # [m]
# detection range
object_check_forward_distance: 150.0 # [m]
object_check_backward_distance: 2.0 # [m]
object_check_goal_distance: 20.0 # [m]
# filtering parking objects
threshold_distance_object_is_on_center: 1.0 # [m]
Expand All @@ -128,6 +126,13 @@
# lost object compensation
object_last_seen_threshold: 2.0

# detection area generation parameters
detection_area:
static: true # [-]
min_forward_distance: 50.0 # [m]
max_forward_distance: 150.0 # [m]
backward_distance: 10.0 # [m]

# For safety check
safety_check:
# safety check configuration
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -145,9 +145,9 @@ struct AvoidanceParameters
double object_ignore_section_crosswalk_behind_distance{0.0};

// distance to avoid object detection
double object_check_forward_distance{0.0};

// continue to detect backward vehicles as avoidance targets until they are this distance away
bool use_static_detection_area{true};
double object_check_min_forward_distance{0.0};
double object_check_max_forward_distance{0.0};
double object_check_backward_distance{0.0};

// if the distance between object and goal position is less than this parameter, the module ignore
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -173,6 +173,22 @@ class AvoidanceHelper
: std::max(shift_length, getRightShiftBound());
}

double getForwardDetectionRange() const
{
if (parameters_->use_static_detection_area) {
return parameters_->object_check_max_forward_distance;
}

const auto max_shift_length = std::max(
std::abs(parameters_->max_right_shift_length), std::abs(parameters_->max_left_shift_length));
const auto dynamic_distance = PathShifter::calcLongitudinalDistFromJerk(
max_shift_length, getLateralMinJerkLimit(), getEgoSpeed());

return std::clamp(
1.5 * dynamic_distance, parameters_->object_check_min_forward_distance,
parameters_->object_check_max_forward_distance);
}

void alignShiftLinesOrder(AvoidLineArray & lines, const bool align_shift_length = true) const
{
if (lines.empty()) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -164,7 +164,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
const bool is_running, DebugData & debug);
const double object_check_forward_distance, const bool is_running, DebugData & debug);

DrivableLanes generateExpandDrivableLanes(
const lanelet::ConstLanelet & lanelet, const std::shared_ptr<const PlannerData> & planner_data,
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -322,11 +322,12 @@ void AvoidanceModule::fillAvoidanceTargetObjects(
getCurrentStatus() == ModuleStatus::WAITING_APPROVAL;

// Separate dynamic objects based on whether they are inside or outside of the expanded lanelets.
const auto sparse_resample_path = utils::resamplePathWithSpline(
helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output);
const auto [object_within_target_lane, object_outside_target_lane] =
utils::avoidance::separateObjectsByPath(
utils::resamplePathWithSpline(
helper_.getPreviousSplineShiftPath().path, parameters_->resample_interval_for_output),
planner_data_, data, parameters_, is_running, debug);
sparse_resample_path, planner_data_, data, parameters_, helper_.getForwardDetectionRange(),
is_running, debug);

for (const auto & object : object_outside_target_lane.objects) {
ObjectData other_object;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -114,10 +114,6 @@ AvoidanceModuleManager::AvoidanceModuleManager(
*node, ns + "object_ignore_section_crosswalk_in_front_distance");
p.object_ignore_section_crosswalk_behind_distance =
getOrDeclareParameter<double>(*node, ns + "object_ignore_section_crosswalk_behind_distance");
p.object_check_forward_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_forward_distance");
p.object_check_backward_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_backward_distance");
p.object_check_goal_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
p.threshold_distance_object_is_on_center =
Expand All @@ -130,6 +126,17 @@ AvoidanceModuleManager::AvoidanceModuleManager(
getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
}

{
std::string ns = "avoidance.target_filtering.detection_area.";
p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");
p.object_check_min_forward_distance =
getOrDeclareParameter<double>(*node, ns + "min_forward_distance");
p.object_check_max_forward_distance =
getOrDeclareParameter<double>(*node, ns + "max_forward_distance");
p.object_check_backward_distance =
getOrDeclareParameter<double>(*node, ns + "backward_distance");
}

Check warning on line 139 in planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Complex Method

AvoidanceModuleManager::AvoidanceModuleManager already has high cyclomatic complexity, and now it increases in Lines of Code from 237 to 243. 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.
// safety check general params
{
std::string ns = "avoidance.safety_check.";
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -329,10 +329,6 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
*node, ns + "object_ignore_section_crosswalk_in_front_distance");
p.object_ignore_section_crosswalk_behind_distance =
getOrDeclareParameter<double>(*node, ns + "object_ignore_section_crosswalk_behind_distance");
p.object_check_forward_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_forward_distance");
p.object_check_backward_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_backward_distance");
p.object_check_goal_distance =
getOrDeclareParameter<double>(*node, ns + "object_check_goal_distance");
p.threshold_distance_object_is_on_center =
Expand All @@ -345,6 +341,17 @@ AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager(
getOrDeclareParameter<double>(*node, ns + "object_last_seen_threshold");
}

{
std::string ns = "avoidance.target_filtering.detection_area.";
p.use_static_detection_area = getOrDeclareParameter<bool>(*node, ns + "static");
p.object_check_min_forward_distance =
getOrDeclareParameter<double>(*node, ns + "min_forward_distance");
p.object_check_max_forward_distance =
getOrDeclareParameter<double>(*node, ns + "max_forward_distance");
p.object_check_backward_distance =
getOrDeclareParameter<double>(*node, ns + "backward_distance");
}

Check warning on line 354 in planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

❌ Getting worse: Large Method

AvoidanceByLaneChangeModuleManager::AvoidanceByLaneChangeModuleManager increases from 98 to 104 lines of code, threshold = 70. Large functions with many lines of code are generally harder to understand and lower the code health. Avoid adding more lines to this function.
// safety check
{
std::string ns = "avoidance.safety_check.";
Expand Down
11 changes: 3 additions & 8 deletions planning/behavior_path_planner/src/utils/avoidance/utils.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -977,11 +977,6 @@ void filterTargetObjects(
data.other_objects.push_back(o);
continue;
}

Check notice on line 979 in planning/behavior_path_planner/src/utils/avoidance/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

✅ Getting better: Complex Method

filterTargetObjects decreases in cyclomatic complexity from 62 to 60, 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.
if (o.longitudinal > parameters->object_check_forward_distance) {
o.reason = AvoidanceDebugFactor::OBJECT_IS_IN_FRONT_THRESHOLD;
data.other_objects.push_back(o);
continue;
}

// Target object is behind the path goal -> ignore.
if (o.longitudinal > dist_to_goal) {
Expand Down Expand Up @@ -1465,7 +1460,7 @@ lanelet::ConstLanelets getAdjacentLane(
const std::shared_ptr<AvoidanceParameters> & parameters, const bool is_right_shift)
{
const auto & rh = planner_data->route_handler;
const auto & forward_distance = parameters->object_check_forward_distance;
const auto & forward_distance = parameters->object_check_max_forward_distance;
const auto & backward_distance = parameters->safety_check_backward_distance;
const auto & vehicle_pose = planner_data->self_odometry->pose.pose;

Expand Down Expand Up @@ -1605,7 +1600,7 @@ std::vector<ExtendedPredictedObject> getSafetyCheckTargetObjects(
std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const PathWithLaneId & path, const std::shared_ptr<const PlannerData> & planner_data,
const AvoidancePlanningData & data, const std::shared_ptr<AvoidanceParameters> & parameters,
const bool is_running, DebugData & debug)
const double object_check_forward_distance, const bool is_running, DebugData & debug)
{
PredictedObjects target_objects;
PredictedObjects other_objects;
Expand All @@ -1628,7 +1623,7 @@ std::pair<PredictedObjects, PredictedObjects> separateObjectsByPath(
const auto & p_ego_back = path.points.at(i + 1).point.pose;

const auto distance_from_ego = calcSignedArcLength(path.points, ego_idx, i);
if (distance_from_ego > parameters->object_check_forward_distance) {
if (distance_from_ego > object_check_forward_distance) {

Check notice on line 1626 in planning/behavior_path_planner/src/utils/avoidance/utils.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Excess Number of Function Arguments

separateObjectsByPath increases from 6 to 7 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.
break;
}

Expand Down

0 comments on commit a11c9db

Please sign in to comment.