From 0a7c6214acf94378dfd5b71a8cb69d3e600b04dc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Tue, 8 Oct 2024 12:57:01 +0900 Subject: [PATCH 1/4] RT0-33761 fix lane change stopping logic Signed-off-by: Zulfaqar Azmi --- .../src/scene.cpp | 195 ++++++++++-------- 1 file changed, 111 insertions(+), 84 deletions(-) diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 2fe72c8d964d8..c9e8cbafa856d 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -204,7 +204,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() || status_.target_lanes.empty()) { return; } @@ -215,8 +215,13 @@ void NormalLaneChange::insertStopPoint( } 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); + + if (shift_intervals.empty()) { + return; + } + + const auto min_single_lc_length = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, {shift_intervals.front()}, 0.0); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -224,40 +229,51 @@ void NormalLaneChange::insertStopPoint( // 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); - } + const auto dist_to_terminal = std::invoke([&]() -> double { + if (route_handler->isInGoalRouteSection(lanelets.back())) { + const auto goal = route_handler->getGoalPose(); + return getDistanceAlongLanelet(goal); + } + return utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); + }); - 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_start = std::invoke([&]() { + const auto lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( + *lane_change_parameters_, shift_intervals, 0.0); + const auto stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; + return dist_to_terminal - stop_point_buffer - lane_change_buffer; }); - if (!is_valid_start_point) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); + const auto insert_stop_at_arc_length = [&](const auto & arc_length) { + const auto stop_point = utils::insertStopPoint(arc_length, path); setStopPose(stop_point.point.pose); + }; + const auto target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); + if (target_objects.current_lane.empty()) { + insert_stop_at_arc_length(dist_to_terminal_start); 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()); + 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); + + return getDistanceAlongLanelet(target_front); + }); + + const auto dist_to_ego = getDistanceAlongLanelet(getEgoPose()); + // calculate minimum distance from path front to the stationary object on the ego lane. + const auto arc_length_to_current_obj = std::invoke([&]() -> double { + auto min_dist_to_obj = std::numeric_limits::max(); for (const auto & object : target_objects.current_lane) { // check if stationary const auto obj_v = std::abs(object.initial_twist.twist.linear.x); @@ -265,6 +281,14 @@ void NormalLaneChange::insertStopPoint( continue; } + // provide "estimation" based on size of object + const auto dist_to_obj = + getDistanceAlongLanelet(object.initial_pose.pose) - (object.shape.dimensions.x / 2); + + if (dist_to_obj < dist_to_target_lane_start || dist_to_obj < dist_to_ego) { + 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(); @@ -277,70 +301,73 @@ void NormalLaneChange::insertStopPoint( continue; } - const double current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); - - // ignore backward object - if (current_distance_to_obj < distance_to_ego) { - continue; - } - distance_to_obj = std::min(distance_to_obj, current_distance_to_obj); + const auto current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); + min_dist_to_obj = std::min(min_dist_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; - } + return min_dist_to_obj; + }); - // 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; - } + // 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); - 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; - }); + 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; - if (!has_blocking_target_lane_obj) { - stopping_distance = stopping_distance_for_obj; - } + if (stop_arc_length_behind_obj < dist_to_target_lane_start) { + insert_stop_at_arc_length(dist_to_target_lane_start); + return; } - if (stopping_distance > 0.0) { - const auto stop_point = utils::insertStopPoint(stopping_distance, path); - setStopPose(stop_point.point.pose); + if (stop_arc_length_behind_obj > dist_to_terminal_start) { + insert_stop_at_arc_length(dist_to_terminal_start); + return; + } + + const auto target_lanes_poly = + lanelet::utils::combineLaneletsShape(status_.target_lanes).polygon2d().basicPolygon(); + + // 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 = std::any_of( + target_objects.target_lane.begin(), target_objects.target_lane.end(), + [&](const ExtendedPredictedObject & o) { + const auto v = std::abs(o.initial_twist.twist.linear.x); + if (v > lane_change_parameters_->stop_velocity_threshold) { + return false; + } + + // target_objects includes objects out of target lanes, so filter them out + if (boost::geometry::disjoint( + tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), + target_lanes_poly)) { + return false; + } + + const auto arc_length_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); + const auto width_margin = o.shape.dimensions.x / 2; + const auto upper_bound = arc_length_to_target_lane_obj - width_margin; + const auto lower_bound = arc_length_to_target_lane_obj + width_margin; + return lower_bound > dist_to_ego && upper_bound < stop_arc_length_behind_obj; + }); + + if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { + insert_stop_at_arc_length(dist_to_terminal_start); + return; } + + insert_stop_at_arc_length(stop_arc_length_behind_obj); } PathWithLaneId NormalLaneChange::getReferencePath() const From d3daa6c6492b5d6bc1c1e72511ce68ecee739e57 Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Thu, 10 Oct 2024 23:54:32 +0900 Subject: [PATCH 2/4] copied from awf main tested implementation Signed-off-by: Zulfaqar Azmi --- .../scene.hpp | 4 +- .../utils/utils.hpp | 10 ++ .../src/scene.cpp | 158 +++++++----------- .../src/utils/utils.cpp | 85 ++++++++++ 4 files changed, 157 insertions(+), 100 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp index 77efb658e28c3..04f21f5249240 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/scene.hpp @@ -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 extendPath() override; @@ -178,7 +180,7 @@ class NormalLaneChange : public LaneChangeBase std::pair calcCurrentMinMaxAcceleration() const; - void setStopPose(const Pose & stop_pose); + void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); void updateStopTime(); diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index a84acf4decd8d..1dd33d13cfc7e 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -289,5 +289,15 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol double calcPhaseLength( const double velocity, const double maximum_velocity, const double acceleration, const double time); + +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); + +bool has_blocking_target_object_for_stopping( + 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_ diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index c9e8cbafa856d..1cf66c0b3354a 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -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); } @@ -204,7 +203,7 @@ void NormalLaneChange::extendOutputDrivableArea(BehaviorModuleOutput & output) void NormalLaneChange::insertStopPoint( const lanelet::ConstLanelets & lanelets, PathWithLaneId & path) { - if (lanelets.empty() || status_.current_lanes.empty() || status_.target_lanes.empty()) { + if (lanelets.empty() || status_.current_lanes.empty()) { return; } @@ -214,44 +213,57 @@ void NormalLaneChange::insertStopPoint( return; } - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); + 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 (shift_intervals.empty()) { + // 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 min_single_lc_length = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, {shift_intervals.front()}, 0.0); + insert_stop_point_on_current_lanes(path); +} - const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { - return utils::getSignedDistance(path.points.front().point.pose, target, 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::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); }; - // If lanelets.back() is in goal route section, get distance to goal. - // Otherwise, get distance to end of lane. const auto dist_to_terminal = std::invoke([&]() -> double { - if (route_handler->isInGoalRouteSection(lanelets.back())) { - const auto goal = route_handler->getGoalPose(); - return getDistanceAlongLanelet(goal); - } - return utils::getDistanceToEndOfLane(path.points.front().point.pose, lanelets); + 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); }); - const auto dist_to_terminal_start = std::invoke([&]() { - const auto lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, shift_intervals, 0.0); - const auto stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; - return dist_to_terminal - stop_point_buffer - lane_change_buffer; - }); - - const auto insert_stop_at_arc_length = [&](const auto & arc_length) { - const auto stop_point = utils::insertStopPoint(arc_length, 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 target_objects = getTargetObjects(status_.current_lanes, status_.target_lanes); - if (target_objects.current_lane.empty()) { - insert_stop_at_arc_length(dist_to_terminal_start); + 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; } @@ -266,47 +278,12 @@ void NormalLaneChange::insertStopPoint( tf_quat.setRPY(0, 0, yaw); target_front.orientation = tf2::toMsg(tf_quat); - return getDistanceAlongLanelet(target_front); + return get_arc_length_along_lanelet(target_front); }); - const auto dist_to_ego = getDistanceAlongLanelet(getEgoPose()); - - // calculate minimum distance from path front to the stationary object on the ego lane. - const auto arc_length_to_current_obj = std::invoke([&]() -> double { - auto min_dist_to_obj = std::numeric_limits::max(); - 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; - } - - // provide "estimation" based on size of object - const auto dist_to_obj = - getDistanceAlongLanelet(object.initial_pose.pose) - (object.shape.dimensions.x / 2); - - if (dist_to_obj < dist_to_target_lane_start || dist_to_obj < dist_to_ego) { - 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); - - // ignore if the point is around the ego path - if (std::abs(lateral_fp) > planner_data_->parameters.vehicle_width) { - continue; - } - - const auto current_distance_to_obj = calcSignedArcLength(path.points, 0, p_fp); - min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); - } - } - return min_dist_to_obj; - }); + 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); // margin with leading vehicle // consider rss distance when the LC need to avoid obstacles @@ -314,24 +291,23 @@ void NormalLaneChange::insertStopPoint( 0.0, lane_change_parameters_->minimum_lane_changing_velocity, lane_change_parameters_->rss_params); + 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; if (stop_arc_length_behind_obj < dist_to_target_lane_start) { - insert_stop_at_arc_length(dist_to_target_lane_start); + set_stop_pose(dist_to_target_lane_start, path); return; } if (stop_arc_length_behind_obj > dist_to_terminal_start) { - insert_stop_at_arc_length(dist_to_terminal_start); + set_stop_pose(dist_to_terminal_start, path); return; } - const auto target_lanes_poly = - lanelet::utils::combineLaneletsShape(status_.target_lanes).polygon2d().basicPolygon(); - // 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. @@ -340,34 +316,17 @@ void NormalLaneChange::insertStopPoint( // ---------------------------------------------------------- // [ego]> | <--- stop margin ---> [obj]> // ---------------------------------------------------------- - const auto has_blocking_target_lane_obj = std::any_of( - target_objects.target_lane.begin(), target_objects.target_lane.end(), - [&](const ExtendedPredictedObject & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); - if (v > lane_change_parameters_->stop_velocity_threshold) { - return false; - } - - // target_objects includes objects out of target lanes, so filter them out - if (boost::geometry::disjoint( - tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape).outer(), - target_lanes_poly)) { - return false; - } - - const auto arc_length_to_target_lane_obj = getDistanceAlongLanelet(o.initial_pose.pose); - const auto width_margin = o.shape.dimensions.x / 2; - const auto upper_bound = arc_length_to_target_lane_obj - width_margin; - const auto lower_bound = arc_length_to_target_lane_obj + width_margin; - return lower_bound > dist_to_ego && upper_bound < stop_arc_length_behind_obj; - }); + const auto has_blocking_target_lane_obj = + utils::lane_change::has_blocking_target_object_for_stopping( + 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) { - insert_stop_at_arc_length(dist_to_terminal_start); + set_stop_pose(dist_to_terminal_start, path); return; } - insert_stop_at_arc_length(stop_arc_length_behind_obj); + set_stop_pose(stop_arc_length_behind_obj, path); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -1869,9 +1828,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() diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 60d7c699dc6bb..965544e4bcdfe 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1219,4 +1219,89 @@ double calcPhaseLength( const auto length_with_max_velocity = maximum_velocity * duration; return std::min(length_with_acceleration, length_with_max_velocity); } + +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 & ego_pose, const PathWithLaneId & path) +{ + const auto & path_points = path.points; + + auto min_dist_to_obj = std::numeric_limits::max(); + for (const auto & object : filtered_objects.current_lane) { + // check if stationary + const auto obj_v = std::abs(object.initial_twist.twist.linear.x); + if (obj_v > lc_param.stop_velocity_threshold) { + continue; + } + + // provide "estimation" based on size of object + const auto dist_to_obj = + motion_utils::calcSignedArcLength( + path_points, path_points.front().point.pose.position, object.initial_pose.pose.position) - + (object.shape.dimensions.x / 2); + + const auto dist_to_ego = motion_utils::calcSignedArcLength( + path_points, ego_pose.position, object.initial_pose.pose.position) - + (object.shape.dimensions.x / 2); + + if (dist_to_obj < dist_to_target_lane_start || dist_to_obj < dist_to_ego) { + continue; + } + + // calculate distance from path front to the stationary object polygon on the ego lane. + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape).outer(); + for (const auto & polygon_p : obj_poly) { + const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); + const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); + + // ignore if the point is around the ego path + if (std::abs(lateral_fp) > (bpp_param.vehicle_width / 2)) { + continue; + } + + const auto current_distance_to_obj = motion_utils::calcSignedArcLength(path_points, 0, p_fp); + min_dist_to_obj = std::min(min_dist_to_obj, current_distance_to_obj); + } + } + return min_dist_to_obj; +} + +bool has_blocking_target_object_for_stopping( + 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) +{ + const auto target_lane_poly = + lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon(); + return std::any_of( + filtered_objects.target_lane.begin(), filtered_objects.target_lane.end(), + [&](const ExtendedPredictedObject & o) { + const auto v = std::abs(o.initial_twist.twist.linear.x); + if (v > lc_param.stop_velocity_threshold) { + return false; + } + + const auto arc_length_to_ego = + motion_utils::calcSignedArcLength( + path.points, ego_pose.position, o.initial_pose.pose.position) - + (o.shape.dimensions.x / 2); + + if (arc_length_to_ego < 0.0) { + return false; + } + + const auto obj_poly = tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape); + // filtered_objects includes objects out of target lanes, so filter them out + if (boost::geometry::disjoint(obj_poly, target_lane_poly)) { + return false; + } + + const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( + path.points, path.points.front().point.pose.position, o.initial_pose.pose.position); + const auto width_margin = o.shape.dimensions.x / 2; + return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; + }); +} } // namespace behavior_path_planner::utils::lane_change From a0aa598868d9bdc4670fc08738fb3a307ae4c5fc Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Fri, 11 Oct 2024 11:51:11 +0900 Subject: [PATCH 3/4] doxygen comment Signed-off-by: Zulfaqar Azmi --- .../utils/utils.hpp | 41 ++++++++++++++++++- .../src/scene.cpp | 7 ++-- .../src/utils/utils.cpp | 17 ++++---- 3 files changed, 52 insertions(+), 13 deletions(-) diff --git a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp index 1dd33d13cfc7e..48b940093c2cd 100644 --- a/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/behavior_path_lane_change_module/include/behavior_path_lane_change_module/utils/utils.hpp @@ -290,12 +290,51 @@ 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); -bool has_blocking_target_object_for_stopping( +/** + * @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); diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp index 1cf66c0b3354a..284ea9409e924 100644 --- a/planning/behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_lane_change_module/src/scene.cpp @@ -316,10 +316,9 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) // ---------------------------------------------------------- // [ego]> | <--- stop margin ---> [obj]> // ---------------------------------------------------------- - const auto has_blocking_target_lane_obj = - utils::lane_change::has_blocking_target_object_for_stopping( - status_.target_lanes, filtered_objects, *lane_change_parameters_, stop_arc_length_behind_obj, - getEgoPose(), path); + 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); diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 965544e4bcdfe..6ad5404fedbae 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1268,7 +1268,7 @@ double get_min_dist_to_current_lanes_obj( return min_dist_to_obj; } -bool has_blocking_target_object_for_stopping( +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) @@ -1277,30 +1277,31 @@ bool has_blocking_target_object_for_stopping( lanelet::utils::combineLaneletsShape(target_lanes).polygon2d().basicPolygon(); return std::any_of( filtered_objects.target_lane.begin(), filtered_objects.target_lane.end(), - [&](const ExtendedPredictedObject & o) { - const auto v = std::abs(o.initial_twist.twist.linear.x); + [&](const auto & object) { + const auto v = std::abs(object.initial_twist.twist.linear.x); if (v > lc_param.stop_velocity_threshold) { return false; } const auto arc_length_to_ego = motion_utils::calcSignedArcLength( - path.points, ego_pose.position, o.initial_pose.pose.position) - - (o.shape.dimensions.x / 2); + path.points, ego_pose.position, object.initial_pose.pose.position) - + (object.shape.dimensions.x / 2); if (arc_length_to_ego < 0.0) { return false; } - const auto obj_poly = tier4_autoware_utils::toPolygon2d(o.initial_pose.pose, o.shape); + const auto obj_poly = + tier4_autoware_utils::toPolygon2d(object.initial_pose.pose, object.shape); // filtered_objects includes objects out of target lanes, so filter them out if (boost::geometry::disjoint(obj_poly, target_lane_poly)) { return false; } const auto arc_length_to_target_lane_obj = motion_utils::calcSignedArcLength( - path.points, path.points.front().point.pose.position, o.initial_pose.pose.position); - const auto width_margin = o.shape.dimensions.x / 2; + path.points, path.points.front().point.pose.position, object.initial_pose.pose.position); + const auto width_margin = object.shape.dimensions.x / 2; return (arc_length_to_target_lane_obj - width_margin) >= stop_arc_length; }); } From 61ac7de12b8282f2f59afec1ef80683e31f1c8ae Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi <93502286+zulfaqar-azmi-t4@users.noreply.github.com> Date: Fri, 11 Oct 2024 15:11:40 +0900 Subject: [PATCH 4/4] Update planning/behavior_path_lane_change_module/src/utils/utils.cpp Co-authored-by: mkquda <168697710+mkquda@users.noreply.github.com> --- planning/behavior_path_lane_change_module/src/utils/utils.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_lane_change_module/src/utils/utils.cpp b/planning/behavior_path_lane_change_module/src/utils/utils.cpp index 6ad5404fedbae..0d5b20db3c0c8 100644 --- a/planning/behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/behavior_path_lane_change_module/src/utils/utils.cpp @@ -1256,7 +1256,7 @@ double get_min_dist_to_current_lanes_obj( const auto p_fp = tier4_autoware_utils::toMsg(polygon_p.to_3d()); const auto lateral_fp = motion_utils::calcLateralOffset(path_points, p_fp); - // ignore if the point is around the ego path + // ignore if the point is not on ego path if (std::abs(lateral_fp) > (bpp_param.vehicle_width / 2)) { continue; }