Skip to content

Commit

Permalink
feat(goal_planner): extend pull over lanes inward to extract objects (#…
Browse files Browse the repository at this point in the history 
…8714)

* feat(goal_planner): extend pull over lanes inward to extract objects

Signed-off-by: kosuke55 <[email protected]>

* update from review

Signed-off-by: kosuke55 <[email protected]>

* use optionale

Signed-off-by: kosuke55 <[email protected]>

* rename lamda

Signed-off-by: kosuke55 <[email protected]>

* return nullopt

Signed-off-by: kosuke55 <[email protected]>

* Update planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp

Co-authored-by: Mamoru Sobue <[email protected]>

* pre-commit

Signed-off-by: kosuke55 <[email protected]>

---------

Signed-off-by: kosuke55 <[email protected]>
Co-authored-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@kosuke55 @soblin
kosuke55 and soblin authored Sep 3, 2024
1 parent 1557806 commit 799e4f9
Show file tree
Hide file tree
Showing 7 changed files with 204 additions and 50 deletions.
1 change: 1 addition & 0 deletions ...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 @@ -275,6 +275,7 @@ struct GoalPlannerDebugData
FreespacePlannerDebugData freespace_planner{};
std::vector<Polygon2d> ego_polygons_expanded{};
lanelet::ConstLanelet expanded_pull_over_lane_between_ego{};
Polygon2d objects_extraction_polygon{};
};

struct LastApprovalData
Expand Down
10 changes: 5 additions & 5 deletions ..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -31,13 +31,12 @@ class GoalSearcher : public GoalSearcherBase
public:
GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);

GoalCandidates search(
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) override;
GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) override;
void update(
GoalCandidates & goal_candidates,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const override;
const std::shared_ptr<const PlannerData> & planner_data,
const PredictedObjects & objects) const override;

// todo(kosuke55): Functions for this specific use should not be in the interface,
// so it is better to consider interface design when we implement other goal searchers.
Expand All @@ -47,7 +46,8 @@ class GoalSearcher : public GoalSearcherBase
bool isSafeGoalWithMarginScaleFactor(
const GoalCandidate & goal_candidate, const double margin_scale_factor,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const override;
const std::shared_ptr<const PlannerData> & planner_data,
const PredictedObjects & objects) const override;

private:
void countObjectsToAvoid(
Expand Down
10 changes: 5 additions & 5 deletions ..._planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -53,13 +53,12 @@ class GoalSearcherBase
const Pose & getReferenceGoal() const { return reference_goal_pose_; }

MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
virtual GoalCandidates search(
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) = 0;
virtual GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) = 0;
virtual void update(
[[maybe_unused]] GoalCandidates & goal_candidates,
[[maybe_unused]] const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
[[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data) const
[[maybe_unused]] const std::shared_ptr<const PlannerData> & planner_data,
[[maybe_unused]] const PredictedObjects & objects) const
{
return;
}
Expand All @@ -69,7 +68,8 @@ class GoalSearcherBase
virtual bool isSafeGoalWithMarginScaleFactor(
const GoalCandidate & goal_candidate, const double margin_scale_factor,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const = 0;
const std::shared_ptr<const PlannerData> & planner_data,
const PredictedObjects & objects) const = 0;

protected:
GoalPlannerParameters parameters_{};
Expand Down
13 changes: 13 additions & 0 deletions ...vior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/util.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -64,6 +64,19 @@ lanelet::ConstLanelets generateBetweenEgoAndExpandedPullOverLanes(
const geometry_msgs::msg::Pose ego_pose,
const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double outer_road_offset,
const double inner_road_offset);

/*
* @brief generate polygon to extract objects
* @param pull_over_lanes pull over lanes
* @param left_side left side or right side
* @param outer_offset outer offset from pull over lane boundary
* @param inner_offset inner offset from pull over lane boundary
* @return polygon to extract objects
*/
std::optional<Polygon2d> generateObjectExtractionPolygon(
const lanelet::ConstLanelets & pull_over_lanes, const bool left_side, const double outer_offset,
const double inner_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);
Expand Down
65 changes: 51 additions & 14 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 @@ -459,18 +459,33 @@ void GoalPlannerModule::updateData()
{
universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);

// extract static and dynamic objects in expanded pull over lanes
// extract static and dynamic objects in extraction polygon for path coliision check
{
const auto & p = parameters_;
const auto & rh = *(planner_data_->route_handler);
const auto objects = *(planner_data_->dynamic_object);
const auto static_target_objects =
goal_planner_utils::extractStaticObjectsInExpandedPullOverLanes(
rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
p->detection_bound_offset, objects, p->th_moving_object_velocity);
const auto dynamic_target_objects = goal_planner_utils::extractObjectsInExpandedPullOverLanes(
rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length,
p->detection_bound_offset, objects);
const double vehicle_width = planner_data_->parameters.vehicle_width;
const auto pull_over_lanes = goal_planner_utils::getPullOverLanes(
rh, left_side_parking_, p->backward_goal_search_length, p->forward_goal_search_length);
const auto objects_extraction_polygon = goal_planner_utils::generateObjectExtractionPolygon(
pull_over_lanes, left_side_parking_, p->detection_bound_offset,
p->margin_from_boundary + p->max_lateral_offset + vehicle_width);
if (objects_extraction_polygon.has_value()) {
debug_data_.objects_extraction_polygon = objects_extraction_polygon.value();
}
PredictedObjects dynamic_target_objects{};
for (const auto & object : objects.objects) {
const auto object_polygon = universe_utils::toPolygon2d(object);
if (
objects_extraction_polygon.has_value() &&
boost::geometry::intersects(object_polygon, objects_extraction_polygon.value())) {
dynamic_target_objects.objects.push_back(object);
}
}
const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity(
dynamic_target_objects, p->th_moving_object_velocity);

// these objects are used for path collision check not for safety check
thread_safe_data_.set_static_target_objects(static_target_objects);
thread_safe_data_.set_dynamic_target_objects(dynamic_target_objects);
}
Expand Down Expand Up @@ -739,7 +754,9 @@ bool GoalPlannerModule::planFreespacePath(
{
GoalCandidates goal_candidates{};
goal_candidates = thread_safe_data_.get_goal_candidates();
goal_searcher->update(goal_candidates, occupancy_grid_map, planner_data);
goal_searcher->update(
goal_candidates, occupancy_grid_map, planner_data,
thread_safe_data_.get_static_target_objects());
thread_safe_data_.set_goal_candidates(goal_candidates);
debug_data_.freespace_planner.num_goal_candidates = goal_candidates.size();
debug_data_.freespace_planner.is_planning = true;
Expand Down Expand Up @@ -824,7 +841,7 @@ GoalCandidates GoalPlannerModule::generateGoalCandidates() const
// calculate goal candidates
const auto & route_handler = planner_data_->route_handler;
if (utils::isAllowedGoalModification(route_handler)) {
return goal_searcher_->search(occupancy_grid_map_, planner_data_);
return goal_searcher_->search(planner_data_);
}

// NOTE:
Expand Down Expand Up @@ -1282,9 +1299,9 @@ DecidingPathStatusWithStamp GoalPlannerModule::checkDecidingPathStatus(
// check goal pose collision
const auto modified_goal_opt = thread_safe_data_.get_modified_goal_pose();
if (
modified_goal_opt &&
!goal_searcher->isSafeGoalWithMarginScaleFactor(
modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map, planner_data)) {
modified_goal_opt && !goal_searcher->isSafeGoalWithMarginScaleFactor(
modified_goal_opt.value(), hysteresis_factor, occupancy_grid_map,
planner_data, thread_safe_data_.get_static_target_objects())) {
RCLCPP_DEBUG(getLogger(), "[DecidingPathStatus]: DECIDING->NOT_DECIDED. goal is not safe");
return {DecidingPathStatus::NOT_DECIDED, clock_->now()};
}
Expand Down Expand Up @@ -1443,7 +1460,9 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput()

// update goal candidates
auto goal_candidates = thread_safe_data_.get_goal_candidates();
goal_searcher_->update(goal_candidates, occupancy_grid_map_, planner_data_);
goal_searcher_->update(
goal_candidates, occupancy_grid_map_, planner_data_,
thread_safe_data_.get_static_target_objects());

// Select a path that is as safe as possible and has a high priority.
const auto pull_over_path_candidates = thread_safe_data_.get_pull_over_path_candidates();
Expand Down Expand Up @@ -2518,6 +2537,24 @@ void GoalPlannerModule::setDebugData()
// Visualize goal candidates
const auto goal_candidates = thread_safe_data_.get_goal_candidates();
add(goal_planner_utils::createGoalCandidatesMarkerArray(goal_candidates, color));

// Visualize objects extraction polygon
auto marker = autoware::universe_utils::createDefaultMarker(
"map", rclcpp::Clock{RCL_ROS_TIME}.now(), "objects_extraction_polygon", 0, Marker::LINE_LIST,
autoware::universe_utils::createMarkerScale(0.1, 0.0, 0.0),
autoware::universe_utils::createMarkerColor(0.0, 1.0, 1.0, 0.999));
const double ego_z = planner_data_->self_odometry->pose.pose.position.z;
for (size_t i = 0; i < debug_data_.objects_extraction_polygon.outer().size(); ++i) {
const auto & current_point = debug_data_.objects_extraction_polygon.outer().at(i);
const auto & next_point = debug_data_.objects_extraction_polygon.outer().at(
(i + 1) % debug_data_.objects_extraction_polygon.outer().size());
marker.points.push_back(
autoware::universe_utils::createPoint(current_point.x(), current_point.y(), ego_z));
marker.points.push_back(
autoware::universe_utils::createPoint(next_point.x(), next_point.y(), ego_z));
}

debug_marker_.markers.push_back(marker);
}

// Visualize previous module output
Expand Down
34 changes: 8 additions & 26 deletions ...ng/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_searcher.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,9 +97,7 @@ GoalSearcher::GoalSearcher(
{
}

GoalCandidates GoalSearcher::search(
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data)
GoalCandidates GoalSearcher::search(const std::shared_ptr<const PlannerData> & planner_data)
{
GoalCandidates goal_candidates{};

Expand Down Expand Up @@ -193,8 +191,6 @@ GoalCandidates GoalSearcher::search(
}
createAreaPolygons(original_search_poses, planner_data);

update(goal_candidates, occupancy_grid_map, planner_data);

return goal_candidates;
}

Expand Down Expand Up @@ -268,16 +264,10 @@ void GoalSearcher::countObjectsToAvoid(
void GoalSearcher::update(
GoalCandidates & goal_candidates,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const
const std::shared_ptr<const PlannerData> & planner_data, const PredictedObjects & objects) const
{
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);

if (parameters_.prioritize_goals_before_objects) {
countObjectsToAvoid(goal_candidates, pull_over_lane_stop_objects, planner_data);
countObjectsToAvoid(goal_candidates, objects, planner_data);
}

if (parameters_.goal_priority == "minimum_weighted_distance") {
Expand All @@ -297,15 +287,15 @@ void GoalSearcher::update(
const Pose goal_pose = goal_candidate.goal_pose;

// check collision with footprint
if (checkCollision(goal_pose, pull_over_lane_stop_objects, occupancy_grid_map)) {
if (checkCollision(goal_pose, objects, occupancy_grid_map)) {
goal_candidate.is_safe = false;
continue;
}

// check longitudinal margin with pull over lane objects
constexpr bool filter_inside = true;
const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance(
goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects,
goal_pose, planner_data->parameters.vehicle_width, objects,
parameters_.object_recognition_collision_check_hard_margins.back(), filter_inside);
if (checkCollisionWithLongitudinalDistance(
goal_pose, target_objects, occupancy_grid_map, planner_data)) {
Expand All @@ -323,33 +313,25 @@ void GoalSearcher::update(
bool GoalSearcher::isSafeGoalWithMarginScaleFactor(
const GoalCandidate & goal_candidate, const double margin_scale_factor,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const std::shared_ptr<const PlannerData> & planner_data) const
const std::shared_ptr<const PlannerData> & planner_data, const PredictedObjects & objects) const
{
if (!parameters_.use_object_recognition) {
return true;
}

const Pose goal_pose = goal_candidate.goal_pose;

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);

const double margin =
parameters_.object_recognition_collision_check_hard_margins.back() * margin_scale_factor;

if (utils::checkCollisionBetweenFootprintAndObjects(
vehicle_footprint_, goal_pose, pull_over_lane_stop_objects, margin)) {
vehicle_footprint_, goal_pose, objects, margin)) {
return false;
}

// check longitudinal margin with pull over lane objects
constexpr bool filter_inside = true;
const auto target_objects = goal_planner_utils::filterObjectsByLateralDistance(
goal_pose, planner_data->parameters.vehicle_width, pull_over_lane_stop_objects, margin,
filter_inside);
goal_pose, planner_data->parameters.vehicle_width, objects, margin, filter_inside);
if (checkCollisionWithLongitudinalDistance(
goal_pose, target_objects, occupancy_grid_map, planner_data)) {
return false;
Expand Down
Loading

0 comments on commit 799e4f9

Please sign in to comment.