From 3189fe47b570289981daa4e287fbf6fcea7a8ff4 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 1 Dec 2022 18:04:19 +0900 Subject: [PATCH 1/6] fix(behavior_path_planner): lane change candidate resolution Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 63 ++++++++++++++++++- 1 file changed, 62 insertions(+), 1 deletion(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 9b3e65ec92a87..ec8a261c1c586 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -502,7 +502,68 @@ PathWithLaneId getReferencePathFromTargetLane( lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); s_end = std::min(s_end, goal_arc_coordinates.length); } - return route_handler.getCenterLinePath(target_lanes, s_start, s_end); + const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); + + const auto & ref_points = lane_changing_reference_path.points; + + PathPointWithLaneId ref_back_pose; + PathWithLaneId interpolated_path; + interpolated_path.points.push_back(ref_points.front()); + std::vector sum_segments{0.0}; + + for (size_t idx = 1; idx < ref_points.size() - 1; ++idx) { + const auto pt0 = ref_points.at(idx - 1); + const auto pt1 = ref_points.at(idx); + + ref_back_pose = pt1; + const auto sum_segment = sum_segments.back() + tier4_autoware_utils::calcDistance2d(pt0, pt1); + sum_segments.push_back(sum_segment); + + if (sum_segment > lane_changing_distance) { + break; + } + } + + const auto min_speed = 1.0; + const auto deceleration = -1.0; + const auto dt = 0.5; + + double sum_interval{0.0}; + double prev_speed = 0.0; + double sum_duration = dt; + size_t ref_point_idx = 0; + const auto path = util::convertToGeometryPointArray(lane_changing_reference_path); + + while (sum_interval < lane_changing_distance) { + const auto current_speed = prev_speed + deceleration * sum_duration; + + const auto traveled_dist = std::invoke([&]() { + constexpr auto max_interval_resolution = 1.0; + double dist = (current_speed < min_speed) + ? (min_speed * dt) + : (current_speed * dt + 0.5 * deceleration * std::pow(dt, 2)); + return std::max(dist, max_interval_resolution); + }); + + sum_interval += traveled_dist; + const auto lerped_pose = util::lerpByLength(lane_changing_reference_path.points, sum_interval); + + if (sum_interval > sum_segments.at(ref_point_idx) && ref_point_idx < sum_segments.size()) { + ++ref_point_idx; + } + PathPointWithLaneId pt = ref_points.at(ref_point_idx); + pt.point.pose.position = lerped_pose; + interpolated_path.points.push_back(pt); + + prev_speed = current_speed; + sum_duration += dt; + } + + PathWithLaneId path_back; + path_back.points.insert( + path_back.points.end(), ref_points.begin() + ref_point_idx - 1, ref_points.end()); + + return combineReferencePath(interpolated_path, path_back); } PathWithLaneId getReferencePathFromTargetLane( From aa067729f027f122f3b04ba2b965ab5f6fcf88e7 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 1 Dec 2022 20:32:43 +0900 Subject: [PATCH 2/6] rework sampling based on current speed Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/util.hpp | 5 ++-- .../src/scene_module/lane_change/util.cpp | 26 +++++++------------ 2 files changed, 13 insertions(+), 18 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 9ce619c189146..0cce35415eaa1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -110,8 +110,9 @@ ShiftLine getLaneChangeShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double & prepare_distance, - const double & lane_changing_distance, const double & forward_path_length); + const Pose & lane_changing_start_pose, const double prepare_distance, + const double lane_changing_distance, const double forward_path_length, + const double current_speed); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index ec8a261c1c586..e880c3852e23e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -237,7 +237,7 @@ LaneChangePaths getLaneChangePaths( const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length); + lane_changing_distance, forward_path_length, lane_changing_speed); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, @@ -490,8 +490,8 @@ bool isLaneChangePathSafe( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, - const Pose & lane_changing_start_pose, const double & prepare_distance, - const double & lane_changing_distance, const double & forward_path_length) + const Pose & lane_changing_start_pose, const double prepare_distance, + const double lane_changing_distance, const double forward_path_length, const double current_speed) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -525,38 +525,32 @@ PathWithLaneId getReferencePathFromTargetLane( } const auto min_speed = 1.0; - const auto deceleration = -1.0; const auto dt = 0.5; double sum_interval{0.0}; - double prev_speed = 0.0; - double sum_duration = dt; + double prev_speed = current_speed; size_t ref_point_idx = 0; const auto path = util::convertToGeometryPointArray(lane_changing_reference_path); while (sum_interval < lane_changing_distance) { - const auto current_speed = prev_speed + deceleration * sum_duration; - const auto traveled_dist = std::invoke([&]() { constexpr auto max_interval_resolution = 1.0; - double dist = (current_speed < min_speed) - ? (min_speed * dt) - : (current_speed * dt + 0.5 * deceleration * std::pow(dt, 2)); - return std::max(dist, max_interval_resolution); + const double dist = (prev_speed < min_speed) ? (min_speed * dt) : (prev_speed * dt); + return std::min(dist, max_interval_resolution); }); sum_interval += traveled_dist; const auto lerped_pose = util::lerpByLength(lane_changing_reference_path.points, sum_interval); + std::cerr << tier4_autoware_utils::calcDistance2d(lerped_pose, interpolated_path.points.back()) + << '\n'; - if (sum_interval > sum_segments.at(ref_point_idx) && ref_point_idx < sum_segments.size()) { + if (sum_interval > sum_segments.at(ref_point_idx)) { ++ref_point_idx; } + PathPointWithLaneId pt = ref_points.at(ref_point_idx); pt.point.pose.position = lerped_pose; interpolated_path.points.push_back(pt); - - prev_speed = current_speed; - sum_duration += dt; } PathWithLaneId path_back; From a09c9095bf85f4b6ed41c5496bb633a1139de543 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Thu, 1 Dec 2022 20:46:59 +0900 Subject: [PATCH 3/6] refactor code Signed-off-by: Muhammad Zulfaqar Azmi --- .../src/scene_module/lane_change/util.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index e880c3852e23e..59d1d0ca1b33e 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -502,13 +502,12 @@ PathWithLaneId getReferencePathFromTargetLane( lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); s_end = std::min(s_end, goal_arc_coordinates.length); } - const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); + const auto lane_changing_reference_path = + route_handler.getCenterLinePath(target_lanes, s_start, s_end); const auto & ref_points = lane_changing_reference_path.points; PathPointWithLaneId ref_back_pose; - PathWithLaneId interpolated_path; - interpolated_path.points.push_back(ref_points.front()); std::vector sum_segments{0.0}; for (size_t idx = 1; idx < ref_points.size() - 1; ++idx) { @@ -528,28 +527,26 @@ PathWithLaneId getReferencePathFromTargetLane( const auto dt = 0.5; double sum_interval{0.0}; - double prev_speed = current_speed; size_t ref_point_idx = 0; const auto path = util::convertToGeometryPointArray(lane_changing_reference_path); + PathWithLaneId interpolated_path; + interpolated_path.points.push_back(ref_points.front()); + while (sum_interval < lane_changing_distance) { const auto traveled_dist = std::invoke([&]() { constexpr auto max_interval_resolution = 1.0; - const double dist = (prev_speed < min_speed) ? (min_speed * dt) : (prev_speed * dt); + const double dist = (current_speed < min_speed) ? (min_speed * dt) : (current_speed * dt); return std::min(dist, max_interval_resolution); }); sum_interval += traveled_dist; - const auto lerped_pose = util::lerpByLength(lane_changing_reference_path.points, sum_interval); - std::cerr << tier4_autoware_utils::calcDistance2d(lerped_pose, interpolated_path.points.back()) - << '\n'; - if (sum_interval > sum_segments.at(ref_point_idx)) { ++ref_point_idx; } PathPointWithLaneId pt = ref_points.at(ref_point_idx); - pt.point.pose.position = lerped_pose; + pt.point.pose.position = util::lerpByLength(lane_changing_reference_path.points, sum_interval); interpolated_path.points.push_back(pt); } From 321e6b012d703e9abe83b24c1bcfd0977591666d Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Azmi Date: Fri, 2 Dec 2022 10:34:13 +0900 Subject: [PATCH 4/6] use util's resampler Signed-off-by: Muhammad Zulfaqar Azmi --- .../scene_module/lane_change/util.hpp | 3 +- .../src/scene_module/lane_change/util.cpp | 56 ++----------------- 2 files changed, 6 insertions(+), 53 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index 0cce35415eaa1..d462e3c19c887 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -111,8 +111,7 @@ ShiftLine getLaneChangeShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double prepare_distance, - const double lane_changing_distance, const double forward_path_length, - const double current_speed); + const double lane_changing_distance, const double forward_path_length); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index 59d1d0ca1b33e..b2f40ab99edf6 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/scene_module/lane_change/util.hpp" +#include "behavior_path_planner/path_utilities.hpp" #include "behavior_path_planner/scene_module/utils/path_shifter.hpp" #include "behavior_path_planner/utilities.hpp" @@ -237,7 +238,7 @@ LaneChangePaths getLaneChangePaths( const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length, lane_changing_speed); + lane_changing_distance, forward_path_length); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, @@ -491,7 +492,7 @@ bool isLaneChangePathSafe( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double prepare_distance, - const double lane_changing_distance, const double forward_path_length, const double current_speed) + const double lane_changing_distance, const double forward_path_length) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -505,56 +506,9 @@ PathWithLaneId getReferencePathFromTargetLane( const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); - const auto & ref_points = lane_changing_reference_path.points; + constexpr double resample_interval{1.0}; - PathPointWithLaneId ref_back_pose; - std::vector sum_segments{0.0}; - - for (size_t idx = 1; idx < ref_points.size() - 1; ++idx) { - const auto pt0 = ref_points.at(idx - 1); - const auto pt1 = ref_points.at(idx); - - ref_back_pose = pt1; - const auto sum_segment = sum_segments.back() + tier4_autoware_utils::calcDistance2d(pt0, pt1); - sum_segments.push_back(sum_segment); - - if (sum_segment > lane_changing_distance) { - break; - } - } - - const auto min_speed = 1.0; - const auto dt = 0.5; - - double sum_interval{0.0}; - size_t ref_point_idx = 0; - const auto path = util::convertToGeometryPointArray(lane_changing_reference_path); - - PathWithLaneId interpolated_path; - interpolated_path.points.push_back(ref_points.front()); - - while (sum_interval < lane_changing_distance) { - const auto traveled_dist = std::invoke([&]() { - constexpr auto max_interval_resolution = 1.0; - const double dist = (current_speed < min_speed) ? (min_speed * dt) : (current_speed * dt); - return std::min(dist, max_interval_resolution); - }); - - sum_interval += traveled_dist; - if (sum_interval > sum_segments.at(ref_point_idx)) { - ++ref_point_idx; - } - - PathPointWithLaneId pt = ref_points.at(ref_point_idx); - pt.point.pose.position = util::lerpByLength(lane_changing_reference_path.points, sum_interval); - interpolated_path.points.push_back(pt); - } - - PathWithLaneId path_back; - path_back.points.insert( - path_back.points.end(), ref_points.begin() + ref_point_idx - 1, ref_points.end()); - - return combineReferencePath(interpolated_path, path_back); + return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval); } PathWithLaneId getReferencePathFromTargetLane( From eb6daf7205a8b88bc839ba8b9d855871dd72891b Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Date: Fri, 9 Dec 2022 18:51:46 +0900 Subject: [PATCH 5/6] consider min_resampling_points and resampling dt Signed-off-by: Muhammad Zulfaqar --- .../scene_module/lane_change/util.hpp | 3 ++- .../scene_module/lane_change/lane_change_module.cpp | 3 +-- .../src/scene_module/lane_change/util.cpp | 12 ++++++++---- 3 files changed, 11 insertions(+), 7 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp index d462e3c19c887..1e15ee93a7d19 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp @@ -111,7 +111,8 @@ ShiftLine getLaneChangeShiftLine( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double prepare_distance, - const double lane_changing_distance, const double forward_path_length); + const double lane_changing_distance, const double forward_path_length, + const double lane_changing_speed); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index afb1997d01e74..9cd4759ac5d90 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -143,8 +143,7 @@ BT::NodeStatus LaneChangeModule::updateState() BehaviorModuleOutput LaneChangeModule::plan() { - constexpr double resample_interval{1.0}; - auto path = util::resamplePathWithSpline(status_.lane_change_path.path, resample_interval); + auto path = status_.lane_change_path.path; if (!isValidPath(path)) { status_.is_safe = false; diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index b2f40ab99edf6..e1c8c2162c59a 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -238,7 +238,8 @@ LaneChangePaths getLaneChangePaths( const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length); + lane_changing_distance, forward_path_length, + std::max(lane_changing_speed, minimum_lane_change_velocity)); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, @@ -492,7 +493,8 @@ bool isLaneChangePathSafe( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double prepare_distance, - const double lane_changing_distance, const double forward_path_length) + const double lane_changing_distance, const double forward_path_length, + const double lane_changing_speed) { const ArcCoordinates lane_change_start_arc_position = lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose); @@ -506,8 +508,10 @@ PathWithLaneId getReferencePathFromTargetLane( const auto lane_changing_reference_path = route_handler.getCenterLinePath(target_lanes, s_start, s_end); - constexpr double resample_interval{1.0}; - + constexpr auto min_resampling_points{30.0}; + constexpr auto resampling_dt{0.2}; + const auto resample_interval = + std::min(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval); } From 3a284e34c8013f5a71427b3b9354c9694b1dc404 Mon Sep 17 00:00:00 2001 From: Muhammad Zulfaqar Date: Fri, 9 Dec 2022 19:29:04 +0900 Subject: [PATCH 6/6] simplify code Signed-off-by: Muhammad Zulfaqar --- .../src/scene_module/lane_change/util.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp index e1c8c2162c59a..11e91e14a4ac0 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/util.cpp @@ -238,8 +238,7 @@ LaneChangePaths getLaneChangePaths( const PathWithLaneId target_lane_reference_path = getReferencePathFromTargetLane( route_handler, target_lanelets, lane_changing_start_pose, prepare_distance, - lane_changing_distance, forward_path_length, - std::max(lane_changing_speed, minimum_lane_change_velocity)); + lane_changing_distance, forward_path_length, lane_changing_speed); const ShiftLine shift_line = getLaneChangeShiftLine( prepare_segment_reference, lane_changing_segment_reference, target_lanelets, @@ -511,7 +510,7 @@ PathWithLaneId getReferencePathFromTargetLane( constexpr auto min_resampling_points{30.0}; constexpr auto resampling_dt{0.2}; const auto resample_interval = - std::min(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); + std::max(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt); return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval); }