diff --git a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp index ac3904c060f3a..3371ff297a1ba 100644 --- a/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_avoidance_by_lane_change_module/src/scene.cpp @@ -14,6 +14,7 @@ #include "scene.hpp" +#include "autoware_behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" #include "autoware_behavior_path_planner_common/utils/path_utils.hpp" @@ -283,8 +284,8 @@ double AvoidanceByLaneChange::calcMinimumLaneChangeLength() const return std::numeric_limits::infinity(); } - return utils::lane_change::calcMinimumLaneChangeLength( - getRouteHandler(), current_lanes.back(), *lane_change_parameters_, direction_); + return utils::lane_change::calculation::calc_min_lane_change_length( + common_data_ptr_, current_lanes, direction_); } double AvoidanceByLaneChange::calcLateralOffset() const diff --git a/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt b/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt index b0d9967e65ddd..509f38d52dd45 100644 --- a/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt +++ b/planning/autoware_behavior_path_lane_change_module/CMakeLists.txt @@ -9,6 +9,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED src/interface.cpp src/manager.cpp src/scene.cpp + src/utils/calculation.cpp src/utils/markers.cpp src/utils/utils.cpp ) diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp index 523c700b399b8..34817efe2cc68 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/base_class.hpp @@ -50,7 +50,10 @@ class LaneChangeBase LaneChangeBase( std::shared_ptr parameters, LaneChangeModuleType type, Direction direction) - : lane_change_parameters_{std::move(parameters)}, direction_{direction}, type_{type} + : lane_change_parameters_{std::move(parameters)}, + common_data_ptr_{std::make_shared()}, + direction_{direction}, + type_{type} { } @@ -151,7 +154,18 @@ class LaneChangeBase bool isValidPath() const { return status_.is_valid_path; } - void setData(const std::shared_ptr & data) { planner_data_ = data; } + void setData(const std::shared_ptr & data) + { + planner_data_ = data; + if (!common_data_ptr_->bpp_params) { + common_data_ptr_->bpp_params = + std::make_shared(data->parameters); + } + common_data_ptr_->self_odometry = data->self_odometry; + common_data_ptr_->route_handler = data->route_handler; + common_data_ptr_->lc_params = lane_change_parameters_; + common_data_ptr_->direction = direction_; + } void toNormalState() { current_lane_change_state_ = LaneChangeStates::Normal; } @@ -219,6 +233,7 @@ class LaneChangeBase std::shared_ptr lane_change_parameters_{}; std::shared_ptr abort_path_{}; std::shared_ptr planner_data_{}; + lane_change::CommonDataPtr common_data_ptr_{}; BehaviorModuleOutput prev_module_output_{}; std::optional lane_change_stop_pose_{std::nullopt}; diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/calculation.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/calculation.hpp new file mode 100644 index 0000000000000..2e650628e2aa9 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/calculation.hpp @@ -0,0 +1,37 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ +#define AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ + +#include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" + +namespace autoware::behavior_path_planner::utils::lane_change::calculation +{ +using behavior_path_planner::lane_change::CommonDataPtr; +using behavior_path_planner::lane_change::LCParamPtr; +using behavior_path_planner::lane_change::RouteHandlerPtr; +using route_handler::Direction; + +double calc_min_lane_change_length( + const LCParamPtr & lc_params, const std::vector & shift_intervals); + +double calc_min_lane_change_length( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, + Direction direction = Direction::NONE); + +double calc_ego_remaining_distance_in_current_lanes(const CommonDataPtr & common_data_ptr); +} // namespace autoware::behavior_path_planner::utils::lane_change::calculation + +#endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__CALCULATION_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp index e88029420e296..8e70a6e3dfcd3 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/data_structs.hpp @@ -17,11 +17,16 @@ #include "autoware_behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" +#include +#include #include +#include + #include #include +#include #include #include @@ -205,6 +210,12 @@ enum class LaneChangeModuleType { namespace autoware::behavior_path_planner::lane_change { +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using route_handler::Direction; +using route_handler::RouteHandler; + struct PathSafetyStatus { bool is_safe{true}; @@ -217,6 +228,44 @@ struct LanesPolygon std::optional target; std::vector target_backward; }; + +struct Lanes +{ + lanelet::ConstLanelets current; + lanelet::ConstLanelets target; + std::vector preceding_target; +}; + +struct CommonData +{ + std::shared_ptr route_handler; + Odometry::ConstSharedPtr self_odometry; + std::shared_ptr bpp_params; + std::shared_ptr lc_params; + Lanes lanes; + Direction direction; + + [[nodiscard]] Pose get_ego_pose() const { return self_odometry->pose.pose; } + + [[nodiscard]] Twist get_ego_twist() const { return self_odometry->twist.twist; } + + [[nodiscard]] double get_ego_speed(bool use_norm = false) const + { + if (!use_norm) { + return get_ego_twist().linear.x; + } + + const auto x = get_ego_twist().linear.x; + const auto y = get_ego_twist().linear.y; + return std::hypot(x, y); + } +}; + +using RouteHandlerPtr = std::shared_ptr; +using BppParamPtr = std::shared_ptr; +using LCParamPtr = std::shared_ptr; +using CommonDataPtr = std::shared_ptr; +using LanesPtr = std::shared_ptr; } // namespace autoware::behavior_path_planner::lane_change #endif // AUTOWARE_BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DATA_STRUCTS_HPP_ diff --git a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp index daa4e08ce1d5d..3e988f9326a8e 100644 --- a/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp +++ b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp @@ -47,6 +47,7 @@ using autoware::route_handler::Direction; using autoware_perception_msgs::msg::PredictedObject; using autoware_perception_msgs::msg::PredictedObjects; using autoware_perception_msgs::msg::PredictedPath; +using behavior_path_planner::lane_change::CommonDataPtr; using behavior_path_planner::lane_change::LanesPolygon; using behavior_path_planner::lane_change::PathSafetyStatus; using geometry_msgs::msg::Point; @@ -59,13 +60,6 @@ using tier4_planning_msgs::msg::PathWithLaneId; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); -double calcMinimumLaneChangeLength( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals); - -double calcMinimumLaneChangeLength( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction); - double calcMaximumLaneChangeLength( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc); @@ -144,14 +138,10 @@ std::vector generateDrivableLanes( double getLateralShift(const LaneChangePath & path); bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & curent_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction); + const CommonDataPtr & common_data_ptr, const double abort_return_dist, const Direction direction); double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); -double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer); - std::string getStrDirection(const std::string & name, const Direction direction); CandidateOutput assignToCandidate( diff --git a/planning/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/autoware_behavior_path_lane_change_module/src/scene.cpp index 44132f490c018..efc0de9749ace 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -14,6 +14,7 @@ #include "autoware_behavior_path_lane_change_module/scene.hpp" +#include "autoware_behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware_behavior_path_lane_change_module/utils/utils.hpp" #include "autoware_behavior_path_planner_common/utils/drivable_area_expansion/static_drivable_area.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/objects_filtering.hpp" @@ -38,8 +39,8 @@ namespace autoware::behavior_path_planner { using motion_utils::calcSignedArcLength; -using utils::lane_change::calcMinimumLaneChangeLength; using utils::lane_change::createLanesPolygon; +using utils::lane_change::calculation::calc_min_lane_change_length; using utils::path_safety_checker::isPolygonOverlapLanelet; using utils::traffic_light::getDistanceToNextTrafficLight; @@ -172,10 +173,8 @@ TurnSignalInfo NormalLaneChange::get_current_turn_signal_info() lane_change_param.min_length_for_turn_signal_activation; const auto & route_handler = getRouteHandler(); const auto & common_parameter = getCommonParam(); - const auto shift_intervals = - route_handler->getLateralIntervalsToPreferredLane(current_lanes.back()); const double next_lane_change_buffer = - utils::lane_change::calcMinimumLaneChangeLength(lane_change_param, shift_intervals); + calc_min_lane_change_length(common_data_ptr_, current_lanes); const double nearest_dist_threshold = common_parameter.ego_nearest_dist_threshold; const double nearest_yaw_threshold = common_parameter.ego_nearest_yaw_threshold; const double base_to_front = common_parameter.base_link2front; @@ -338,7 +337,7 @@ void NormalLaneChange::insertStopPoint( const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lanelets.back()); const auto lane_change_buffer = - calcMinimumLaneChangeLength(*lane_change_parameters_, shift_intervals); + calc_min_lane_change_length(lane_change_parameters_, shift_intervals); const auto getDistanceAlongLanelet = [&](const geometry_msgs::msg::Pose & target) { return utils::getSignedDistance(path.points.front().point.pose, target, lanelets); @@ -418,7 +417,7 @@ void NormalLaneChange::insertStopPoint( 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); + calc_min_lane_change_length(lane_change_parameters_, shift_intervals); const auto stopping_distance_for_obj = distance_to_ego_lane_obj - lane_change_buffer_for_blocking_object - @@ -625,8 +624,7 @@ bool NormalLaneChange::isNearEndOfCurrentLanes( const auto & route_handler = getRouteHandler(); const auto & current_pose = getEgoPose(); - const auto lane_change_buffer = calcMinimumLaneChangeLength( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + const auto lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes); const auto distance_to_lane_change_end = std::invoke([&]() { auto distance_to_end = utils::getDistanceToEndOfLane(current_pose, current_lanes); @@ -1075,8 +1073,8 @@ void NormalLaneChange::filterAheadTerminalObjects( { 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 minimum_lane_change_length = + calc_min_lane_change_length(common_data_ptr_, current_lanes); const auto dist_ego_to_current_lanes_center = lanelet::utils::getLateralDistanceToClosestLanelet(current_lanes, current_pose); @@ -1235,9 +1233,8 @@ bool NormalLaneChange::hasEnoughLength( const auto current_pose = getEgoPose(); const auto & route_handler = getRouteHandler(); const auto overall_graphs_ptr = route_handler->getOverallGraphPtr(); - const auto minimum_lane_change_length_to_preferred_lane = calcMinimumLaneChangeLength( - route_handler, target_lanes.back(), *lane_change_parameters_, direction); - + const auto minimum_lane_change_length_to_preferred_lane = + calc_min_lane_change_length(common_data_ptr_, target_lanes, direction); const double lane_change_length = path.info.length.sum(); if (lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; @@ -1342,12 +1339,9 @@ bool NormalLaneChange::getLaneChangePaths( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const double lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes); + const double next_lane_change_buffer = + calc_min_lane_change_length(common_data_ptr_, target_lanes); const auto dist_to_end_of_current_lanes = utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); @@ -1638,12 +1632,9 @@ std::optional NormalLaneChange::calcTerminalLaneChangePath( const auto is_goal_in_route = route_handler.isInGoalRouteSection(target_lanes.back()); - const double lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(current_lanes.back())); - const double next_lane_change_buffer = utils::lane_change::calcMinimumLaneChangeLength( - *lane_change_parameters_, - route_handler.getLateralIntervalsToPreferredLane(target_lanes.back())); + const double lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes); + const double next_lane_change_buffer = + calc_min_lane_change_length(common_data_ptr_, target_lanes); const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanes); @@ -1884,8 +1875,8 @@ bool NormalLaneChange::calcAbortPath() const auto ego_nearest_yaw_threshold = common_param.ego_nearest_yaw_threshold; const auto direction = getDirection(); - const auto minimum_lane_change_length = utils::lane_change::calcMinimumLaneChangeLength( - route_handler, selected_path.info.current_lanes.back(), *lane_change_parameters_, direction); + const auto minimum_lane_change_length = + calc_min_lane_change_length(common_data_ptr_, selected_path.info.current_lanes, direction); const auto & lane_changing_path = selected_path.path; const auto lane_changing_end_pose_idx = std::invoke([&]() { @@ -1932,8 +1923,7 @@ bool NormalLaneChange::calcAbortPath() } if (!utils::lane_change::hasEnoughLengthToLaneChangeAfterAbort( - route_handler, reference_lanelets, current_pose, abort_return_dist, - *lane_change_parameters_, direction)) { + common_data_ptr_, abort_return_dist, direction)) { RCLCPP_ERROR(logger_, "insufficient distance to abort."); return false; } @@ -2132,8 +2122,7 @@ bool NormalLaneChange::isVehicleStuck( route_handler->isInGoalRouteSection(current_lanes.back()) ? utils::getSignedDistance(getEgoPose(), route_handler->getGoalPose(), current_lanes) : utils::getDistanceToEndOfLane(getEgoPose(), current_lanes); - const auto lane_change_buffer = calcMinimumLaneChangeLength( - route_handler, current_lanes.back(), *lane_change_parameters_, Direction::NONE); + const auto lane_change_buffer = calc_min_lane_change_length(common_data_ptr_, current_lanes); const double stop_point_buffer = lane_change_parameters_->backward_length_buffer_for_end_of_lane; const double terminal_judge_buffer = lane_change_buffer + stop_point_buffer + 1.0; if (distance_to_terminal < terminal_judge_buffer) { diff --git a/planning/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp b/planning/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp new file mode 100644 index 0000000000000..9b7ec46251461 --- /dev/null +++ b/planning/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -0,0 +1,85 @@ +// Copyright 2024 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "autoware_behavior_path_lane_change_module/utils/calculation.hpp" + +#include "autoware_behavior_path_planner_common/utils/utils.hpp" + +namespace autoware::behavior_path_planner::utils::lane_change::calculation +{ +static rclcpp::Logger logger{rclcpp::get_logger("lane_change.utils")}; + +double calc_min_lane_change_length( + const LCParamPtr & lc_params, const std::vector & shift_intervals) +{ + if (shift_intervals.empty()) { + return 0.0; + } + + const auto min_vel = lc_params->minimum_lane_changing_velocity; + const auto lat_acc = lc_params->lane_change_lat_acc_map.find(min_vel); + const auto max_lat_acc = lat_acc.second; + const auto lat_jerk = lc_params->lane_changing_lateral_jerk; + const auto finish_judge_buffer = lc_params->lane_change_finish_judge_buffer; + + const auto calc_sum = [&](double sum, double shift_interval) { + const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); + return sum + (min_vel * t + finish_judge_buffer); + }; + + const auto total_length = + std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); + + const auto backward_buffer = lc_params->backward_length_buffer_for_end_of_lane; + return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); +} + +double calc_min_lane_change_length( + const CommonDataPtr & common_data_ptr, const lanelet::ConstLanelets & lanes, Direction direction) +{ + if (lanes.empty()) { + RCLCPP_DEBUG(logger, "Because the lanes are empty, 0.0 meter is returned."); + return 0.0; + } + + const auto & route_handler = common_data_ptr->route_handler; + if (!route_handler) { + RCLCPP_DEBUG(logger, "Because route hander pointer is null, 0.0 is returned."); + return 0.0; + } + + const auto shift_intervals = + route_handler->getLateralIntervalsToPreferredLane(lanes.back(), direction); + + const auto & lc_params = common_data_ptr->lc_params; + return calc_min_lane_change_length(lc_params, shift_intervals); +} + +double calc_ego_remaining_distance_in_current_lanes(const CommonDataPtr & common_data_ptr) +{ + const auto & current_lanes = common_data_ptr->lanes.current; + if (current_lanes.empty()) { + RCLCPP_DEBUG(logger, "Because the lanes are empty, 0.0 is returned."); + return 0.0; + } + + const auto & route_handler = common_data_ptr->route_handler; + if (route_handler->is_in_goal_route_section(current_lanes)) { + const auto & goal_pose = route_handler->getGoalPose(); + return utils::getSignedDistance(common_data_ptr->get_ego_pose(), goal_pose, current_lanes); + } + + return utils::getDistanceToEndOfLane(common_data_ptr->get_ego_pose(), current_lanes); +} +} // namespace autoware::behavior_path_planner::utils::lane_change::calculation diff --git a/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp b/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp index b07d2100f210a..3dc85bad6ab22 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp +++ b/planning/autoware_behavior_path_lane_change_module/src/utils/utils.cpp @@ -14,9 +14,9 @@ #include "autoware_behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware_behavior_path_lane_change_module/utils/calculation.hpp" #include "autoware_behavior_path_lane_change_module/utils/data_structs.hpp" #include "autoware_behavior_path_lane_change_module/utils/path.hpp" -#include "autoware_behavior_path_planner_common/marker_utils/utils.hpp" #include "autoware_behavior_path_planner_common/parameters.hpp" #include "autoware_behavior_path_planner_common/utils/path_safety_checker/safety_check.hpp" #include "autoware_behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -84,43 +84,6 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } -double calcMinimumLaneChangeLength( - const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals) -{ - if (shift_intervals.empty()) { - return 0.0; - } - - const auto min_vel = lane_change_parameters.minimum_lane_changing_velocity; - const auto [min_lat_acc, max_lat_acc] = - lane_change_parameters.lane_change_lat_acc_map.find(min_vel); - const auto lat_jerk = lane_change_parameters.lane_changing_lateral_jerk; - const auto finish_judge_buffer = lane_change_parameters.lane_change_finish_judge_buffer; - - const auto calc_sum = [&](double sum, double shift_interval) { - const auto t = PathShifter::calcShiftTimeFromJerk(shift_interval, lat_jerk, max_lat_acc); - return sum + (min_vel * t + finish_judge_buffer); - }; - - const auto total_length = - std::accumulate(shift_intervals.begin(), shift_intervals.end(), 0.0, calc_sum); - - const auto backward_buffer = lane_change_parameters.backward_length_buffer_for_end_of_lane; - return total_length + backward_buffer * (static_cast(shift_intervals.size()) - 1.0); -} - -double calcMinimumLaneChangeLength( - const std::shared_ptr & route_handler, const lanelet::ConstLanelet & lane, - const LaneChangeParameters & lane_change_parameters, Direction direction) -{ - if (!route_handler) { - return std::numeric_limits::max(); - } - - const auto shift_intervals = route_handler->getLateralIntervalsToPreferredLane(lane, direction); - return utils::lane_change::calcMinimumLaneChangeLength(lane_change_parameters, shift_intervals); -} - double calcMaximumLaneChangeLength( const double current_velocity, const LaneChangeParameters & lane_change_parameters, const std::vector & shift_intervals, const double max_acc) @@ -659,24 +622,26 @@ double getLateralShift(const LaneChangePath & path) } bool hasEnoughLengthToLaneChangeAfterAbort( - const std::shared_ptr & route_handler, const lanelet::ConstLanelets & current_lanes, - const Pose & current_pose, const double abort_return_dist, - const LaneChangeParameters & lane_change_parameters, const Direction direction) + const CommonDataPtr & common_data_ptr, const double abort_return_dist, const Direction direction) { + const auto & current_lanes = common_data_ptr->lanes.current; if (current_lanes.empty()) { return false; } - const auto minimum_lane_change_length = calcMinimumLaneChangeLength( - route_handler, current_lanes.back(), lane_change_parameters, direction); + const auto minimum_lane_change_length = + calculation::calc_min_lane_change_length(common_data_ptr, current_lanes, direction); const auto abort_plus_lane_change_length = abort_return_dist + minimum_lane_change_length; + const auto current_pose = common_data_ptr->get_ego_pose(); if (abort_plus_lane_change_length > utils::getDistanceToEndOfLane(current_pose, current_lanes)) { return false; } + const auto & goal_pose = common_data_ptr->route_handler->getGoalPose(); + if ( abort_plus_lane_change_length > - utils::getSignedDistance(current_pose, route_handler->getGoalPose(), current_lanes)) { + utils::getSignedDistance(current_pose, goal_pose, current_lanes)) { return false; } diff --git a/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp b/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp index 976db246275d6..ee518c41447fa 100644 --- a/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp +++ b/planning/autoware_route_handler/include/autoware_route_handler/route_handler.hpp @@ -103,6 +103,9 @@ class RouteHandler // for goal bool isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) const; + + [[nodiscard]] bool is_in_goal_route_section(const lanelet::ConstLanelets & lanelets) const; + Pose getGoalPose() const; Pose getStartPose() const; Pose getOriginalStartPose() const; diff --git a/planning/autoware_route_handler/src/route_handler.cpp b/planning/autoware_route_handler/src/route_handler.cpp index dfe79a7a758af..377fd61916024 100644 --- a/planning/autoware_route_handler/src/route_handler.cpp +++ b/planning/autoware_route_handler/src/route_handler.cpp @@ -543,6 +543,13 @@ bool RouteHandler::isInGoalRouteSection(const lanelet::ConstLanelet & lanelet) c return exists(route_ptr_->segments.back().primitives, lanelet.id()); } +bool RouteHandler::is_in_goal_route_section(const lanelet::ConstLanelets & lanelets) const +{ + return std::any_of(lanelets.rbegin(), lanelets.rend(), [&](const auto & lane) { + return isInGoalRouteSection(lane); + }); +} + lanelet::ConstLanelets RouteHandler::getLaneletsFromIds(const lanelet::Ids & ids) const { lanelet::ConstLanelets lanelets;