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

Jump to bottom

fix(lane_change): use previous output #3059

Merged
merged 1 commit into from
Mar 13, 2023
Merged

fix(lane_change): use previous output #3059

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
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
13 changes: 13 additions & 0 deletions planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,14 @@ std::pair<bool, bool> getLaneChangePaths(
const double check_distance, LaneChangePaths * candidate_paths,
std::unordered_map<std::string, CollisionCheckDebug> * debug_data);

std::pair<bool, bool> getLaneChangePaths(
const PathWithLaneId & original_path, const RouteHandler & route_handler,
const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects,
const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter,
const double check_distance, LaneChangePaths * candidate_paths,
std::unordered_map<std::string, CollisionCheckDebug> * debug_data);

bool isLaneChangePathSafe(
const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects,
const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose,
Expand Down Expand Up @@ -108,6 +116,11 @@ PathWithLaneId getLaneChangePathPrepareSegment(
const double arc_length_from_current, const double backward_path_length,
const double prepare_distance, const double prepare_speed);

PathWithLaneId getLaneChangePathPrepareSegment(
const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets,
const Pose & current_pose, const double backward_path_length, const double prepare_distance,
const double prepare_speed);

PathWithLaneId getLaneChangePathLaneChangingSegment(
const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets,
const double forward_path_length, const double arc_length_from_target,
Expand Down
7 changes: 7 additions & 0 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 @@ -437,10 +437,17 @@ std::pair<bool, bool> LaneChangeModule::getSafePath(

// find candidate paths
LaneChangePaths valid_paths;
#ifdef USE_OLD_ARCHITECTURE
const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths(
*route_handler, current_lanes, lane_change_lanes, current_pose, current_twist,
planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths,
&object_debug_);
#else
const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths(
*getPreviousModuleOutput().path, *route_handler, current_lanes, lane_change_lanes, current_pose,
current_twist, planner_data_->dynamic_object, common_parameters, *parameters_, check_distance,
&valid_paths, &object_debug_);
#endif
debug_valid_path_ = valid_paths;

if (parameters_->publish_debug_marker) {
Expand Down
40 changes: 39 additions & 1 deletion planning/behavior_path_planner/src/util/lane_change/util.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -268,13 +268,23 @@ std::optional<LaneChangePath> constructCandidatePath(
return std::optional<LaneChangePath>{candidate_path};
}

#ifdef USE_OLD_ARCHITECTURE
std::pair<bool, bool> getLaneChangePaths(
const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets,
const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist,
const PredictedObjects::ConstSharedPtr dynamic_objects,
const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter,
const double check_distance, LaneChangePaths * candidate_paths,
std::unordered_map<std::string, CollisionCheckDebug> * debug_data)
#else
std::pair<bool, bool> getLaneChangePaths(
const PathWithLaneId & original_path, const RouteHandler & route_handler,
const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets,
const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects,
const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter,
const double check_distance, LaneChangePaths * candidate_paths,
std::unordered_map<std::string, CollisionCheckDebug> * debug_data)
#endif
{
debug_data->clear();
if (original_lanelets.empty() || target_lanelets.empty()) {
Expand Down Expand Up @@ -320,7 +330,8 @@ std::pair<bool, bool> getLaneChangePaths(
const auto required_total_min_distance =
util::calcLaneChangeBuffer(common_parameter, num_to_preferred_lane);

const auto arc_position_from_current = lanelet::utils::getArcCoordinates(original_lanelets, pose);
[[maybe_unused]] const auto arc_position_from_current =
lanelet::utils::getArcCoordinates(original_lanelets, pose);
const auto arc_position_from_target = lanelet::utils::getArcCoordinates(target_lanelets, pose);

const auto target_lane_length = lanelet::utils::getLaneletLength2d(target_lanelets);
Expand Down Expand Up @@ -352,9 +363,15 @@ std::pair<bool, bool> getLaneChangePaths(
continue;
}

#ifdef USE_OLD_ARCHITECTURE
const auto prepare_segment_reference = getLaneChangePathPrepareSegment(
route_handler, original_lanelets, arc_position_from_current.length, backward_path_length,
prepare_distance, std::max(prepare_speed, minimum_lane_change_velocity));
#else
const auto prepare_segment_reference = getLaneChangePathPrepareSegment(
original_path, original_lanelets, pose, backward_path_length, prepare_distance,
std::max(prepare_speed, minimum_lane_change_velocity));
#endif

const auto estimated_shift_length = lanelet::utils::getArcCoordinates(
target_lanelets, prepare_segment_reference.points.front().point.pose);
Expand Down Expand Up @@ -667,6 +684,27 @@ PathWithLaneId getLaneChangePathPrepareSegment(
return prepare_segment;
}

PathWithLaneId getLaneChangePathPrepareSegment(
const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets,
const Pose & current_pose, const double backward_path_length, const double prepare_distance,
const double prepare_speed)
{
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);
util::clipPathLength(prepare_segment, current_seg_idx, prepare_distance, backward_path_length);

prepare_segment.points.back().point.longitudinal_velocity_mps = std::min(
prepare_segment.points.back().point.longitudinal_velocity_mps,
static_cast<float>(prepare_speed));

return prepare_segment;
}

std::pair<double, double> calcLaneChangingSpeedAndDistanceWhenDecelerate(
const double velocity, const double shift_length, const double deceleration,
const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param,
Expand Down