From 86c7ac4737c5590e1746d67525fde9d2b0d46a37 Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 18 May 2023 02:15:13 +0900 Subject: [PATCH 01/14] refactor(behavior_path_planner): remove unnecessary prepare segment function Signed-off-by: yutaka --- .../scene_module/lane_change/normal.hpp | 5 -- .../utils/lane_change/utils.hpp | 10 ---- .../src/scene_module/lane_change/normal.cpp | 26 ---------- .../src/utils/lane_change/utils.cpp | 50 ------------------- 4 files changed, 91 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 879a13859fc9f..be4b00d038b30 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -126,11 +126,6 @@ class NormalLaneChangeBT : public NormalLaneChange int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, - const double prepare_velocity) const override; - std::vector getDrivableLanes() const override; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 594a91907f655..24bcb1f760d11 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -111,16 +111,6 @@ PathWithLaneId getReferencePathFromTargetLane( const double resample_interval, const bool is_goal_in_route, const double next_lane_change_buffer); -PathWithLaneId getPrepareSegment( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const double arc_length_from_current, const double backward_path_length, - const double prepare_length, const double prepare_velocity); - -PathWithLaneId getPrepareSegment( - const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets, - const Pose & current_pose, const double backward_path_length, const double prepare_length, - const double prepare_velocity); - PathWithLaneId getTargetSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index e94517d314554..26682d5f284ba 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -958,32 +958,6 @@ int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane)); } -PathWithLaneId NormalLaneChangeBT::getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, - const double prepare_velocity) const -{ - if (current_lanes.empty()) { - return PathWithLaneId(); - } - - const double s_start = arc_length_from_current - backward_path_length; - const double s_end = arc_length_from_current + prepare_length; - - RCLCPP_DEBUG( - rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()).get_child("getPrepareSegment"), - "start: %f, end: %f", s_start, s_end); - - PathWithLaneId prepare_segment = - getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); - - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); - - return prepare_segment; -} - std::vector NormalLaneChangeBT::getDrivableLanes() const { return utils::lane_change::generateDrivableLanes( diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index f1123084b0a76..57c9cfbb83edc 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -387,56 +387,6 @@ bool isObjectIndexIncluded( return std::count(dynamic_objects_indices.begin(), dynamic_objects_indices.end(), index) != 0; } -PathWithLaneId getPrepareSegment( - const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, - const double arc_length_from_current, const double backward_path_length, - const double prepare_length, const double prepare_velocity) -{ - if (original_lanelets.empty()) { - return PathWithLaneId(); - } - - const double s_start = arc_length_from_current - backward_path_length; - const double s_end = arc_length_from_current + prepare_length; - - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("getPrepareSegment"), - "start: %f, end: %f", s_start, s_end); - - PathWithLaneId prepare_segment = - route_handler.getCenterLinePath(original_lanelets, s_start, s_end); - - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); - - return prepare_segment; -} - -PathWithLaneId getPrepareSegment( - const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets, - const Pose & current_pose, const double backward_path_length, const double prepare_length, - const double prepare_velocity) -{ - if (original_lanelets.empty()) { - return PathWithLaneId(); - } - - auto prepare_segment = original_path; - const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( - prepare_segment.points, current_pose, 3.0, 1.0); - utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); - - return prepare_segment; -} - double calcLaneChangingLength( const double lane_changing_velocity, const double shift_length, const double lateral_acc, const double lateral_jerk) From 63b88ec4108019ea032698bbe756ebe54813835c Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 18 May 2023 03:03:56 +0900 Subject: [PATCH 02/14] update Signed-off-by: yutaka --- .../scene_module/lane_change/normal.hpp | 5 ++++ .../src/scene_module/lane_change/normal.cpp | 26 +++++++++++++++++++ 2 files changed, 31 insertions(+) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index be4b00d038b30..fc325e52695fb 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -121,6 +121,11 @@ class NormalLaneChangeBT : public NormalLaneChange PathWithLaneId getReferencePath() const override; + PathWithLaneId getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, + const double prepare_velocity) const override; + protected: lanelet::ConstLanelets getCurrentLanes() const override; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 26682d5f284ba..e94517d314554 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -958,6 +958,32 @@ int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane return std::abs(getRouteHandler()->getNumLaneToPreferredLane(lane)); } +PathWithLaneId NormalLaneChangeBT::getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, + const double prepare_velocity) const +{ + if (current_lanes.empty()) { + return PathWithLaneId(); + } + + const double s_start = arc_length_from_current - backward_path_length; + const double s_end = arc_length_from_current + prepare_length; + + RCLCPP_DEBUG( + rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()).get_child("getPrepareSegment"), + "start: %f, end: %f", s_start, s_end); + + PathWithLaneId prepare_segment = + getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); + + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); + + return prepare_segment; +} + std::vector NormalLaneChangeBT::getDrivableLanes() const { return utils::lane_change::generateDrivableLanes( From 79101474d04c05ad536ed55d8c8f99077ef57a6f Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 18 May 2023 03:04:52 +0900 Subject: [PATCH 03/14] update Signed-off-by: yutaka --- .../scene_module/lane_change/normal.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index fc325e52695fb..879a13859fc9f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -121,16 +121,16 @@ class NormalLaneChangeBT : public NormalLaneChange PathWithLaneId getReferencePath() const override; - PathWithLaneId getPrepareSegment( - const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, - const double prepare_velocity) const override; - protected: lanelet::ConstLanelets getCurrentLanes() const override; int getNumToPreferredLane(const lanelet::ConstLanelet & lane) const override; + PathWithLaneId getPrepareSegment( + const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, + const double backward_path_length, const double prepare_length, + const double prepare_velocity) const override; + std::vector getDrivableLanes() const override; }; } // namespace behavior_path_planner From c2c977ddd37b8f62ed4c59542438882f7c5720e4 Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 18 May 2023 15:19:36 +0900 Subject: [PATCH 04/14] feat(lane_change): enable acceleration on the prepare segment Signed-off-by: yutaka --- .../config/lane_change/lane_change.param.yaml | 2 +- .../scene_module/lane_change/base_class.hpp | 2 +- .../scene_module/lane_change/normal.hpp | 4 +- .../utils/lane_change/utils.hpp | 10 +++ .../src/scene_module/lane_change/normal.cpp | 28 ++++---- .../src/utils/lane_change/utils.cpp | 69 +++++++++++++++++++ 6 files changed, 96 insertions(+), 19 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 0514ad9a61f72..031ddc08fba1d 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -12,7 +12,7 @@ minimum_lane_changing_velocity: 2.78 # [m/s] prediction_time_resolution: 0.5 # [s] - longitudinal_acceleration_sampling_num: 3 + longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 # lateral acceleration map diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index bca2f2fe1082b..827eddbad499d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -182,7 +182,7 @@ class LaneChangeBase virtual PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, + const double backward_path_length, const double prepare_length, const double current_velocity, const double prepare_velocity) const = 0; virtual bool getLaneChangePaths( diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 879a13859fc9f..9e262a677d365 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -91,7 +91,7 @@ class NormalLaneChange : public LaneChangeBase PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, + const double backward_path_length, const double prepare_length, const double current_velocity, const double prepare_velocity) const override; bool getLaneChangePaths( @@ -128,7 +128,7 @@ class NormalLaneChangeBT : public NormalLaneChange PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, + const double backward_path_length, const double prepare_length, const double current_velocity, const double prepare_velocity) const override; std::vector getDrivableLanes() const override; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 24bcb1f760d11..eb81c92eb80a8 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -53,6 +53,16 @@ using tier4_autoware_utils::Polygon2d; double calcLaneChangeResampleInterval( const double lane_changing_length, const double lane_changing_velocity); +double calcMaximumAcceleration( + const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, + const BehaviorPathPlannerParameters & params); + +void setPrepareVelocity( + PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); + +std::vector getAccelerationValues( + const double min_acc, const double max_acc, const size_t sampling_num); + std::vector replaceWithSortedIds( const std::vector & original_lane_ids, const std::vector> & sorted_lane_ids); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index e94517d314554..2c51640be6aa6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -371,7 +371,7 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) PathWithLaneId NormalLaneChange::getPrepareSegment( const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const double arc_length_from_current, const double backward_path_length, - const double prepare_length, const double prepare_velocity) const + const double prepare_length, const double current_velocity, const double prepare_velocity) const { if (current_lanes.empty()) { return PathWithLaneId(); @@ -382,9 +382,7 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); return prepare_segment; } @@ -412,7 +410,7 @@ bool NormalLaneChange::getLaneChangePaths( // get velocity const auto current_velocity = getEgoTwist().linear.x; - // compute maximum longitudinal deceleration + // compute maximum longitudinal deceleration and acceleration const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, this]() { const double min_a = (minimum_lane_changing_velocity - current_velocity) / @@ -420,9 +418,12 @@ bool NormalLaneChange::getLaneChangePaths( return std::clamp( min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); }); + const auto maximum_acceleration = utils::lane_change::calcMaximumAcceleration( + prev_module_path_, getEgoPose(), current_velocity, common_parameter); - const auto longitudinal_acc_resolution = - std::abs(maximum_deceleration) / longitudinal_acc_sampling_num; + // get sampling acceleration values + const auto longitudinal_acc_sampling_values = utils::lane_change::getAccelerationValues( + maximum_deceleration, maximum_acceleration, longitudinal_acc_sampling_num); const auto target_length = utils::getArcLengthToTargetLanelet(original_lanelets, target_lanelets.front(), getEgoPose()); @@ -451,9 +452,8 @@ bool NormalLaneChange::getLaneChangePaths( LaneChangeTargetObjectIndices dynamic_object_indices; - candidate_paths->reserve(longitudinal_acc_sampling_num * lateral_acc_sampling_num); - for (double sampled_longitudinal_acc = 0.0; sampled_longitudinal_acc >= maximum_deceleration; - sampled_longitudinal_acc -= longitudinal_acc_resolution) { + candidate_paths->reserve(longitudinal_acc_sampling_values.size() * lateral_acc_sampling_num); + for (const auto & sampled_longitudinal_acc : longitudinal_acc_sampling_values) { const auto prepare_velocity = std::max( current_velocity + sampled_longitudinal_acc * prepare_duration, minimum_lane_changing_velocity); @@ -475,7 +475,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto prepare_segment = getPrepareSegment( original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length, - prepare_velocity); + current_velocity, prepare_velocity); if (prepare_segment.points.empty()) { RCLCPP_DEBUG( @@ -960,7 +960,7 @@ int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane PathWithLaneId NormalLaneChangeBT::getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, + const double backward_path_length, const double prepare_length, const double current_velocity, const double prepare_velocity) const { if (current_lanes.empty()) { @@ -977,9 +977,7 @@ PathWithLaneId NormalLaneChangeBT::getPrepareSegment( PathWithLaneId prepare_segment = getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); - prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( - prepare_segment.points.back().point.longitudinal_velocity_mps, - static_cast(prepare_velocity)); + utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); return prepare_segment; } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 57c9cfbb83edc..a354350c0e166 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -61,6 +61,75 @@ double calcLaneChangeResampleInterval( lane_changing_length / min_resampling_points, lane_changing_velocity * resampling_dt); } +double calcMaximumAcceleration( + const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, + const BehaviorPathPlannerParameters & params) +{ + const double & max_acc = params.max_acc; + if (path.points.empty()) { + return max_acc; + } + + const double & nearest_dist_threshold = params.ego_nearest_dist_threshold; + const double & nearest_yaw_threshold = params.ego_nearest_yaw_threshold; + + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + path.points, current_pose, nearest_dist_threshold, nearest_yaw_threshold); + const double & max_path_velocity = + path.points.at(current_seg_idx).point.longitudinal_velocity_mps; + const double & prepare_duration = params.lane_change_prepare_duration; + + const double acc = (max_path_velocity - current_velocity) / prepare_duration; + return std::clamp(acc, 0.0, max_acc); +} + +void setPrepareVelocity( + PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) +{ + if (current_velocity < prepare_velocity) { + // acceleration + for (size_t i = 0; i < prepare_segment.points.size(); ++i) { + prepare_segment.points.at(i).point.longitudinal_velocity_mps = std::min( + prepare_segment.points.at(i).point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); + } + } else { + // deceleration + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast(prepare_velocity)); + } +} + +std::vector getAccelerationValues( + const double min_acc, const double max_acc, const size_t sampling_num) +{ + if (min_acc > max_acc) { + return {}; + } + + if (max_acc - min_acc < std::numeric_limits::epsilon()) { + return {0.0}; + } + + constexpr double epsilon = 0.001; + const auto resolution = std::abs(max_acc - min_acc) / sampling_num; + + std::vector sampled_values{min_acc}; + for (double sampled_acc = min_acc + resolution; + sampled_acc < max_acc + std::numeric_limits::epsilon(); sampled_acc += resolution) { + // check whether if we need to add 0.0 + if (sampled_values.back() < -epsilon && sampled_acc > epsilon) { + sampled_values.push_back(0.0); + } + + sampled_values.push_back(sampled_acc); + } + std::reverse(sampled_values.begin(), sampled_values.end()); + + return sampled_values; +} + PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2) { PathWithLaneId path; From 1cb8a1ba2db31910b7385956ede51ae0750f60a2 Mon Sep 17 00:00:00 2001 From: yutaka Date: Thu, 18 May 2023 23:39:10 +0900 Subject: [PATCH 05/14] feat(behavior_path_planner): add longitudinal acc to path shifter Signed-off-by: yutaka --- .../utils/path_shifter/path_shifter.hpp | 17 +++- .../src/utils/path_shifter/path_shifter.cpp | 91 +++++++++++++------ 2 files changed, 78 insertions(+), 30 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp index b5b1e5af5c328..76119a18646ab 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/path_shifter/path_shifter.hpp @@ -78,9 +78,14 @@ class PathShifter void setVelocity(const double velocity); /** - * @brief Set acceleration limit + * @brief Set lateral acceleration limit */ - void setLateralAccelerationLimit(const double acc); + void setLateralAccelerationLimit(const double lateral_acc); + + /** + * @brief Set longitudinal acceleration + */ + void setLongitudinalAcceleration(const double longitudinal_acc); /** * @brief Add shift point. You don't have to care about the start/end_idx. @@ -165,7 +170,9 @@ class PathShifter double velocity_{0.0}; // lateral acceleration limit considered in the path planning - double acc_limit_{-1.0}; + double lateral_acc_limit_{-1.0}; + + double longitudinal_acc_{0.0}; // Logger mutable rclcpp::Logger logger_{ @@ -180,6 +187,10 @@ class PathShifter std::pair, std::vector> getBaseLengthsWithoutAccelLimit( const double arclength, const double shift_length, const bool offset_back) const; + std::pair, std::vector> getBaseLengthsWithoutAccelLimit( + const double arclength, const double shift_length, const double velocity, + const double longitudinal_acc, const double total_time, const bool offset_back) const; + /** * @brief Calculate path index for shift_lines and set is_index_aligned_ to true. */ diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index c6356b8ca42c2..28fe6ceeba6b3 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -69,9 +69,14 @@ void PathShifter::setVelocity(const double velocity) velocity_ = velocity; } -void PathShifter::setLateralAccelerationLimit(const double acc) +void PathShifter::setLateralAccelerationLimit(const double lateral_acc) { - acc_limit_ = acc; + lateral_acc_limit_ = lateral_acc; +} + +void PathShifter::setLongitudinalAcceleration(const double longitudinal_acc) +{ + longitudinal_acc_ = longitudinal_acc; } void PathShifter::addShiftLine(const ShiftLine & line) @@ -271,39 +276,67 @@ std::pair, std::vector> PathShifter::getBaseLengthsW return std::pair{base_lon, base_lat}; } +std::pair, std::vector> PathShifter::getBaseLengthsWithoutAccelLimit( + const double arclength, const double shift_length, const double velocity, + const double longitudinal_acc, const double total_time, const bool offset_back) const +{ + const auto & s = arclength; + const auto & l = shift_length; + const auto & v = velocity; + const auto & a = longitudinal_acc; + const auto & T = total_time; + const double t = T / 4; + + const double s1 = v * t + 0.5 * a * t * t; + const double s2 = s1 + 2 * v * t + 2 * a * t * t; + std::vector base_lon = {0.0, s1, s2, s}; + std::vector base_lat = {0.0, 1.0 / 12.0 * l, 11.0 / 12.0 * l, l}; + + if (!offset_back) std::reverse(base_lat.begin(), base_lat.end()); + + return std::pair{base_lon, base_lat}; +} + std::pair, std::vector> PathShifter::calcBaseLengths( const double arclength, const double shift_length, const bool offset_back) const { const auto speed = std::abs(velocity_); - if (speed < 1.0e-5) { + // For longitudinal acceleration, we only consider positive side + // negative acceleration (deceleration) is treated as 0.0 + const double acc_threshold = 0.0001; + const auto & a = longitudinal_acc_ > acc_threshold ? longitudinal_acc_ : 0.0; + + if (speed < 1.0e-5 && a < acc_threshold) { // no need to consider acceleration limit - RCLCPP_DEBUG(logger_, "set velocity is zero. acc limit is ignored"); + RCLCPP_DEBUG(logger_, "set velocity is zero. lateral acc limit is ignored"); return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); } - const auto T = arclength / speed; const auto L = std::abs(shift_length); - const auto a_max = 8.0 * L / (T * T); + const auto T = + a > acc_threshold ? (-speed + std::sqrt(speed * speed + 2 * a * L)) / a : L / speed; + const auto lateral_a_max = 8.0 * L / (T * T); - if (a_max < acc_limit_) { + if (lateral_a_max < lateral_acc_limit_) { // no need to consider acceleration limit RCLCPP_WARN_THROTTLE( - logger_, clock_, 3000, "No need to consider acc limit. max: %f, limit: %f", a_max, - acc_limit_); - return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); + logger_, clock_, 3000, "No need to consider lateral acc limit. max: %f, limit: %f", + lateral_a_max, lateral_acc_limit_); + return getBaseLengthsWithoutAccelLimit(arclength, shift_length, speed, a, T, offset_back); } - const auto tj = T / 2.0 - 2.0 * L / (acc_limit_ * T); - const auto ta = 4.0 * L / (acc_limit_ * T) - T / 2.0; - const auto jerk = (2.0 * acc_limit_ * acc_limit_ * T) / (acc_limit_ * T * T - 4.0 * L); + const auto tj = T / 2.0 - 2.0 * L / (lateral_acc_limit_ * T); + const auto ta = 4.0 * L / (lateral_acc_limit_ * T) - T / 2.0; + const auto lat_jerk = + (2.0 * lateral_acc_limit_ * lateral_acc_limit_ * T) / (lateral_acc_limit_ * T * T - 4.0 * L); - if (tj < 0.0 || ta < 0.0 || jerk < 0.0 || tj / T < 0.1) { + if (tj < 0.0 || ta < 0.0 || lat_jerk < 0.0 || tj / T < 0.1) { // no need to consider acceleration limit RCLCPP_WARN_THROTTLE( logger_, clock_, 3000, "Acc limit is too small to be applied. Tj: %f, Ta: %f, j: %f, a_max: %f, acc_limit: %f", tj, - ta, jerk, a_max, acc_limit_); + ta, lat_jerk, lateral_a_max, lateral_acc_limit_); return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); } @@ -311,20 +344,24 @@ std::pair, std::vector> PathShifter::calcBaseLengths const auto ta2_tj = ta * ta * tj; const auto ta_tj2 = ta * tj * tj; - const auto s1 = tj * speed; - const auto s2 = s1 + ta * speed; - const auto s3 = s2 + tj * speed; // = s4 - const auto s5 = s3 + tj * speed; - const auto s6 = s5 + ta * speed; - const auto s7 = s6 + tj * speed; + const auto s1 = tj * speed + 0.5 * a * tj * tj; + const auto s2 = s1 + ta * speed + 0.5 * a * ta * ta; + const auto s3 = s2 + tj * speed + 0.5 * a * tj * tj; // = s4 + const auto s5 = s3 + tj * speed + 0.5 * a * tj * tj; + const auto s6 = s5 + ta * speed + 0.5 * a * ta * ta; + const auto s7 = s6 + tj * speed + 0.5 * a * tj * tj; const auto sign = shift_length > 0.0 ? 1.0 : -1.0; - const auto l1 = sign * (1.0 / 6.0 * jerk * tj3); - const auto l2 = sign * (1.0 / 6.0 * jerk * tj3 + 0.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); - const auto l3 = sign * (jerk * tj3 + 1.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); // = l4 - const auto l5 = sign * (11.0 / 6.0 * jerk * tj3 + 2.5 * jerk * ta_tj2 + 0.5 * jerk * ta2_tj); - const auto l6 = sign * (11.0 / 6.0 * jerk * tj3 + 3.0 * jerk * ta_tj2 + jerk * ta2_tj); - const auto l7 = sign * (2.0 * jerk * tj3 + 3.0 * jerk * ta_tj2 + jerk * ta2_tj); + const auto l1 = sign * (1.0 / 6.0 * lat_jerk * tj3); + const auto l2 = + sign * (1.0 / 6.0 * lat_jerk * tj3 + 0.5 * lat_jerk * ta_tj2 + 0.5 * lat_jerk * ta2_tj); + const auto l3 = + sign * (lat_jerk * tj3 + 1.5 * lat_jerk * ta_tj2 + 0.5 * lat_jerk * ta2_tj); // = l4 + const auto l5 = + sign * (11.0 / 6.0 * lat_jerk * tj3 + 2.5 * lat_jerk * ta_tj2 + 0.5 * lat_jerk * ta2_tj); + const auto l6 = + sign * (11.0 / 6.0 * lat_jerk * tj3 + 3.0 * lat_jerk * ta_tj2 + lat_jerk * ta2_tj); + const auto l7 = sign * (2.0 * lat_jerk * tj3 + 3.0 * lat_jerk * ta_tj2 + lat_jerk * ta2_tj); std::vector base_lon = {0.0, s1, s2, s3, s5, s6, s7}; std::vector base_lat = {0.0, l1, l2, l3, l5, l6, l7}; From 394214966a194116706d61e907f9dc3c4094bc69 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 19 May 2023 01:11:43 +0900 Subject: [PATCH 06/14] feat(lane_change): enable lane change to accelerate Signed-off-by: yutaka --- .../scene_module/lane_change/base_class.hpp | 3 +- .../scene_module/lane_change/normal.hpp | 6 +-- .../utils/lane_change/utils.hpp | 9 +--- .../src/scene_module/lane_change/normal.cpp | 46 ++++++++++--------- .../src/utils/lane_change/utils.cpp | 44 ++++-------------- 5 files changed, 38 insertions(+), 70 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index 827eddbad499d..67aa9b3bfb5ea 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -182,8 +182,7 @@ class LaneChangeBase virtual PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const = 0; + const double backward_path_length, const double prepare_length) const = 0; virtual bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index 9e262a677d365..6869ad22cae54 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -91,8 +91,7 @@ class NormalLaneChange : public LaneChangeBase PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const override; + const double backward_path_length, const double prepare_length) const override; bool getLaneChangePaths( const lanelet::ConstLanelets & original_lanelets, @@ -128,8 +127,7 @@ class NormalLaneChangeBT : public NormalLaneChange PathWithLaneId getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const override; + const double backward_path_length, const double prepare_length) const override; std::vector getDrivableLanes() const override; }; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index eb81c92eb80a8..aa95d90cb3411 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -77,19 +77,14 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); -double calcLaneChangingLength( - const double lane_changing_velocity, const double shift_length, const double lateral_acc, - const double lateral_jerk); - std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & target_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const std::vector> & sorted_lane_ids, const double longitudinal_acceleration, const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length, - const LaneChangePhaseInfo lane_change_velocity, - const BehaviorPathPlannerParameters & common_parameter, - const LaneChangeParameters & lane_change_param); + const LaneChangePhaseInfo lane_change_velocity, const double terminal_lane_changing_velocity, + const LaneChangePhaseInfo lane_change_time); PathSafetyStatus isLaneChangePathSafe( const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 2c51640be6aa6..7230fd766f598 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -371,7 +371,7 @@ int NormalLaneChange::getNumToPreferredLane(const lanelet::ConstLanelet & lane) PathWithLaneId NormalLaneChange::getPrepareSegment( const lanelet::ConstLanelets & current_lanes, [[maybe_unused]] const double arc_length_from_current, const double backward_path_length, - const double prepare_length, const double current_velocity, const double prepare_velocity) const + const double prepare_length) const { if (current_lanes.empty()) { return PathWithLaneId(); @@ -382,8 +382,6 @@ PathWithLaneId NormalLaneChange::getPrepareSegment( prepare_segment.points, getEgoPose(), 3.0, 1.0); utils::clipPathLength(prepare_segment, current_seg_idx, prepare_length, backward_path_length); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); - return prepare_segment; } @@ -473,9 +471,8 @@ bool NormalLaneChange::getLaneChangePaths( break; } - const auto prepare_segment = getPrepareSegment( - original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length, - current_velocity, prepare_velocity); + auto prepare_segment = getPrepareSegment( + original_lanelets, arc_position_from_current.length, backward_path_length, prepare_length); if (prepare_segment.points.empty()) { RCLCPP_DEBUG( @@ -502,21 +499,27 @@ bool NormalLaneChange::getLaneChangePaths( const auto shift_length = lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); - // we assume constant velocity during lane change - const auto lane_changing_velocity = prepare_velocity; + const auto initial_lane_changing_velocity = prepare_velocity; + const double lane_changing_longitudinal_acc = longitudinal_acc > 0.0 ? longitudinal_acc : 0.0; // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = - common_parameter.lane_change_lat_acc_map.find(lane_changing_velocity); + common_parameter.lane_change_lat_acc_map.find(initial_lane_changing_velocity); const auto lateral_acc_resolution = std::abs(max_lateral_acc - min_lateral_acc) / lateral_acc_sampling_num; constexpr double lateral_acc_epsilon = 0.01; for (double lateral_acc = min_lateral_acc; lateral_acc < max_lateral_acc + lateral_acc_epsilon; lateral_acc += lateral_acc_resolution) { - const auto lane_changing_length = utils::lane_change::calcLaneChangingLength( - lane_changing_velocity, shift_length, lateral_acc, - common_parameter.lane_changing_lateral_jerk); + const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( + shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); + const auto lane_changing_length = + initial_lane_changing_velocity * lane_changing_time + + 0.5 * lane_changing_longitudinal_acc * lane_changing_time * lane_changing_time; + const auto terminal_lane_changing_velocity = + initial_lane_changing_velocity + lane_changing_longitudinal_acc * lane_changing_time; + utils::lane_change::setPrepareVelocity( + prepare_segment, current_velocity, terminal_lane_changing_velocity); if (lane_changing_length + prepare_length > dist_to_end_of_current_lanes) { RCLCPP_DEBUG( @@ -543,7 +546,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto target_segment = utils::lane_change::getTargetSegment( route_handler, target_lanelets, forward_path_length, lane_changing_start_pose, - target_lane_length, lane_changing_length, lane_changing_velocity, next_lane_change_buffer); + target_lane_length, lane_changing_length, initial_lane_changing_velocity, + next_lane_change_buffer); if (target_segment.points.empty()) { RCLCPP_DEBUG( @@ -553,7 +557,7 @@ bool NormalLaneChange::getLaneChangePaths( } const auto resample_interval = utils::lane_change::calcLaneChangeResampleInterval( - lane_changing_length, lane_changing_velocity); + lane_changing_length, initial_lane_changing_velocity); const auto lc_length = LaneChangePhaseInfo{prepare_length, lane_changing_length}; const auto target_lane_reference_path = utils::lane_change::getReferencePathFromTargetLane( @@ -571,12 +575,15 @@ bool NormalLaneChange::getLaneChangePaths( const auto shift_line = utils::lane_change::getLaneChangingShiftLine( prepare_segment, target_segment, target_lane_reference_path, shift_length); - const auto lc_velocity = LaneChangePhaseInfo{prepare_velocity, lane_changing_velocity}; + const auto lc_velocity = + LaneChangePhaseInfo{prepare_velocity, initial_lane_changing_velocity}; + + const auto lc_time = LaneChangePhaseInfo{prepare_duration, lane_changing_time}; const auto candidate_path = utils::lane_change::constructCandidatePath( prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, - target_lanelets, sorted_lane_ids, longitudinal_acc, lateral_acc, lc_length, lc_velocity, - common_parameter, *lane_change_parameters_); + target_lanelets, sorted_lane_ids, lane_changing_longitudinal_acc, lateral_acc, lc_length, + lc_velocity, terminal_lane_changing_velocity, lc_time); if (!candidate_path) { RCLCPP_DEBUG( @@ -960,8 +967,7 @@ int NormalLaneChangeBT::getNumToPreferredLane(const lanelet::ConstLanelet & lane PathWithLaneId NormalLaneChangeBT::getPrepareSegment( const lanelet::ConstLanelets & current_lanes, const double arc_length_from_current, - const double backward_path_length, const double prepare_length, const double current_velocity, - const double prepare_velocity) const + const double backward_path_length, const double prepare_length) const { if (current_lanes.empty()) { return PathWithLaneId(); @@ -977,8 +983,6 @@ PathWithLaneId NormalLaneChangeBT::getPrepareSegment( PathWithLaneId prepare_segment = getRouteHandler()->getCenterLinePath(current_lanes, s_start, s_end); - utils::lane_change::setPrepareVelocity(prepare_segment, current_velocity, prepare_velocity); - return prepare_segment; } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 3145bc4ac7394..1ab0ddae0ed0d 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -171,20 +171,20 @@ std::optional constructCandidatePath( const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const std::vector> & sorted_lane_ids, const double longitudinal_acceleration, const double lateral_acceleration, const LaneChangePhaseInfo lane_change_length, - const LaneChangePhaseInfo lane_change_velocity, - const BehaviorPathPlannerParameters & common_parameter, - const LaneChangeParameters & lane_change_param) + const LaneChangePhaseInfo lane_change_velocity, const double terminal_lane_changing_velocity, + const LaneChangePhaseInfo lane_change_time) { PathShifter path_shifter; path_shifter.setPath(target_lane_reference_path); path_shifter.addShiftLine(shift_line); + path_shifter.setLongitudinalAcceleration(longitudinal_acceleration); ShiftedPath shifted_path; // offset front side bool offset_back = false; - const auto lane_changing_velocity = lane_change_velocity.lane_changing; - path_shifter.setVelocity(lane_changing_velocity); + const auto initial_lane_changing_velocity = lane_change_velocity.lane_changing; + path_shifter.setVelocity(initial_lane_changing_velocity); path_shifter.setLateralAccelerationLimit(std::abs(lateral_acceleration)); if (!path_shifter.generate(&shifted_path, offset_back)) { @@ -200,15 +200,8 @@ std::optional constructCandidatePath( candidate_path.acceleration = longitudinal_acceleration; candidate_path.length.prepare = prepare_length; candidate_path.length.lane_changing = lane_changing_length; - candidate_path.duration.prepare = std::invoke([&]() { - const auto duration = prepare_length / lane_change_velocity.prepare; - return std::min(duration, common_parameter.lane_change_prepare_duration); - }); - candidate_path.duration.lane_changing = std::invoke([&]() { - const auto rounding_multiplier = 1.0 / lane_change_param.prediction_time_resolution; - return std::ceil((lane_changing_length / lane_changing_velocity) * rounding_multiplier) / - rounding_multiplier; - }); + candidate_path.duration.prepare = lane_change_time.prepare; + candidate_path.duration.lane_changing = lane_change_time.lane_changing; candidate_path.shift_line = shift_line; candidate_path.reference_lanelets = original_lanelets; candidate_path.target_lanelets = target_lanelets; @@ -220,7 +213,6 @@ std::optional constructCandidatePath( .get_child("constructCandidatePath"), "prepare_length: %f, lane_change: %f", prepare_length, lane_changing_length); - const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); const PathPointWithLaneId & lane_changing_end_point = target_segment.points.front(); const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; const auto lane_change_end_idx = @@ -238,8 +230,7 @@ std::optional constructCandidatePath( if (i < *lane_change_end_idx) { point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); point.point.longitudinal_velocity_mps = std::min( - point.point.longitudinal_velocity_mps, - lane_changing_start_point.point.longitudinal_velocity_mps); + point.point.longitudinal_velocity_mps, static_cast(terminal_lane_changing_velocity)); continue; } const auto nearest_idx = @@ -454,25 +445,6 @@ bool isObjectIndexIncluded( return std::count(dynamic_objects_indices.begin(), dynamic_objects_indices.end(), index) != 0; } -double calcLaneChangingLength( - const double lane_changing_velocity, const double shift_length, const double lateral_acc, - const double lateral_jerk) -{ - const auto required_time = - PathShifter::calcShiftTimeFromJerk(shift_length, lateral_jerk, lateral_acc); - const auto lane_changing_length = lane_changing_velocity * required_time; - - RCLCPP_DEBUG( - rclcpp::get_logger("behavior_path_planner") - .get_child("lane_change") - .get_child("util") - .get_child("calcLaneChangingLength"), - "required_time: %f [s] lane_changing_velocity : %f [m/s], lane_changing_length: %f [m]", - required_time, lane_changing_velocity, lane_changing_length); - - return lane_changing_length; -} - PathWithLaneId getTargetSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const Pose & lane_changing_start_pose, From d21854af67c946bafed996648dc094519a84358a Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 19 May 2023 11:45:12 +0900 Subject: [PATCH 07/14] update Signed-off-by: yutaka --- .../src/utils/path_shifter/path_shifter.cpp | 38 ++++++++++++------- 1 file changed, 24 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 28fe6ceeba6b3..554e0568b4cbb 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -282,13 +282,14 @@ std::pair, std::vector> PathShifter::getBaseLengthsW { const auto & s = arclength; const auto & l = shift_length; - const auto & v = velocity; + const auto & v0 = velocity; const auto & a = longitudinal_acc; const auto & T = total_time; const double t = T / 4; - const double s1 = v * t + 0.5 * a * t * t; - const double s2 = s1 + 2 * v * t + 2 * a * t * t; + const double s1 = v0 * t + 0.5 * a * t * t; + const double v1 = v0 + a * t; + const double s2 = s1 + 2 * v1 * t + 2 * a * t * t; std::vector base_lon = {0.0, s1, s2, s}; std::vector base_lat = {0.0, 1.0 / 12.0 * l, 11.0 / 12.0 * l, l}; @@ -300,22 +301,21 @@ std::pair, std::vector> PathShifter::getBaseLengthsW std::pair, std::vector> PathShifter::calcBaseLengths( const double arclength, const double shift_length, const bool offset_back) const { - const auto speed = std::abs(velocity_); + const auto v0 = std::abs(velocity_); // For longitudinal acceleration, we only consider positive side // negative acceleration (deceleration) is treated as 0.0 const double acc_threshold = 0.0001; const auto & a = longitudinal_acc_ > acc_threshold ? longitudinal_acc_ : 0.0; - if (speed < 1.0e-5 && a < acc_threshold) { + if (v0 < 1.0e-5 && a < acc_threshold) { // no need to consider acceleration limit RCLCPP_DEBUG(logger_, "set velocity is zero. lateral acc limit is ignored"); return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); } const auto L = std::abs(shift_length); - const auto T = - a > acc_threshold ? (-speed + std::sqrt(speed * speed + 2 * a * L)) / a : L / speed; + const auto T = a > acc_threshold ? (-v0 + std::sqrt(v0 * v0 + 2 * a * L)) / a : L / v0; const auto lateral_a_max = 8.0 * L / (T * T); if (lateral_a_max < lateral_acc_limit_) { @@ -323,7 +323,7 @@ std::pair, std::vector> PathShifter::calcBaseLengths RCLCPP_WARN_THROTTLE( logger_, clock_, 3000, "No need to consider lateral acc limit. max: %f, limit: %f", lateral_a_max, lateral_acc_limit_); - return getBaseLengthsWithoutAccelLimit(arclength, shift_length, speed, a, T, offset_back); + return getBaseLengthsWithoutAccelLimit(arclength, shift_length, v0, a, T, offset_back); } const auto tj = T / 2.0 - 2.0 * L / (lateral_acc_limit_ * T); @@ -344,12 +344,22 @@ std::pair, std::vector> PathShifter::calcBaseLengths const auto ta2_tj = ta * ta * tj; const auto ta_tj2 = ta * tj * tj; - const auto s1 = tj * speed + 0.5 * a * tj * tj; - const auto s2 = s1 + ta * speed + 0.5 * a * ta * ta; - const auto s3 = s2 + tj * speed + 0.5 * a * tj * tj; // = s4 - const auto s5 = s3 + tj * speed + 0.5 * a * tj * tj; - const auto s6 = s5 + ta * speed + 0.5 * a * ta * ta; - const auto s7 = s6 + tj * speed + 0.5 * a * tj * tj; + const auto s1 = tj * v0 + 0.5 * a * tj * tj; + const auto v1 = v0 + a * tj; + + const auto s2 = s1 + ta * v1 + 0.5 * a * ta * ta; + const auto v2 = v1 + a * ta; + + const auto s3 = s2 + tj * v2 + 0.5 * a * tj * tj; // = s4 + const auto v3 = v2 + a * tj; + + const auto s5 = s3 + tj * v3 + 0.5 * a * tj * tj; + const auto v5 = v3 + a * tj; + + const auto s6 = s5 + ta * v5 + 0.5 * a * ta * ta; + const auto v6 = v5 + a * ta; + + const auto s7 = s6 + tj * v6 + 0.5 * a * tj * tj; const auto sign = shift_length > 0.0 ? 1.0 : -1.0; const auto l1 = sign * (1.0 / 6.0 * lat_jerk * tj3); From 197af385f3b6553e0d38636c76a1b1d4dc23d4d2 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 19 May 2023 13:24:39 +0900 Subject: [PATCH 08/14] update Signed-off-by: yutaka --- .../utils/lane_change/utils.hpp | 9 +++- .../behavior_path_planner/utils/utils.hpp | 5 -- .../src/scene_module/lane_change/normal.cpp | 8 +++- .../src/utils/lane_change/utils.cpp | 46 +++++++++++++++++-- .../behavior_path_planner/src/utils/utils.cpp | 37 --------------- 5 files changed, 56 insertions(+), 49 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index aa95d90cb3411..49f27dc5c0636 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -92,8 +92,8 @@ PathSafetyStatus isLaneChangePathSafe( const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, const behavior_path_planner::LaneChangeParameters & lane_change_parameter, const double front_decel, const double rear_decel, - std::unordered_map & debug_data, - const double acceleration = 0.0); + std::unordered_map & debug_data, const double prepare_acc = 0.0, + const double lane_changing_acc = 0.0); bool isObjectIndexIncluded( const size_t & index, const std::vector & dynamic_objects_indices); @@ -167,5 +167,10 @@ CandidateOutput assignToCandidate( boost::optional getLaneChangeTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & current_lanes, const LaneChangeModuleType type, const Direction & direction); + +PredictedPath convertToPredictedPath( + const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose, + const size_t nearest_seg_idx, const double duration, const double resolution, + const double prepare_time, const double prepare_acc, const double lane_changing_acc); } // namespace behavior_path_planner::utils::lane_change #endif // BEHAVIOR_PATH_PLANNER__UTILS__LANE_CHANGE__UTILS_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index 87bced439ce46..f9b1b25c5c428 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -107,11 +107,6 @@ struct FrenetPoint }; // data conversions -PredictedPath convertToPredictedPath( - const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & pose, - const size_t nearest_seg_idx, const double duration, const double resolution, - const double prepare_time, const double acceleration); - template FrenetPoint convertToFrenetPoint( const T & points, const Point & search_point_geom, const size_t seg_idx) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 7230fd766f598..c243021e73eba 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -500,7 +500,7 @@ bool NormalLaneChange::getLaneChangePaths( lanelet::utils::getLateralDistanceToClosestLanelet(target_lanelets, lane_changing_start_pose); const auto initial_lane_changing_velocity = prepare_velocity; - const double lane_changing_longitudinal_acc = longitudinal_acc > 0.0 ? longitudinal_acc : 0.0; + const auto & max_path_velocity = prepare_segment.points.back().point.longitudinal_velocity_mps; // get lateral acceleration range const auto [min_lateral_acc, max_lateral_acc] = @@ -513,6 +513,9 @@ bool NormalLaneChange::getLaneChangePaths( lateral_acc += lateral_acc_resolution) { const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); + const double lane_changing_longitudinal_acc = std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + longitudinal_acc); const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + 0.5 * lane_changing_longitudinal_acc * lane_changing_time * lane_changing_time; @@ -619,7 +622,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto [is_safe, is_object_coming_from_rear] = utils::lane_change::isLaneChangePathSafe( *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration, - common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc); + common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc, + lane_changing_longitudinal_acc); if (is_safe) { return true; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 1ab0ddae0ed0d..cb0a201f3dc54 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -307,7 +307,7 @@ PathSafetyStatus isLaneChangePathSafe( const Twist & current_twist, const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & lane_change_parameter, const double front_decel, const double rear_decel, std::unordered_map & debug_data, - const double acceleration) + const double prepare_acc, const double lane_changing_acc) { PathSafetyStatus path_safety_status; @@ -333,9 +333,9 @@ PathSafetyStatus isLaneChangePathSafe( path.points, current_pose, common_parameter.ego_nearest_dist_threshold, common_parameter.ego_nearest_yaw_threshold); - const auto ego_predicted_path = utils::convertToPredictedPath( + const auto ego_predicted_path = convertToPredictedPath( path, current_twist, current_pose, current_seg_idx, check_end_time, time_resolution, - prepare_duration, acceleration); + prepare_duration, prepare_acc, lane_changing_acc); const auto & vehicle_info = common_parameter.vehicle_info; auto in_lane_object_indices = dynamic_objects_indices.target_lane; @@ -996,4 +996,44 @@ boost::optional getLaneChangeTargetLane( return route_handler.getLaneChangeTargetExceptPreferredLane(current_lanes, direction); } + +PredictedPath convertToPredictedPath( + const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, + const size_t nearest_seg_idx, const double duration, const double resolution, + const double prepare_time, const double prepare_acc, const double lane_changing_acc) +{ + PredictedPath predicted_path{}; + predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); + predicted_path.path.reserve(std::min(path.points.size(), static_cast(100))); + + if (path.points.empty()) { + return predicted_path; + } + + FrenetPoint vehicle_pose_frenet = + convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); + const double initial_velocity = std::abs(vehicle_twist.linear.x); + + // prepare segment + for (double t = 0.0; t < prepare_time; t += resolution) { + const double length = initial_velocity * t + 0.5 * prepare_acc * t * t; + predicted_path.path.push_back( + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); + } + + // lane changing segment + const double lane_changing_velocity = + std::max(initial_velocity + prepare_acc * prepare_time, 0.0); + const double offset = + initial_velocity * prepare_time + 0.5 * prepare_acc * prepare_time * prepare_time; + for (double t = prepare_time; t < duration; t += resolution) { + const double delta_t = t - prepare_time; + const double length = + lane_changing_velocity * delta_t + 0.5 * lane_changing_acc * delta_t * delta_t + offset; + predicted_path.path.push_back( + motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); + } + + return predicted_path; +} } // namespace behavior_path_planner::utils::lane_change diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index 2de726ac82315..10e393a6daa3a 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -518,43 +518,6 @@ double l2Norm(const Vector3 vector) return std::sqrt(std::pow(vector.x, 2) + std::pow(vector.y, 2) + std::pow(vector.z, 2)); } -PredictedPath convertToPredictedPath( - const PathWithLaneId & path, const Twist & vehicle_twist, const Pose & vehicle_pose, - const size_t nearest_seg_idx, const double duration, const double resolution, - const double prepare_time, const double acceleration) -{ - PredictedPath predicted_path{}; - predicted_path.time_step = rclcpp::Duration::from_seconds(resolution); - predicted_path.path.reserve(std::min(path.points.size(), static_cast(100))); - - if (path.points.empty()) { - return predicted_path; - } - - FrenetPoint vehicle_pose_frenet = - convertToFrenetPoint(path.points, vehicle_pose.position, nearest_seg_idx); - const double initial_velocity = std::abs(vehicle_twist.linear.x); - const double lane_change_velocity = std::max(initial_velocity + acceleration * prepare_time, 0.0); - - // prepare segment - for (double t = 0.0; t < prepare_time; t += resolution) { - const double length = initial_velocity * t + 0.5 * acceleration * t * t; - predicted_path.path.push_back( - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); - } - - // lane changing segment - const double offset = - initial_velocity * prepare_time + 0.5 * acceleration * prepare_time * prepare_time; - for (double t = prepare_time; t < duration; t += resolution) { - const double length = lane_change_velocity * (t - prepare_time) + offset; - predicted_path.path.push_back( - motion_utils::calcInterpolatedPose(path.points, vehicle_pose_frenet.length + length)); - } - - return predicted_path; -} - double getDistanceBetweenPredictedPaths( const PredictedPath & object_path, const PredictedPath & ego_path, const double start_time, const double end_time, const double resolution) From 75c92a213861f2099d987635ac815375ca48e797 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 19 May 2023 15:05:42 +0900 Subject: [PATCH 09/14] update Signed-off-by: yutaka --- .../src/utils/path_shifter/path_shifter.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 554e0568b4cbb..7a9e7ceb5a125 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -314,8 +314,9 @@ std::pair, std::vector> PathShifter::calcBaseLengths return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); } + const auto & S = arclength; const auto L = std::abs(shift_length); - const auto T = a > acc_threshold ? (-v0 + std::sqrt(v0 * v0 + 2 * a * L)) / a : L / v0; + const auto T = a > acc_threshold ? (-v0 + std::sqrt(v0 * v0 + 2 * a * S)) / a : S / v0; const auto lateral_a_max = 8.0 * L / (T * T); if (lateral_a_max < lateral_acc_limit_) { @@ -323,7 +324,7 @@ std::pair, std::vector> PathShifter::calcBaseLengths RCLCPP_WARN_THROTTLE( logger_, clock_, 3000, "No need to consider lateral acc limit. max: %f, limit: %f", lateral_a_max, lateral_acc_limit_); - return getBaseLengthsWithoutAccelLimit(arclength, shift_length, v0, a, T, offset_back); + return getBaseLengthsWithoutAccelLimit(S, shift_length, v0, a, T, offset_back); } const auto tj = T / 2.0 - 2.0 * L / (lateral_acc_limit_ * T); @@ -337,7 +338,7 @@ std::pair, std::vector> PathShifter::calcBaseLengths logger_, clock_, 3000, "Acc limit is too small to be applied. Tj: %f, Ta: %f, j: %f, a_max: %f, acc_limit: %f", tj, ta, lat_jerk, lateral_a_max, lateral_acc_limit_); - return getBaseLengthsWithoutAccelLimit(arclength, shift_length, offset_back); + return getBaseLengthsWithoutAccelLimit(S, shift_length, offset_back); } const auto tj3 = tj * tj * tj; From 36c6123b395cf3016b6fc6eca87fc2b34b334cbf Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 19 May 2023 17:32:00 +0900 Subject: [PATCH 10/14] udpate --- .../src/scene_module/lane_change/normal.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index c243021e73eba..6a5e9b8fd54cd 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -515,7 +515,7 @@ bool NormalLaneChange::getLaneChangePaths( shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); const double lane_changing_longitudinal_acc = std::clamp( (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, - longitudinal_acc); + common_parameter.max_acc); const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + 0.5 * lane_changing_longitudinal_acc * lane_changing_time * lane_changing_time; From 4dcce8e6ba965dc5008aa42fabe6dae3d80bb567 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 19 May 2023 18:13:30 +0900 Subject: [PATCH 11/14] update Signed-off-by: yutaka --- .../src/utils/path_shifter/path_shifter.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp index 7a9e7ceb5a125..c1058969434d9 100644 --- a/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp +++ b/planning/behavior_path_planner/src/utils/path_shifter/path_shifter.cpp @@ -287,9 +287,9 @@ std::pair, std::vector> PathShifter::getBaseLengthsW const auto & T = total_time; const double t = T / 4; - const double s1 = v0 * t + 0.5 * a * t * t; + const double s1 = std::min(v0 * t + 0.5 * a * t * t, s); const double v1 = v0 + a * t; - const double s2 = s1 + 2 * v1 * t + 2 * a * t * t; + const double s2 = std::min(s1 + 2 * v1 * t + 2 * a * t * t, s); std::vector base_lon = {0.0, s1, s2, s}; std::vector base_lat = {0.0, 1.0 / 12.0 * l, 11.0 / 12.0 * l, l}; @@ -345,22 +345,22 @@ std::pair, std::vector> PathShifter::calcBaseLengths const auto ta2_tj = ta * ta * tj; const auto ta_tj2 = ta * tj * tj; - const auto s1 = tj * v0 + 0.5 * a * tj * tj; + const auto s1 = std::min(tj * v0 + 0.5 * a * tj * tj, S); const auto v1 = v0 + a * tj; - const auto s2 = s1 + ta * v1 + 0.5 * a * ta * ta; + const auto s2 = std::min(s1 + ta * v1 + 0.5 * a * ta * ta, S); const auto v2 = v1 + a * ta; - const auto s3 = s2 + tj * v2 + 0.5 * a * tj * tj; // = s4 + const auto s3 = std::min(s2 + tj * v2 + 0.5 * a * tj * tj, S); // = s4 const auto v3 = v2 + a * tj; - const auto s5 = s3 + tj * v3 + 0.5 * a * tj * tj; + const auto s5 = std::min(s3 + tj * v3 + 0.5 * a * tj * tj, S); const auto v5 = v3 + a * tj; - const auto s6 = s5 + ta * v5 + 0.5 * a * ta * ta; + const auto s6 = std::min(s5 + ta * v5 + 0.5 * a * ta * ta, S); const auto v6 = v5 + a * ta; - const auto s7 = s6 + tj * v6 + 0.5 * a * tj * tj; + const auto s7 = std::min(s6 + tj * v6 + 0.5 * a * tj * tj, S); const auto sign = shift_length > 0.0 ? 1.0 : -1.0; const auto l1 = sign * (1.0 / 6.0 * lat_jerk * tj3); From 29571ca965625d5cfdfe1e1e7681cf04f890206a Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 19 May 2023 18:47:52 +0900 Subject: [PATCH 12/14] update Signed-off-by: yutaka --- .../config/lane_change/lane_change.param.yaml | 4 ++++ .../lane_change/lane_change_module_data.hpp | 4 ++++ .../utils/lane_change/utils.hpp | 2 +- .../src/behavior_path_planner_node.cpp | 4 ++++ .../src/scene_module/lane_change/normal.cpp | 22 +++++++++++-------- .../src/utils/lane_change/utils.cpp | 7 +++--- 6 files changed, 29 insertions(+), 14 deletions(-) diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 031ddc08fba1d..a8631128d8abd 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -15,6 +15,10 @@ longitudinal_acceleration_sampling_num: 5 lateral_acceleration_sampling_num: 3 + # longitudinal acceleration + min_longitudinal_acc: -1.0 + max_longitudinal_acc: 1.0 + # lateral acceleration map lateral_acceleration: velocity: [0.0, 4.0, 10.0] diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 2b5e11238383c..8c25b7873d8db 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -35,6 +35,10 @@ struct LaneChangeParameters int longitudinal_acc_sampling_num{10}; int lateral_acc_sampling_num{10}; + // acceleration data + double min_longitudinal_acc{-1.0}; + double max_longitudinal_acc{1.0}; + // collision check bool enable_prepare_segment_collision_check{true}; double prepare_segment_ignore_object_velocity_thresh{0.1}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 49f27dc5c0636..40004d1315655 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -55,7 +55,7 @@ double calcLaneChangeResampleInterval( double calcMaximumAcceleration( const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, - const BehaviorPathPlannerParameters & params); + const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 407f2d853f88a..51650803f8be8 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -749,6 +749,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.lateral_acc_sampling_num = declare_parameter(parameter("lateral_acceleration_sampling_num")); + // acceleration + p.min_longitudinal_acc = declare_parameter(parameter("min_longitudinal_acc")); + p.max_longitudinal_acc = declare_parameter(parameter("max_longitudinal_acc")); + // collision check p.enable_prepare_segment_collision_check = declare_parameter(parameter("enable_prepare_segment_collision_check")); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 6a5e9b8fd54cd..9a5ea9cc8d831 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -404,20 +404,24 @@ bool NormalLaneChange::getLaneChangePaths( const auto minimum_lane_changing_velocity = common_parameter.minimum_lane_changing_velocity; const auto longitudinal_acc_sampling_num = lane_change_parameters_->longitudinal_acc_sampling_num; const auto lateral_acc_sampling_num = lane_change_parameters_->lateral_acc_sampling_num; + const auto min_longitudinal_acc = + std::max(common_parameter.min_acc, lane_change_parameters_->min_longitudinal_acc); + const auto max_longitudinal_acc = + std::min(common_parameter.max_acc, lane_change_parameters_->max_longitudinal_acc); // get velocity const auto current_velocity = getEgoTwist().linear.x; // compute maximum longitudinal deceleration and acceleration - const auto maximum_deceleration = - std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, &common_parameter, this]() { - const double min_a = (minimum_lane_changing_velocity - current_velocity) / - common_parameter.lane_change_prepare_duration; - return std::clamp( - min_a, -std::abs(common_parameter.min_acc), -std::numeric_limits::epsilon()); - }); + const auto maximum_deceleration = std::invoke([&minimum_lane_changing_velocity, ¤t_velocity, + &min_longitudinal_acc, &common_parameter, this]() { + const double min_a = (minimum_lane_changing_velocity - current_velocity) / + common_parameter.lane_change_prepare_duration; + return std::clamp( + min_a, -std::abs(min_longitudinal_acc), -std::numeric_limits::epsilon()); + }); const auto maximum_acceleration = utils::lane_change::calcMaximumAcceleration( - prev_module_path_, getEgoPose(), current_velocity, common_parameter); + prev_module_path_, getEgoPose(), current_velocity, max_longitudinal_acc, common_parameter); // get sampling acceleration values const auto longitudinal_acc_sampling_values = utils::lane_change::getAccelerationValues( @@ -515,7 +519,7 @@ bool NormalLaneChange::getLaneChangePaths( shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); const double lane_changing_longitudinal_acc = std::clamp( (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, - common_parameter.max_acc); + max_longitudinal_acc); const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + 0.5 * lane_changing_longitudinal_acc * lane_changing_time * lane_changing_time; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index cb0a201f3dc54..58414de715111 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -63,11 +63,10 @@ double calcLaneChangeResampleInterval( double calcMaximumAcceleration( const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, - const BehaviorPathPlannerParameters & params) + const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params) { - const double & max_acc = params.max_acc; if (path.points.empty()) { - return max_acc; + return max_longitudinal_acc; } const double & nearest_dist_threshold = params.ego_nearest_dist_threshold; @@ -80,7 +79,7 @@ double calcMaximumAcceleration( const double & prepare_duration = params.lane_change_prepare_duration; const double acc = (max_path_velocity - current_velocity) / prepare_duration; - return std::clamp(acc, 0.0, max_acc); + return std::clamp(acc, 0.0, max_longitudinal_acc); } void setPrepareVelocity( From 284779fdfb16ffc468293966dbed6206aed06a32 Mon Sep 17 00:00:00 2001 From: yutaka Date: Sat, 20 May 2023 13:38:10 +0900 Subject: [PATCH 13/14] update Signed-off-by: yutaka --- .../utils/lane_change/utils.hpp | 4 ++++ .../src/scene_module/lane_change/normal.cpp | 13 ++++++------- .../src/utils/lane_change/utils.cpp | 13 +++++++++++++ 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp index 40004d1315655..bea70e01e239f 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/utils.hpp @@ -57,6 +57,10 @@ double calcMaximumAcceleration( const PathWithLaneId & path, const Pose & current_pose, const double current_velocity, const double max_longitudinal_acc, const BehaviorPathPlannerParameters & params); +double calcLaneChangingAcceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc); + void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 9a5ea9cc8d831..7e66839b49f32 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -517,14 +517,13 @@ bool NormalLaneChange::getLaneChangePaths( lateral_acc += lateral_acc_resolution) { const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); - const double lane_changing_longitudinal_acc = std::clamp( - (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, - max_longitudinal_acc); + const double lane_changing_lon_acc = utils::lane_change::calcLaneChangingAcceleration( + initial_lane_changing_velocity, max_path_velocity, lane_changing_time, longitudinal_acc); const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + - 0.5 * lane_changing_longitudinal_acc * lane_changing_time * lane_changing_time; + 0.5 * lane_changing_lon_acc * lane_changing_time * lane_changing_time; const auto terminal_lane_changing_velocity = - initial_lane_changing_velocity + lane_changing_longitudinal_acc * lane_changing_time; + initial_lane_changing_velocity + lane_changing_lon_acc * lane_changing_time; utils::lane_change::setPrepareVelocity( prepare_segment, current_velocity, terminal_lane_changing_velocity); @@ -589,7 +588,7 @@ bool NormalLaneChange::getLaneChangePaths( const auto candidate_path = utils::lane_change::constructCandidatePath( prepare_segment, target_segment, target_lane_reference_path, shift_line, original_lanelets, - target_lanelets, sorted_lane_ids, lane_changing_longitudinal_acc, lateral_acc, lc_length, + target_lanelets, sorted_lane_ids, lane_changing_lon_acc, lateral_acc, lc_length, lc_velocity, terminal_lane_changing_velocity, lc_time); if (!candidate_path) { @@ -627,7 +626,7 @@ bool NormalLaneChange::getLaneChangePaths( *candidate_path, dynamic_objects, dynamic_object_indices, getEgoPose(), getEgoTwist(), common_parameter, *lane_change_parameters_, common_parameter.expected_front_deceleration, common_parameter.expected_rear_deceleration, object_debug_, longitudinal_acc, - lane_changing_longitudinal_acc); + lane_changing_lon_acc); if (is_safe) { return true; diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 58414de715111..a6525cebaa19b 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -82,6 +82,19 @@ double calcMaximumAcceleration( return std::clamp(acc, 0.0, max_longitudinal_acc); } +double calcLaneChangingAcceleration( + const double initial_lane_changing_velocity, const double max_path_velocity, + const double lane_changing_time, const double prepare_longitudinal_acc) +{ + if (prepare_longitudinal_acc <= 0.0) { + return 0.0; + } + + return std::clamp( + (max_path_velocity - initial_lane_changing_velocity) / lane_changing_time, 0.0, + prepare_longitudinal_acc); +} + void setPrepareVelocity( PathWithLaneId & prepare_segment, const double current_velocity, const double prepare_velocity) { From ed67019bf288efaa7e7ae79efaf72f030cb6dbad Mon Sep 17 00:00:00 2001 From: yutaka Date: Sat, 20 May 2023 22:00:07 +0900 Subject: [PATCH 14/14] update Signed-off-by: yutaka --- .../src/scene_module/lane_change/normal.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index 7e66839b49f32..e380993cc01c1 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -518,7 +518,8 @@ bool NormalLaneChange::getLaneChangePaths( const auto lane_changing_time = PathShifter::calcShiftTimeFromJerk( shift_length, common_parameter.lane_changing_lateral_jerk, lateral_acc); const double lane_changing_lon_acc = utils::lane_change::calcLaneChangingAcceleration( - initial_lane_changing_velocity, max_path_velocity, lane_changing_time, longitudinal_acc); + initial_lane_changing_velocity, max_path_velocity, lane_changing_time, + sampled_longitudinal_acc); const auto lane_changing_length = initial_lane_changing_velocity * lane_changing_time + 0.5 * lane_changing_lon_acc * lane_changing_time * lane_changing_time;