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(goal_planner): use expanded pull over lanes for filtering in goal searcher #6142

Merged
merged 1 commit into from
Jan 23, 2024
Merged

feat(goal_planner): use expanded pull over lanes for filtering in goal searcher #6142

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
10 changes: 10 additions & 0 deletions ...ning/behavior_path_goal_planner_module/include/behavior_path_goal_planner_module/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -45,9 +45,19 @@ using visualization_msgs::msg::MarkerArray;
lanelet::ConstLanelets getPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance);
lanelet::ConstLanelets generateExpandedPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance, double bound_offset);
PredictedObjects extractObjectsInExpandedPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance, double bound_offset, const PredictedObjects & objects);
PredictedObjects filterObjectsByLateralDistance(
const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
const double distance_thresh, const bool filter_inside);
PredictedObjects extractStaticObjectsInExpandedPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance, double bound_offset, const PredictedObjects & objects,
const double velocity_thresh);

double calcLateralDeviationBetweenPaths(
const PathWithLaneId & reference_path, const PathWithLaneId & target_path);
Expand Down
20 changes: 5 additions & 15 deletions planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/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 better: Lines of Code in a Single File 

The lines of code decreases from 1565 to 1556, 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.

Check notice on line 1 in planning/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 better: Overall Code Complexity 

The mean cyclomatic complexity decreases from 4.91 to 4.89, 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 @@ -1509,22 +1509,12 @@
const PathWithLaneId & path, const double collision_check_margin,
const bool update_debug_data) const
{
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);
const auto pull_over_lane_stop_objects =
goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
*(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length,

Check warning on line 1514 in planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp#L1514 

Added line #L1514 was not covered by tests
parameters_->forward_goal_search_length, parameters_->detection_bound_offset,
*(planner_data_->dynamic_object), parameters_->th_moving_object_velocity);

Check warning on line 1516 in planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/goal_planner_module.cpp#L1516 

Added line #L1516 was not covered by tests

const auto expanded_pull_over_lanes =
left_side_parking_ ? lanelet::utils::getExpandedLanelets(
pull_over_lanes, parameters_->detection_bound_offset, 0.0)
: lanelet::utils::getExpandedLanelets(
pull_over_lanes, 0.0, parameters_->detection_bound_offset);

const auto [pull_over_lane_objects, others] =
utils::path_safety_checker::separateObjectsByLanelets(
*(planner_data_->dynamic_object), expanded_pull_over_lanes,
utils::path_safety_checker::isPolygonOverlapLanelet);
const auto pull_over_lane_stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
pull_over_lane_objects, parameters_->th_moving_object_velocity);
std::vector<Polygon2d> obj_polygons;
for (const auto & object : pull_over_lane_stop_objects.objects) {
obj_polygons.push_back(tier4_autoware_utils::toPolygon2d(object));
Expand Down
13 changes: 5 additions & 8 deletions planning/behavior_path_goal_planner_module/src/goal_searcher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -266,14 +266,11 @@

void GoalSearcher::update(GoalCandidates & goal_candidates) const
{
const auto stop_objects = utils::path_safety_checker::filterObjectsByVelocity(
*(planner_data_->dynamic_object), parameters_.th_moving_object_velocity);
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);
const auto [pull_over_lane_stop_objects, others] =
utils::path_safety_checker::separateObjectsByLanelets(
stop_objects, pull_over_lanes, utils::path_safety_checker::isPolygonOverlapLanelet);
const auto pull_over_lane_stop_objects =
goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
*(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length,
parameters_.forward_goal_search_length, parameters_.detection_bound_offset,
*(planner_data_->dynamic_object), parameters_.th_moving_object_velocity);

Check warning on line 273 in planning/behavior_path_goal_planner_module/src/goal_searcher.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/goal_searcher.cpp#L271-L273 

Added lines #L271 - L273 were not covered by tests

if (parameters_.prioritize_goals_before_objects) {
countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects);
Expand Down
37 changes: 37 additions & 0 deletions planning/behavior_path_goal_planner_module/src/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
// Copyright 2021 Tier IV, Inc.

Check notice on line 1 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ Getting worse: Primitive Obsession 

The ratio of primitive types in function arguments increases from 32.50% to 44.83%, threshold = 30.0%. The functions in this file have too many primitive types (e.g. int, double, float) in their function argument lists. Using many primitive types lead to the code smell Primitive Obsession. Avoid adding more primitive arguments.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
Expand All @@ -14,6 +14,7 @@

#include "behavior_path_goal_planner_module/util.hpp"

#include "behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp"
#include "behavior_path_planner_common/utils/utils.hpp"

#include <lanelet2_extension/utility/query.hpp>
Expand Down Expand Up @@ -41,6 +42,7 @@
using tier4_autoware_utils::createMarkerColor;
using tier4_autoware_utils::createMarkerScale;
using tier4_autoware_utils::createPoint;

lanelet::ConstLanelets getPullOverLanes(
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance)
Expand Down Expand Up @@ -74,6 +76,41 @@
outermost_lane, backward_distance_with_buffer, forward_distance, only_route_lanes);
}

lanelet::ConstLanelets generateExpandedPullOverLanes(

Check warning on line 79 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L79 

Added line #L79 was not covered by tests
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance, double bound_offset)
{
const auto pull_over_lanes =
getPullOverLanes(route_handler, left_side, backward_distance, forward_distance);

Check warning on line 84 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L84 

Added line #L84 was not covered by tests

return left_side ? lanelet::utils::getExpandedLanelets(pull_over_lanes, bound_offset, 0.0)
: lanelet::utils::getExpandedLanelets(pull_over_lanes, 0.0, bound_offset);
}

Check notice on line 88 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Excess Number of Function Arguments 

generateExpandedPullOverLanes has 5 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check warning on line 88 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L87-L88 

Added lines #L87 - L88 were not covered by tests

PredictedObjects extractObjectsInExpandedPullOverLanes(

Check warning on line 90 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L90 

Added line #L90 was not covered by tests
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance, double bound_offset, const PredictedObjects & objects)
{
const auto lanes = generateExpandedPullOverLanes(
route_handler, left_side, backward_distance, forward_distance, bound_offset);

Check warning on line 95 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L95 

Added line #L95 was not covered by tests

const auto [objects_in_lanes, others] = utils::path_safety_checker::separateObjectsByLanelets(
objects, lanes, utils::path_safety_checker::isPolygonOverlapLanelet);

Check warning on line 98 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L98 

Added line #L98 was not covered by tests

return objects_in_lanes;
}

Check notice on line 101 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Excess Number of Function Arguments 

extractObjectsInExpandedPullOverLanes has 6 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check warning on line 101 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L100-L101 

Added lines #L100 - L101 were not covered by tests

PredictedObjects extractStaticObjectsInExpandedPullOverLanes(

Check warning on line 103 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L103 

Added line #L103 was not covered by tests
const RouteHandler & route_handler, const bool left_side, const double backward_distance,
const double forward_distance, double bound_offset, const PredictedObjects & objects,
const double velocity_thresh)
{
const auto objects_in_lanes = extractObjectsInExpandedPullOverLanes(
route_handler, left_side, backward_distance, forward_distance, bound_offset, objects);

Check warning on line 109 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L109 

Added line #L109 was not covered by tests

return utils::path_safety_checker::filterObjectsByVelocity(objects_in_lanes, velocity_thresh);
}

Check notice on line 112 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

CodeScene Delta Analysis / CodeScene Cloud Delta Analysis (main)

ℹ New issue: Excess Number of Function Arguments 

extractStaticObjectsInExpandedPullOverLanes has 7 arguments, threshold = 4. This function has too many arguments, indicating a lack of encapsulation. Avoid adding more arguments.

Check warning on line 112 in planning/behavior_path_goal_planner_module/src/util.cpp

View check run for this annotation

Codecov / codecov/patch

planning/behavior_path_goal_planner_module/src/util.cpp#L111-L112 

Added lines #L111 - L112 were not covered by tests

PredictedObjects filterObjectsByLateralDistance(
const Pose & ego_pose, const double vehicle_width, const PredictedObjects & objects,
const double distance_thresh, const bool filter_inside)
Expand Down
Loading