Skip to content

Commit

Permalink
fix(lane_change): filtering object ahead of terminal (autowarefoundat…
Browse files Browse the repository at this point in the history 
…ion#8093)

* employ lanelet based filtering before distance based filtering

Signed-off-by: Zulfaqar Azmi <[email protected]>

* use distance based to terminal check instead

Signed-off-by: Zulfaqar Azmi <[email protected]>

* remove RCLCPP INFO

Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>

* update flow chart

Signed-off-by: Zulfaqar Azmi <[email protected]>

---------

Signed-off-by: Zulfaqar Azmi <[email protected]>
Signed-off-by: Muhammad Zulfaqar Azmi <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4
zulfaqar-azmi-t4 committed Aug 1, 2024
1 parent 8306d04 commit 7dee9e7
Show file tree
Hide file tree
Showing 6 changed files with 153 additions and 129 deletions.
84 changes: 27 additions & 57 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -331,8 +331,7 @@ title NormalLaneChange::filterObjects Method Execution Flow
start
group "Filter Objects by Class" {
:Iterate through each object in objects list;
while (has not finished iterating through object list) is (TRUE)
while (has not finished iterating through predicted object list) is (TRUE)
if (current object type != param.object_types_to_check?) then (TRUE)
#LightPink:Remove current object;
else (FALSE)
Expand All @@ -341,17 +340,15 @@ endif
end while
end group
if (object list is empty?) then (TRUE)
if (predicted object list is empty?) then (TRUE)
:Return empty result;
stop
else (FALSE)
endif
group "Filter Oncoming Objects" #PowderBlue {
:Iterate through each object in target lane objects list;
while (has not finished iterating through object list?) is (TRUE)
:check object's yaw with reference to ego's yaw.;
if (yaw difference < 90 degree?) then (TRUE)
while (has not finished iterating through predicted object list?) is (TRUE)
if (object's yaw with reference to ego's yaw difference < 90 degree?) then (TRUE)
:Keep current object;
else (FALSE)
if (object is stopping?) then (TRUE)
Expand All @@ -363,53 +360,35 @@ endif
endwhile
end group
if (object list is empty?) then (TRUE)
:Return empty result;
stop
else (FALSE)
endif
group "Filter Objects Ahead Terminal" #Beige {
:Calculate lateral distance from ego to current lanes center;
:Iterate through each object in objects list;
while (has not finished iterating through object list) is (TRUE)
:Get current object's polygon;
:Initialize distance to terminal from object to max;
while (has not finished iterating through object polygon's vertices) is (TRUE)
:Calculate object's lateral distance to end of lane;
:Update minimum distance to terminal from object;
end while
if (Is object's distance to terminal exceeds minimum lane change length?) then (TRUE)
#LightPink:Remove current object;
else (FALSE)
endif
end while
end group
if (object list is empty?) then (TRUE)
if (predicted object list is empty?) then (TRUE)
:Return empty result;
stop
else (FALSE)
endif
group "Filter Objects By Lanelets" #LightGreen {
:Iterate through each object in objects list;
while (has not finished iterating through object list) is (TRUE)
:lateral distance diff = difference between object's lateral distance and ego's lateral distance to the current lanes' centerline.;
if (Object in target lane polygon, and lateral distance diff is more than half of ego's width?) then (TRUE)
:Add to target_lane_objects;
else (FALSE)
if (Object overlaps with backward target lanes?) then (TRUE)
while (has not finished iterating through predicted object list) is (TRUE)
:Calculate lateral distance diff;
if (Object in target lane polygon?) then (TRUE)
if (lateral distance diff > half of ego's width?) then (TRUE)
if (Object's physical position is before terminal point?) then (TRUE)
:Add to target_lane_objects;
else (FALSE)
if (Object in current lane polygon?) then (TRUE)
:Add to current_lane_objects;
else (FALSE)
:Add to other_lane_objects;
endif
endif
else (FALSE)
endif
else (FALSE)
endif
if (Object overlaps with backward target lanes?) then (TRUE)
:Add to target_lane_objects;
else (FALSE)
if (Object in current lane polygon?) then (TRUE)
:Add to current_lane_objects;
else (FALSE)
:Add to other_lane_objects;
endif
endif
end while
Expand All @@ -426,13 +405,10 @@ endif
group "Filter Target Lanes' objects" #LightCyan {
:Iterate through each object in target lane objects list;
while (has not finished iterating through object list) is (TRUE)
:check object's velocity;
while (has not finished iterating through target lanes' object list) is (TRUE)
if(velocity is within threshold?) then (TRUE)
:Keep current object;
else (FALSE)
:check whether object is ahead of ego;
if(object is ahead of ego?) then (TRUE)
:keep current object;
else (FALSE)
Expand All @@ -444,11 +420,8 @@ end group
group "Filter Current Lanes' objects" #LightYellow {
:Iterate through each object in current lane objects list;
while (has not finished iterating through object list) is (TRUE)
:check object's velocity;
while (has not finished iterating through current lanes' object list) is (TRUE)
if(velocity is within threshold?) then (TRUE)
:check whether object is ahead of ego;
if(object is ahead of ego?) then (TRUE)
:keep current object;
else (FALSE)
Expand All @@ -462,11 +435,8 @@ end group
group "Filter Other Lanes' objects" #Lavender {
:Iterate through each object in other lane objects list;
while (has not finished iterating through object list) is (TRUE)
:check object's velocity;
while (has not finished iterating through other lanes' object list) is (TRUE)
if(velocity is within threshold?) then (TRUE)
:check whether object is ahead of ego;
if(object is ahead of ego?) then (TRUE)
:keep current object;
else (FALSE)
Expand All @@ -478,7 +448,7 @@ while (has not finished iterating through object list) is (TRUE)
endwhile
end group
:Trasform the objects into extended predicted object and return them as lane_change_target_objects;
:Transform the objects into extended predicted object and return them as lane_change_target_objects;
stop
@enduml
Expand Down
6 changes: 1 addition & 5 deletions ...avior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -128,12 +128,8 @@ class NormalLaneChange : public LaneChangeBase

void filterOncomingObjects(PredictedObjects & objects) const;

void filterAheadTerminalObjects(
PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const;

void filterObjectsByLanelets(
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes,
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path,
std::vector<PredictedObject> & current_lane_objects,
std::vector<PredictedObject> & target_lane_objects,
std::vector<PredictedObject> & other_lane_objects) const;
Expand Down
1 change: 1 addition & 0 deletions ...ne_change_module/include/autoware/behavior_path_lane_change_module/utils/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -185,6 +185,7 @@ struct PhaseInfo

struct Lanes
{
bool current_lane_in_goal_section{false};
lanelet::ConstLanelets current;
lanelet::ConstLanelets target;
std::vector<lanelet::ConstLanelets> preceding_target;
Expand Down
12 changes: 12 additions & 0 deletions ...path_lane_change_module/include/autoware/behavior_path_lane_change_module/utils/utils.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -302,6 +302,18 @@ LanesPolygon create_lanes_polygon(const CommonDataPtr & common_data_ptr);
bool is_same_lane_with_prev_iteration(
const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes);

Pose to_pose(
const autoware::universe_utils::Point2d & point,
const geometry_msgs::msg::Quaternion & orientation);

bool is_ahead_of_ego(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
const PredictedObject & object);

bool is_before_terminal(
const CommonDataPtr & common_data_ptr, const PathWithLaneId & path,
const PredictedObject & object);
} // namespace autoware::behavior_path_planner::utils::lane_change

namespace autoware::behavior_path_planner::utils::lane_change::debug
Expand Down
99 changes: 32 additions & 67 deletions planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -83,8 +83,11 @@ void NormalLaneChange::update_lanes(const bool is_approved)
common_data_ptr_->lanes_ptr->current = current_lanes;
common_data_ptr_->lanes_ptr->target = target_lanes;

const auto & route_handler_ptr = common_data_ptr_->route_handler_ptr;
common_data_ptr_->lanes_ptr->current_lane_in_goal_section =
route_handler_ptr->isInGoalRouteSection(current_lanes.back());
common_data_ptr_->lanes_ptr->preceding_target = utils::getPrecedingLanelets(
*common_data_ptr_->route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(),
*route_handler_ptr, get_target_lanes(), common_data_ptr_->get_ego_pose(),
common_data_ptr_->lc_param_ptr->backward_lane_length);

*common_data_ptr_->lanes_polygon_ptr = create_lanes_polygon(common_data_ptr_);
Expand Down Expand Up @@ -983,7 +986,6 @@ ExtendedPredictedObjects NormalLaneChange::getTargetObjects(

LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const
{
const auto & current_pose = getEgoPose();
const auto & route_handler = getRouteHandler();
const auto & common_parameters = planner_data_->parameters;
auto objects = *planner_data_->dynamic_object;
Expand All @@ -1006,12 +1008,6 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const
return {};
}

filterAheadTerminalObjects(objects, current_lanes);

if (objects.objects.empty()) {
return {};
}

std::vector<PredictedObject> target_lane_objects;
std::vector<PredictedObject> current_lane_objects;
std::vector<PredictedObject> other_lane_objects;
Expand All @@ -1022,48 +1018,37 @@ LaneChangeLanesFilteredObjects NormalLaneChange::filterObjects() const
return {};
}

const auto path =
route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());

filterObjectsByLanelets(
objects, current_lanes, target_lanes, current_lane_objects, target_lane_objects,
other_lane_objects);
objects, path, current_lane_objects, target_lane_objects, other_lane_objects);

const auto is_within_vel_th = [](const auto & object) -> bool {
constexpr double min_vel_th = 1.0;
constexpr double max_vel_th = std::numeric_limits<double>::max();
return utils::path_safety_checker::filter::velocity_filter(object, min_vel_th, max_vel_th);
};

const auto path =
route_handler->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());

if (path.points.empty()) {
return {};
}

const auto is_ahead_of_ego = [&path, &current_pose](const auto & object) {
const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer();

double max_dist_ego_to_obj = std::numeric_limits<double>::lowest();
for (const auto & polygon_p : obj_polygon) {
const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
const auto dist_ego_to_obj = calcSignedArcLength(path.points, current_pose.position, obj_p);
max_dist_ego_to_obj = std::max(dist_ego_to_obj, max_dist_ego_to_obj);
}
return max_dist_ego_to_obj >= 0.0;
};

utils::path_safety_checker::filterObjects(
target_lane_objects, [&](const PredictedObject & object) {
return (is_within_vel_th(object) || is_ahead_of_ego(object));
const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object);
return is_within_vel_th(object) || ahead_of_ego;
});

utils::path_safety_checker::filterObjects(
other_lane_objects, [&](const PredictedObject & object) {
return is_within_vel_th(object) && is_ahead_of_ego(object);
});
if (lane_change_parameters_->check_objects_on_other_lanes) {
utils::path_safety_checker::filterObjects(
other_lane_objects, [&](const PredictedObject & object) {
const auto ahead_of_ego =
utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object);
return is_within_vel_th(object) && ahead_of_ego;
});
}

utils::path_safety_checker::filterObjects(
current_lane_objects, [&](const PredictedObject & object) {
return is_within_vel_th(object) && is_ahead_of_ego(object);
const auto ahead_of_ego = utils::lane_change::is_ahead_of_ego(common_data_ptr_, path, object);
return is_within_vel_th(object) && ahead_of_ego;
});

LaneChangeLanesFilteredObjects lane_change_target_objects;
Expand Down Expand Up @@ -1119,42 +1104,15 @@ void NormalLaneChange::filterOncomingObjects(PredictedObjects & objects) const
});
}

void NormalLaneChange::filterAheadTerminalObjects(
PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes) const
{
const auto & current_pose = getEgoPose();
const auto & route_handler = getRouteHandler();
const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength(
route_handler, current_lanes.back(), *lane_change_parameters_, direction_);

const auto dist_ego_to_current_lanes_center =
lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose);

// ignore object that are ahead of terminal lane change start
utils::path_safety_checker::filterObjects(objects, [&](const PredictedObject & object) {
const auto obj_polygon = autoware::universe_utils::toPolygon2d(object).outer();
// ignore object that are ahead of terminal lane change start
auto distance_to_terminal_from_object = std::numeric_limits<double>::max();
for (const auto & polygon_p : obj_polygon) {
const auto obj_p = autoware::universe_utils::createPoint(polygon_p.x(), polygon_p.y(), 0.0);
Pose polygon_pose;
polygon_pose.position = obj_p;
polygon_pose.orientation = object.kinematics.initial_pose_with_covariance.pose.orientation;
const auto dist = utils::getDistanceToEndOfLane(polygon_pose, current_lanes);
distance_to_terminal_from_object = std::min(dist_ego_to_current_lanes_center, dist);
}

return (minimum_lane_change_length > distance_to_terminal_from_object);
});
}

void NormalLaneChange::filterObjectsByLanelets(
const PredictedObjects & objects, const lanelet::ConstLanelets & current_lanes,
const lanelet::ConstLanelets & target_lanes, std::vector<PredictedObject> & current_lane_objects,
const PredictedObjects & objects, const PathWithLaneId & current_lanes_ref_path,
std::vector<PredictedObject> & current_lane_objects,
std::vector<PredictedObject> & target_lane_objects,
std::vector<PredictedObject> & other_lane_objects) const
{
const auto & current_pose = getEgoPose();
const auto & current_lanes = common_data_ptr_->lanes_ptr->current;
const auto & target_lanes = common_data_ptr_->lanes_ptr->target;
const auto & route_handler = getRouteHandler();
const auto & common_parameters = planner_data_->parameters;
const auto check_optional_polygon = [](const auto & object, const auto & polygon) {
Expand Down Expand Up @@ -1199,7 +1157,14 @@ void NormalLaneChange::filterObjectsByLanelets(
return std::abs(lateral) > (common_parameters.vehicle_width / 2);
});

if (check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far) {
const auto is_before_terminal = [&]() {
return utils::lane_change::is_before_terminal(
common_data_ptr_, current_lanes_ref_path, object);
};

if (
check_optional_polygon(object, lanes_polygon.expanded_target) && is_lateral_far &&
is_before_terminal()) {
target_lane_objects.push_back(object);
continue;
}
Expand Down
Loading

0 comments on commit 7dee9e7

Please sign in to comment.