From eb0aa8c3bd2f014fb9d8012122d5bf41c098a35d Mon Sep 17 00:00:00 2001 From: Zulfaqar Azmi Date: Mon, 17 Jun 2024 19:26:54 +0900 Subject: [PATCH] remove calcPhaseLength Signed-off-by: Zulfaqar Azmi --- .../utils/calculation.hpp | 12 ++++++- .../utils/utils.hpp | 17 ---------- .../src/scene.cpp | 7 ++-- .../src/utils/calculation.cpp | 32 +++++++++++++++++++ .../src/utils/utils.cpp | 10 ------ 5 files changed, 47 insertions(+), 31 deletions(-) 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 index 2e650628e2aa9..b58c8f2169d6c 100644 --- 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 @@ -32,6 +32,16 @@ double calc_min_lane_change_length( 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 +double calc_length_with_acceleration( + const double velocity, const double acceleration, const double duration); + +double calc_prepare_segment_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration); + +double calc_lane_changing_segment_length( + const double velocity, const double minimum_velocity, const double maximum_velocity, + const double acceleration, const double duration); +} // 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/utils.hpp b/planning/autoware_behavior_path_lane_change_module/include/autoware_behavior_path_lane_change_module/utils/utils.hpp index 3e988f9326a8e..1b697706e3a65 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 @@ -268,23 +268,6 @@ bool isWithinIntersection( */ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Polygon2d & polygon); -/** - * @brief Calculates the distance required during a lane change operation. - * - * Used for computing prepare or lane change length based on current and maximum velocity, - * acceleration, and duration, returning the lesser of accelerated distance or distance at max - * velocity. - * - * @param velocity The current velocity of the vehicle in meters per second (m/s). - * @param maximum_velocity The maximum velocity the vehicle can reach in meters per second (m/s). - * @param acceleration The acceleration of the vehicle in meters per second squared (m/s^2). - * @param duration The duration of the lane change in seconds (s). - * @return The calculated minimum distance in meters (m). - */ -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double time); - LanesPolygon createLanesPolygon( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const std::vector & target_backward_lanes); 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 efc0de9749ace..b3f91cb8fdc25 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -43,6 +43,7 @@ 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; +namespace calculation = utils::lane_change::calculation; NormalLaneChange::NormalLaneChange( const std::shared_ptr & parameters, LaneChangeModuleType type, @@ -1388,7 +1389,7 @@ bool NormalLaneChange::getLaneChangePaths( (prepare_duration < 1e-3) ? 0.0 : ((prepare_velocity - current_velocity) / prepare_duration); - const auto prepare_length = utils::lane_change::calcPhaseLength( + const auto prepare_length = utils::lane_change::calculation::calc_prepare_segment_length( current_velocity, getCommonParam().max_vel, longitudinal_acc_on_prepare, prepare_duration); auto prepare_segment = getPrepareSegment(current_lanes, backward_path_length, prepare_length); @@ -1441,8 +1442,8 @@ bool NormalLaneChange::getLaneChangePaths( utils::lane_change::calcLaneChangingAcceleration( initial_lane_changing_velocity, max_path_velocity, lane_changing_time, sampled_longitudinal_acc); - const auto lane_changing_length = utils::lane_change::calcPhaseLength( - initial_lane_changing_velocity, getCommonParam().max_vel, + const auto lane_changing_length = calculation::calc_lane_changing_segment_length( + initial_lane_changing_velocity, minimum_lane_changing_velocity, getCommonParam().max_vel, longitudinal_acc_on_lane_changing, lane_changing_time); const auto terminal_lane_changing_velocity = std::min( initial_lane_changing_velocity + longitudinal_acc_on_lane_changing * lane_changing_time, 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 index 9b7ec46251461..46f9407279f60 100644 --- a/planning/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp +++ b/planning/autoware_behavior_path_lane_change_module/src/utils/calculation.cpp @@ -82,4 +82,36 @@ double calc_ego_remaining_distance_in_current_lanes(const CommonDataPtr & common return utils::getDistanceToEndOfLane(common_data_ptr->get_ego_pose(), current_lanes); } + +double calc_length_with_acceleration( + const double velocity, const double acceleration, const double duration) +{ + return velocity * duration + 0.5 * acceleration * std::pow(duration, 2); +} + +double calc_prepare_segment_length( + const double velocity, const double maximum_velocity, const double acceleration, + const double duration) +{ + const auto length_with_acceleration = + calc_length_with_acceleration(velocity, acceleration, duration); + + const auto length_with_max_velocity = maximum_velocity * duration; + return std::clamp(length_with_acceleration, 0.0, length_with_max_velocity); +} + +double calc_lane_changing_segment_length( + const double velocity, const double minimum_velocity, const double maximum_velocity, + const double acceleration, const double duration) +{ + if (minimum_velocity > maximum_velocity) { + return 0.0; + } + const auto length_with_acceleration = + calc_length_with_acceleration(velocity, acceleration, duration); + + const auto length_with_min_velocity = minimum_velocity * duration; + const auto length_with_max_velocity = maximum_velocity * duration; + return std::clamp(length_with_acceleration, length_with_min_velocity, length_with_max_velocity); +} } // 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 3dc85bad6ab22..287aeb7e2118b 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 @@ -1160,16 +1160,6 @@ bool isWithinTurnDirectionLanes(const lanelet::ConstLanelet & lanelet, const Pol polygon, utils::toPolygon2d(lanelet::utils::to2D(lanelet.polygon2d().basicPolygon()))); } -double calcPhaseLength( - const double velocity, const double maximum_velocity, const double acceleration, - const double duration) -{ - const auto length_with_acceleration = - velocity * duration + 0.5 * acceleration * std::pow(duration, 2); - const auto length_with_max_velocity = maximum_velocity * duration; - return std::min(length_with_acceleration, length_with_max_velocity); -} - LanesPolygon createLanesPolygon( const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const std::vector & target_backward_lanes)