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): change stopping logic (RT0-33761) #1581

Merged
merged 4 commits into from
Oct 11, 2024
Merged

fix(lane_change): change stopping logic (RT0-33761) #1581

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
0a7c621
RT0-33761 fix lane change stopping logic
zulfaqar-azmi-t4 Oct 8, 2024
d3daa6c
copied from awf main tested implementation
zulfaqar-azmi-t4 Oct 10, 2024
a0aa598
doxygen comment
zulfaqar-azmi-t4 Oct 11, 2024
61ac7de
Update planning/behavior_path_lane_change_module/src/utils/utils.cpp
zulfaqar-azmi-t4 Oct 11, 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
4 changes: 3 additions & 1 deletion planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -60,6 +60,8 @@ class NormalLaneChange : public LaneChangeBase

void insertStopPoint(const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) override;

void insert_stop_point_on_current_lanes(PathWithLaneId & path);

PathWithLaneId getReferencePath() const override;

std::optional<PathWithLaneId> extendPath() override;
Expand Down Expand Up @@ -178,7 +180,7 @@ class NormalLaneChange : public LaneChangeBase

std::pair<double, double> calcCurrentMinMaxAcceleration() const;

void setStopPose(const Pose & stop_pose);
void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path);

void updateStopTime();

Expand Down
49 changes: 49 additions & 0 deletions ...behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -289,5 +289,54 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol
double calcPhaseLength(
const double velocity, const double maximum_velocity, const double acceleration,
const double time);

/**
* @brief Calculates the minimum distance to a stationary object in the current lanes.
*
* This function determines the closest distance from the ego vehicle to a stationary object
* in the current lanes. It checks if the object is within the stopping criteria based on its
* velocity and calculates the distance while accounting for the object's size. Only objects
* positioned after the specified distance to the target lane's start are considered.
*
* @param filtered_objects A collection of lane change target objects, including those in the
* current lane.
* @param bpp_param Parameters for the behavior path planner, such as vehicle dimensions.
* @param lc_param Parameters for the lane change process, including the velocity threshold for
* stopping.
* @param dist_to_target_lane_start The distance from the ego vehicle to the start of the target
* lane.
* @param ego_pose The current pose of the ego vehicle.
* @param path The current path of the ego vehicle, containing path points and lane information.
* @return The minimum distance to a stationary object in the current lanes. If no valid object is
* found, returns the maximum possible double value.
*/
double get_min_dist_to_current_lanes_obj(
const LaneChangeTargetObjects & filtered_objects, const BehaviorPathPlannerParameters & bpp_param,
const LaneChangeParameters & lc_param, const double dist_to_target_lane_start, const Pose & pose,
const PathWithLaneId & path);

/**
* @brief Checks if there is any stationary object in the target lanes that would affect the lane
* change stop decision.
*
* This function determines whether there are any stationary objects in the target lanes that could
* impact the decision to insert a stop point for the ego vehicle. It checks each object's velocity,
* position relative to the ego vehicle, and overlap with the target lanes to identify if any object
* meets the criteria for being a blocking obstacle.
*
* @param target_lanes A collection of lanelets representing the target lanes for the lane change.
* @param filtered_objects A collection of lane change target objects, including those in the target
* lanes.
* @param lc_param Parameters for the lane change process, such as the stop velocity threshold.
* @param stop_arc_length The arc length at which the ego vehicle is expected to stop.
* @param ego_pose The current pose of the ego vehicle.
* @param path The current path of the ego vehicle, containing path points and lane information.
* @return true if there is a stationary object in the target lanes that meets the criteria for
* being a blocking object; otherwise, false.
*/
bool has_blocking_target_object(
const lanelet::ConstLanelets & target_lanes, const LaneChangeTargetObjects & filtered_objects,
const LaneChangeParameters & lc_param, const double stop_arc_length, const Pose & ego_pose,
const PathWithLaneId & path);
} // namespace behavior_path_planner::utils::lane_change
#endif // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__UTILS_HPP_
214 changes: 100 additions & 114 deletions planning/behavior_path_lane_change_module/src/scene.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -165,8 +165,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput()
output.path.points, output.path.points.front().point.pose.position, getEgoPosition());
const auto stop_dist =
-(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, output.path);
setStopPose(stop_point.point.pose);
set_stop_pose(stop_dist + current_dist, output.path);
} else {
insertStopPoint(status_.target_lanes, output.path);
}
Expand Down Expand Up @@ -204,7 +203,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output)
void NormalLaneChange::insertStopPoint(
const lanelet::ConstLanelets & lanelets, PathWithLaneId & path)
{
if (lanelets.empty()) {
if (lanelets.empty() || status_.current_lanes.empty()) {
return;
}

Expand All @@ -214,133 +213,119 @@ void NormalLaneChange::insertStopPoint(
return;
}

const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back());
const double lane_change_buffer =
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0);
const auto & current_lanes = status_.current_lanes;
const auto is_current_lane = lanelets.front().id() == current_lanes.front().id() &&
lanelets.back().id() == current_lanes.back().id();

// if input is not current lane, we can just insert the points at terminal.
if (!is_current_lane) {
const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back());
const auto min_dist_buffer = utils::lane_change::calcMinimumLaneChangeLength(
*lane_change_parameters_, shift_intervals, 0.0);
const auto backward_length_buffer =
lane_change_parameters_->backward_length_buffer_for_end_of_lane;
const auto arc_length_to_stop_pose =
motion_utils::calcArcLength(path.points) - backward_length_buffer + min_dist_buffer;
set_stop_pose(arc_length_to_stop_pose, path);
return;
}

const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) {
return utils::getSignedDistance(path.points.front().point.pose, target, lanelets);
};
insert_stop_point_on_current_lanes(path);
}

// If lanelets.back() is in goal route section, get distance to goal.
// Otherwise, get distance to end of lane.
double distance_to_terminal = 0.0;
if (route_handler->isInGoalRouteSection(lanelets.back())) {
const auto goal = route_handler->getGoalPose();
distance_to_terminal = getDistanceAlongLanelet(goal);
} else {
distance_to_terminal = utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets);
}
void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path)
{
const auto & path_front_pose = path.points.front().point.pose;
const auto & route_handler_ptr = getRouteHandler();
const auto & current_lanes = status_.current_lanes;
const auto current_path =
route_handler_ptr->getCenterLinePath(current_lanes, 0.0, std::numeric_limits<double>::max());
const auto & center_line = current_path.points;
const auto get_arc_length_along_lanelet = [&](const geometry_msgs::msg::Pose & target) {
return motion_utils::calcSignedArcLength(
center_line, path_front_pose.position, target.position);
};

const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane;
const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;

const auto is_valid_start_point = std::invoke([&]() -> bool {
auto lc_start_point = lanelet::utils::conversion::toLaneletPoint(
status_.lane_change_path.info.lane_changing_start.position);
const auto target_neighbor_preferred_lane_poly_2d =
utils::lane_change::getTargetNeighborLanesPolygon(
*route_handler, status_.current_lanes, type_);
return boost::geometry::covered_by(
lanelet::traits::to2D(lc_start_point), target_neighbor_preferred_lane_poly_2d);
const auto dist_to_terminal = std::invoke([&]() -> double {
const auto target_pose = route_handler_ptr->isInGoalRouteSection(current_lanes.back())
? route_handler_ptr->getGoalPose()
: center_line.back().point.pose;
return get_arc_length_along_lanelet(target_pose);
});

if (!is_valid_start_point) {
const auto stop_point = utils::insertStopPoint(stopping_distance, path);
setStopPose(stop_point.point.pose);
const auto shift_intervals =
route_handler_ptr->getLateralIntervalsToPreferredLane(current_lanes.back());
const auto min_dist_buffer =
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals, 0.0);
const auto backward_length_buffer =
lane_change_parameters_->backward_length_buffer_for_end_of_lane;
const auto dist_to_terminal_start = dist_to_terminal - min_dist_buffer - backward_length_buffer;

const auto filtered_objects = getTargetObjects(status_.current_lanes, status_.target_lanes);
if (filtered_objects.current_lane.empty()) {
set_stop_pose(dist_to_terminal_start, path);
return;
}

// calculate minimum distance from path front to the stationary object on the ego lane.
const auto distance_to_ego_lane_obj = [&]() -> double {
double distance_to_obj = distance_to_terminal;
const double distance_to_ego = getDistanceAlongLanelet(getEgoPose());

for (const auto & object : target_objects.current_lane) {
// check if stationary
const auto obj_v = std::abs(object.initial_twist.twist.linear.x);
if (obj_v > lane_change_parameters_->stop_velocity_threshold) {
continue;
}

// calculate distance from path front to the stationary object polygon on the ego lane.
const auto polygon =
tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer();
for (const auto & polygon_p : polygon) {
const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d());
const auto lateral_fp = motion_utils::calcLateralOffset(path.points, p_fp);
const auto dist_to_target_lane_start = std::invoke([&]() -> double {
const auto & front_lane = status_.target_lanes.front();
const auto & ll_front_pt = front_lane.centerline2d().front();
const auto front_pt = lanelet::utils::conversion::toGeomMsgPt(ll_front_pt);
const auto yaw = lanelet::utils::getLaneletAngle(front_lane, front_pt);
Pose target_front;
target_front.position = front_pt;
tf2::Quaternion tf_quat;
tf_quat.setRPY(0, 0, yaw);
target_front.orientation = tf2::toMsg(tf_quat);

// ignore if the point is around the ego path
if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) {
continue;
}
return get_arc_length_along_lanelet(target_front);
});

const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp);
const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj(
filtered_objects, getCommonParam(), *lane_change_parameters_, dist_to_target_lane_start,
getEgoPose(), path);

// ignore backward object
if (current_distance_to_obj < distance_to_ego) {
continue;
}
distance_to_obj = std::min(distance_to_obj, current_distance_to_obj);
}
}
return distance_to_obj;
}();

// Need to stop before blocking obstacle
if (distance_to_ego_lane_obj < distance_to_terminal) {
// consider rss distance when the LC need to avoid obstacles
const auto rss_dist = calcRssDistance(
0.0, lane_change_parameters_->minimum_lane_changing_velocity,
lane_change_parameters_->rss_params);
const double lane_change_buffer_for_blocking_object =
utils::lane_change::calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals);

const auto stopping_distance_for_obj =
distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object -
lane_change_parameters_->backward_length_buffer_for_blocking_object - rss_dist -
getCommonParam().base_link2front;

// If the target lane in the lane change section is blocked by a stationary obstacle, there
// is no reason for stopping with a lane change margin. Instead, stop right behind the
// obstacle.
// ----------------------------------------------------------
// [obj]>
// ----------------------------------------------------------
// [ego]> | <--- lane change margin ---> [obj]>
// ----------------------------------------------------------
const bool has_blocking_target_lane_obj = std::any_of(
target_objects.target_lane.begin(), target_objects.target_lane.end(), [&](const auto & o) {
const auto v = std::abs(o.initial_twist.twist.linear.x);
if (v > lane_change_parameters_->stop_velocity_threshold) {
return false;
}
// margin with leading vehicle
// consider rss distance when the LC need to avoid obstacles
const auto rss_dist = calcRssDistance(
0.0, lane_change_parameters_->minimum_lane_changing_velocity,
lane_change_parameters_->rss_params);

// target_objects includes objects out of target lanes, so filter them out
if (!boost::geometry::intersects(
tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(),
lanelet::utils::combineLaneletsShape(status_.target_lanes)
.polygon2d()
.basicPolygon())) {
return false;
}
const auto min_single_lc_length = utils::lane_change::calcMinimumLaneChangeLength(
*lane_change_parameters_, {shift_intervals.front()}, 0.0);
const auto stop_margin = min_single_lc_length +
lane_change_parameters_->backward_length_buffer_for_blocking_object +
rss_dist + getCommonParam().base_link2front;
const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin;

const double distance_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose);
return stopping_distance_for_obj < distance_to_target_lane_obj &&
distance_to_target_lane_obj < distance_to_ego_lane_obj;
});
if (stop_arc_length_behind_obj < dist_to_target_lane_start) {
set_stop_pose(dist_to_target_lane_start, path);
return;
}

if (!has_blocking_target_lane_obj) {
stopping_distance = stopping_distance_for_obj;
}
if (stop_arc_length_behind_obj > dist_to_terminal_start) {
set_stop_pose(dist_to_terminal_start, path);
return;
}

if (stopping_distance > 0.0) {
const auto stop_point = utils::insertStopPoint(stopping_distance, path);
setStopPose(stop_point.point.pose);
// If the target lane in the lane change section is blocked by a stationary obstacle, there
// is no reason for stopping with a lane change margin. Instead, stop right behind the
// obstacle.
// ----------------------------------------------------------
// [obj]>
// ----------------------------------------------------------
// [ego]> | <--- stop margin ---> [obj]>
// ----------------------------------------------------------
const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object(
status_.target_lanes, filtered_objects, *lane_change_parameters_, stop_arc_length_behind_obj,
getEgoPose(), path);

if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) {
set_stop_pose(dist_to_terminal_start, path);
return;
}

set_stop_pose(stop_arc_length_behind_obj, path);
}

PathWithLaneId NormalLaneChange::getReferencePath() const
Expand Down Expand Up @@ -1842,9 +1827,10 @@ bool NormalLaneChange::isVehicleStuck(const lanelet::ConstLanelets & current_lan
return isVehicleStuck(current_lanes, detection_distance);
}

void NormalLaneChange::setStopPose(const Pose & stop_pose)
void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path)
{
lane_change_stop_pose_ = stop_pose;
const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path);
lane_change_stop_pose_ = stop_point.point.pose;
}

void NormalLaneChange::updateStopTime()
Expand Down
Loading
Loading