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

fix(lane_change): filter objects for skip parking objects #992

Merged
merged 2 commits into from
Oct 31, 2023
Merged

fix(lane_change): filter objects for skip parking objects #992

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
776c118
fix(lane_change): filter objects for skip parking objects
rej55 Oct 26, 2023
5ce6cfc
Update planning/behavior_path_planner/src/scene_module/lane_change/no…
rej55 Oct 26, 2023
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
3 changes: 3 additions & 0 deletions ...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -155,6 +155,9 @@ class NormalLaneChange : public LaneChangeBase
const lanelet::ConstLanelets & target_lanes,
const lanelet::ConstLanelets & target_backward_lanes) const;

std::vector<ExtendedPredictedObject> filterObjectsInTargetLane(
const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const;

//! @brief Check if the ego vehicle is in stuck by a stationary obstacle.
//! @param obstacle_check_distance Distance to check ahead for any objects that might be
//! obstructing ego path. It makes sense to use values like the maximum lane change distance.
Expand Down
28 changes: 24 additions & 4 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -914,6 +914,24 @@ LaneChangeTargetObjectIndices NormalLaneChange::filterObject(
return filtered_obj_indices;
}

std::vector<ExtendedPredictedObject> NormalLaneChange::filterObjectsInTargetLane(
const LaneChangeTargetObjects & objects, const lanelet::ConstLanelets & target_lanes) const
{
const auto target_polygon =
utils::lane_change::createPolygon(target_lanes, 0.0, std::numeric_limits<double>::max());
std::vector<ExtendedPredictedObject> filtered_objects{};
if (target_polygon) {
for (auto & obj : objects.target_lane) {
const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj.initial_pose.pose, obj.shape);
if (boost::geometry::intersects(target_polygon.value(), obj_polygon)) {
filtered_objects.push_back(obj);
}
}
}

return filtered_objects;
}

PathWithLaneId NormalLaneChange::getTargetSegment(
const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose,
const double target_lane_length, const double lane_changing_length,
Expand Down Expand Up @@ -1293,11 +1311,13 @@ bool NormalLaneChange::getLaneChangePaths(
}

candidate_paths->push_back(*candidate_path);

std::vector<ExtendedPredictedObject> filtered_objects =
filterObjectsInTargetLane(target_objects, target_lanes);
if (
!is_stuck &&
utils::lane_change::passParkedObject(
route_handler, *candidate_path, target_objects.target_lane, lane_change_buffer,
is_goal_in_route, *lane_change_parameters_, object_debug_)) {
!is_stuck && utils::lane_change::passParkedObject(
route_handler, *candidate_path, filtered_objects, lane_change_buffer,
is_goal_in_route, *lane_change_parameters_, object_debug_)) {
debug_print(
"Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip "
"lane change.");
Expand Down