Skip to content

Commit

Permalink
fix(lane_change): change stopping logic (RT0-33761) (#1581)
Browse files Browse the repository at this point in the history 
* RT0-33761 fix lane change stopping logic

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

* copied from awf main tested implementation

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

* doxygen comment

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

* Update planning/behavior_path_lane_change_module/src/utils/utils.cpp

Co-authored-by: mkquda <[email protected]>

---------

Signed-off-by: Zulfaqar Azmi <[email protected]>
Co-authored-by: mkquda <[email protected]>
  • Loading branch information
@zulfaqar-azmi-t4 @mkquda
zulfaqar-azmi-t4 and mkquda authored Oct 11, 2024
1 parent b25fd6b commit a7e68a1
Show file tree
Hide file tree
Showing 4 changed files with 238 additions and 115 deletions.
4 changes: 3 additions & 1 deletion planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp
View file
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
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
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

0 comments on commit a7e68a1

Please sign in to comment.