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(lane_change): do not cut abort path #1009

Merged
merged 3 commits into from
Nov 8, 2023
Merged

fix(lane_change): do not cut abort path #1009

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
f2db3a2
feat(behavior_path_planner): add virtual stop wall for lane change (#…
rej55 Aug 31, 2023
a560d2b
fix(lane_change): do not cut abort path (#5509)
rej55 Nov 7, 2023
b6aea0e
fix(lane_change): fix namespace
rej55 Nov 8, 2023
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
Empty file added localization/yabloc/COLCON_IGNORE
View file
Open in desktop
Empty file.
7 changes: 6 additions & 1 deletion ...havior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ class LaneChangeBase
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
const double threshold) const = 0;

virtual bool getAbortPath() = 0;
virtual bool calcAbortPath() = 0;

virtual bool specialRequiredCheck() const { return false; }

Expand Down Expand Up @@ -209,6 +209,10 @@ class LaneChangeBase
return direction_;
}

boost::optional<Pose> getStopPose() const { return lane_change_stop_pose_; }

void resetStopPose() { lane_change_stop_pose_ = boost::none; }

protected:
virtual lanelet::ConstLanelets getCurrentLanes() const = 0;

Expand Down Expand Up @@ -244,6 +248,7 @@ class LaneChangeBase
PathWithLaneId prev_module_path_{};
DrivableAreaInfo prev_drivable_area_info_{};
TurnSignalInfo prev_turn_signal_info_{};
boost::optional<Pose> lane_change_stop_pose_{boost::none};

PathWithLaneId prev_approved_path_{};

Expand Down
4 changes: 3 additions & 1 deletion ...g/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -72,7 +72,7 @@ class NormalLaneChange : public LaneChangeBase

TurnSignalInfo updateOutputTurnSignal() override;

bool getAbortPath() override;
bool calcAbortPath() override;

PathSafetyStatus isApprovedPathSafe() const override;

Expand Down Expand Up @@ -147,6 +147,8 @@ class NormalLaneChange : public LaneChangeBase
const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes,
const lanelet::ConstLanelets & target_backward_lanes) const;

void setStopPose(const Pose & stop_pose);

rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr());
};
} // namespace behavior_path_planner
Expand Down
9 changes: 7 additions & 2 deletions planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,7 +172,7 @@ ModuleStatus LaneChangeInterface::updateState()
return ModuleStatus::RUNNING;
}

const auto found_abort_path = module_type_->getAbortPath();
const auto found_abort_path = module_type_->calcAbortPath();
if (!found_abort_path) {
RCLCPP_WARN_STREAM_THROTTLE(
getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000,
Expand Down Expand Up @@ -204,6 +204,7 @@ void LaneChangeInterface::updateData()
module_type_->setPreviousModulePaths(
getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path);
module_type_->updateSpecialData();
module_type_->resetStopPose();
}

BehaviorModuleOutput LaneChangeInterface::plan()
Expand All @@ -220,7 +221,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
auto output = module_type_->generateOutput();
path_reference_ = output.reference_path;
*prev_approved_path_ = *getPreviousModuleOutput().path;
module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path);

stop_pose_ = module_type_->getStopPose();

updateSteeringFactorPtr(output);
clearWaitingApproval();
Expand Down Expand Up @@ -250,6 +252,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()

path_reference_ = getPreviousModuleOutput().reference_path;

stop_pose_ = module_type_->getStopPose();

if (!module_type_->isValidPath()) {
removeRTCStatus();
path_candidate_ = std::make_shared<PathWithLaneId>();
Expand Down Expand Up @@ -288,6 +292,7 @@ void LaneChangeInterface::updateModuleParams(

void LaneChangeInterface::setData(const std::shared_ptr<const PlannerData> & data)
{
planner_data_ = data;
module_type_->setData(data);
}

Expand Down
86 changes: 50 additions & 36 deletions planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -111,37 +111,43 @@ bool NormalLaneChange::isLaneChangeRequired() const

LaneChangePath NormalLaneChange::getLaneChangePath() const
{
return isAbortState() ? *abort_path_ : status_.lane_change_path;
return status_.lane_change_path;
}

BehaviorModuleOutput NormalLaneChange::generateOutput()
{
BehaviorModuleOutput output;
output.path = std::make_shared<PathWithLaneId>(getLaneChangePath().path);

const auto found_extended_path = extendPath();
if (found_extended_path) {
*output.path = utils::combinePath(*output.path, *found_extended_path);
}
extendOutputDrivableArea(output);
output.reference_path = std::make_shared<PathWithLaneId>(getReferencePath());
output.turn_signal_info = updateOutputTurnSignal();

if (isAbortState()) {
if (isAbortState() && abort_path_) {
output.path = std::make_shared<PathWithLaneId>(abort_path_->path);
output.reference_path = std::make_shared<PathWithLaneId>(prev_module_reference_path_);
output.turn_signal_info = prev_turn_signal_info_;
return output;
}
insertStopPoint(status_.current_lanes, *output.path);
} else {
output.path = std::make_shared<PathWithLaneId>(getLaneChangePath().path);

if (isStopState()) {
const auto current_velocity = getEgoVelocity();
const auto current_dist = motion_utils::calcSignedArcLength(
output.path->points, output.path->points.front().point.pose.position, getEgoPosition());
const auto stop_dist =
-(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path);
const auto found_extended_path = extendPath();
if (found_extended_path) {
*output.path = utils::combinePath(*output.path, *found_extended_path);
}
output.reference_path = std::make_shared<PathWithLaneId>(getReferencePath());
output.turn_signal_info = updateOutputTurnSignal();

if (isStopState()) {
const auto current_velocity = getEgoVelocity();
const auto current_dist = motion_utils::calcSignedArcLength(
output.path->points, output.path->points.front().point.pose.position, getEgoPosition());
const auto stop_dist =
-(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc));
const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path);
setStopPose(stop_point.point.pose);
} else {
insertStopPoint(status_.target_lanes, *output.path);
}
}

extendOutputDrivableArea(output);

const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points);
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
*output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
Expand Down Expand Up @@ -200,6 +206,7 @@ void NormalLaneChange::insertStopPoint(
const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer;
if (stopping_distance > 0.0) {
const auto stop_point = utils::insertStopPoint(stopping_distance, path);
setStopPose(stop_point.point.pose);
}
}

Expand Down Expand Up @@ -1176,7 +1183,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) con
isAbleToStopSafely() && is_object_coming_from_rear;
}

bool NormalLaneChange::getAbortPath()
bool NormalLaneChange::calcAbortPath()
{
const auto & route_handler = getRouteHandler();
const auto & common_param = getCommonParam();
Expand Down Expand Up @@ -1284,14 +1291,12 @@ bool NormalLaneChange::getAbortPath()
reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose);
const PathWithLaneId reference_lane_segment = std::invoke([&]() {
const double s_start = arc_position.length;
double s_end = std::max(
lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, s_start);
double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start);

if (route_handler->isInGoalRouteSection(selected_path.info.target_lanes.back())) {
const auto goal_arc_coordinates =
lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose());
const double forward_length =
std::max(goal_arc_coordinates.length - minimum_lane_change_length, s_start);
const double forward_length = std::max(goal_arc_coordinates.length, s_start);
s_end = std::min(s_end, forward_length);
}
PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true);
Expand All @@ -1301,20 +1306,23 @@ bool NormalLaneChange::getAbortPath()
return ref;
});

PathWithLaneId start_to_abort_return_pose;
start_to_abort_return_pose.points.insert(
start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(),
shifted_path.path.points.begin() + abort_return_idx);
if (reference_lane_segment.points.size() > 1) {
start_to_abort_return_pose.points.insert(
start_to_abort_return_pose.points.end(), (reference_lane_segment.points.begin() + 1),
reference_lane_segment.points.end());
}

auto abort_path = selected_path;
abort_path.shifted_path = shifted_path;
abort_path.path = start_to_abort_return_pose;
abort_path.info.shift_line = shift_line;

{
PathWithLaneId aborting_path;
aborting_path.points.insert(
aborting_path.points.begin(), shifted_path.path.points.begin(),
shifted_path.path.points.begin() + abort_return_idx);

if (!reference_lane_segment.points.empty()) {
abort_path.path = utils::combinePath(aborting_path, reference_lane_segment);
} else {
abort_path.path = aborting_path;
}
}

abort_path_ = std::make_shared<LaneChangePath>(abort_path);
return true;
}
Expand Down Expand Up @@ -1381,4 +1389,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe(

return path_safety_status;
}

void NormalLaneChange::setStopPose(const Pose & stop_pose)
{
lane_change_stop_pose_ = stop_pose;
}

} // namespace behavior_path_planner
Loading