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

perf(goal_planner): faster path sorting and selection #8457

Merged
merged 2 commits into from
Aug 21, 2024
Merged

perf(goal_planner): faster path sorting and selection #8457

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
4d3ddf1
perf(goal_planner): faster path sorting and selection
kosuke55 Aug 9, 2024
b7dbcb6
path_id_to_rough_margin_map
kosuke55 Aug 20, 2024
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
14 changes: 7 additions & 7 deletions ...ning/behavior_path_planner/autoware_behavior_path_goal_planner_module/README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -339,13 +339,13 @@ Then there is the concept of soft and hard margins. Although not currently param

#### Parameters for object recognition based collision check

| Name | Unit | Type | Description | Default value |
| :----------------------------------------------------------- | :--- | :------------- | :------------------------------------------------------------------------------------------------------- | :-------------- |
| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true |
| object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation | [2.0, 1.5, 1.0] |
| object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] |
| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 |
| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 |
| Name | Unit | Type | Description | Default value |
| :----------------------------------------------------------- | :--- | :------------- | :--------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :-------------------------------------------- |
| use_object_recognition | [-] | bool | flag whether to use object recognition for collision check | true |
| object_recognition_collision_check_soft_margins | [m] | vector[double] | soft margins for collision check when path generation. It is not strictly the distance between footprints, but the maximum distance when ego and objects are oriented. | [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] |
| object_recognition_collision_check_hard_margins | [m] | vector[double] | hard margins for collision check when path generation | [0.6] |
| object_recognition_collision_check_max_extra_stopping_margin | [m] | double | maximum value when adding longitudinal distance margin for collision check considering stopping distance | 1.0 |
| detection_bound_offset | [m] | double | expand pull over lane with this offset to make detection area for collision check of path generation | 15.0 |

## **safety check**

Expand Down
4 changes: 2 additions & 2 deletions ...or_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,8 +36,8 @@
# object recognition
object_recognition:
use_object_recognition: false
collision_check_soft_margins: [2.0, 1.5, 1.0]
collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker
collision_check_soft_margins: [5.0, 4.5, 4.0, 3.5, 3.0, 2.5, 2.0, 1.5, 1.0] # the maximum margin when ego and objects are oriented.
collision_check_hard_margins: [0.6] # this should be larger than `surround_check_distance` of surround_obstacle_checker.
object_recognition_collision_check_max_extra_stopping_margin: 1.0
th_moving_object_velocity: 1.0
detection_bound_offset: 15.0
Expand Down
3 changes: 0 additions & 3 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -619,9 +619,6 @@ class GoalPlannerModule : public SceneModuleInterface
BehaviorModuleOutput planPullOverAsOutput();
BehaviorModuleOutput planPullOverAsCandidate();
std::optional<std::pair<PullOverPath, GoalCandidate>> selectPullOverPath(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates, const double collision_check_margin) const;
std::vector<PullOverPath> sortPullOverPathCandidatesByGoalPriority(
const std::vector<PullOverPath> & pull_over_path_candidates,
const GoalCandidates & goal_candidates) const;

Expand Down
9 changes: 9 additions & 0 deletions ...vior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -16,6 +16,7 @@
#define AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_

#include "autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp"
#include "autoware/behavior_path_goal_planner_module/pull_over_planner_base.hpp"
#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp"

#include <autoware/lane_departure_checker/lane_departure_checker.hpp>
Expand All @@ -29,6 +30,7 @@

#include <lanelet2_core/Forward.h>

#include <map>
#include <memory>
#include <string>
#include <vector>
Expand Down Expand Up @@ -109,6 +111,13 @@ MarkerArray createLaneletPolygonMarkerArray(
MarkerArray createNumObjectsToAvoidTextsMarkerArray(
const GoalCandidates & goal_candidates, std::string && ns,
const std_msgs::msg::ColorRGBA & color);
std::string makePathPriorityDebugMessage(
const std::vector<size_t> & sorted_path_indices,
const std::vector<PullOverPath> & pull_over_path_candidates,
const std::map<size_t, size_t> & goal_id_to_index, const GoalCandidates & goal_candidates,
const std::map<size_t, double> & path_id_to_rough_margin_map,
const std::function<bool(const PullOverPath &)> & isSoftMargin,
const std::function<bool(const PullOverPath &)> & isHighCurvature);
} // namespace autoware::behavior_path_planner::goal_planner_utils

#endif // AUTOWARE__BEHAVIOR_PATH_GOAL_PLANNER_MODULE__UTIL_HPP_
Loading
Loading