Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

fix(behavior_path_planner): lane change candidate resolution #2426

Merged
merged 6 commits into from
Dec 12, 2022
Merged

fix(behavior_path_planner): lane change candidate resolution #2426

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
3189fe4
fix(behavior_path_planner): lane change candidate resolution
zulfaqar-azmi-t4 Dec 1, 2022
aa06772
rework sampling based on current speed
zulfaqar-azmi-t4 Dec 1, 2022
a09c909
refactor code
zulfaqar-azmi-t4 Dec 1, 2022
321e6b0
use util's resampler
zulfaqar-azmi-t4 Dec 2, 2022
eb6daf7
consider min_resampling_points and resampling dt
zulfaqar-azmi-t4 Dec 9, 2022
3a284e3
simplify code
zulfaqar-azmi-t4 Dec 9, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 3 additions & 2 deletions ...ing/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 lane_changing_speed);

PathWithLaneId getReferencePathFromTargetLane(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes,
Expand Down
3 changes: 1 addition & 2 deletions planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
17 changes: 13 additions & 4 deletions planning/behavior_path_planner/src/scene_module/lane_change/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"

Expand Down Expand Up @@ -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_distance, forward_path_length, lane_changing_speed);

const ShiftLine shift_line = getLaneChangeShiftLine(
prepare_segment_reference, lane_changing_segment_reference, target_lanelets,
Expand Down Expand Up @@ -490,8 +491,9 @@ 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 lane_changing_speed)
{
const ArcCoordinates lane_change_start_arc_position =
lanelet::utils::getArcCoordinates(target_lanes, lane_changing_start_pose);
Expand All @@ -502,7 +504,14 @@ 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);

constexpr auto min_resampling_points{30.0};
constexpr auto resampling_dt{0.2};
const auto resample_interval =
std::max(lane_changing_distance / min_resampling_points, lane_changing_speed * resampling_dt);
return util::resamplePathWithSpline(lane_changing_reference_path, resample_interval);
}

PathWithLaneId getReferencePathFromTargetLane(
Expand Down