Skip to content

Commit 196a688

Browse files
committed
feat(lane_change): wait approval with abort state (autowarefoundation#6234)
Signed-off-by: Fumiya Watanabe <rej55.g@gmail.com>
1 parent 01b081b commit 196a688

File tree

2 files changed

+44
-28
lines changed

2 files changed

+44
-28
lines changed

planning/behavior_path_lane_change_module/src/interface.cpp

+11-2
Original file line numberDiff line numberDiff line change
@@ -70,7 +70,7 @@ bool LaneChangeInterface::isExecutionRequested() const
7070

7171
bool LaneChangeInterface::isExecutionReady() const
7272
{
73-
return module_type_->isSafe();
73+
return module_type_->isSafe() && !module_type_->isAbortState();
7474
}
7575

7676
void LaneChangeInterface::updateData()
@@ -115,7 +115,16 @@ BehaviorModuleOutput LaneChangeInterface::plan()
115115
}
116116

117117
updateSteeringFactorPtr(output);
118-
clearWaitingApproval();
118+
if (module_type_->isAbortState()) {
119+
waitApproval();
120+
removeRTCStatus();
121+
const auto candidate = planCandidate();
122+
path_candidate_ = std::make_shared<PathWithLaneId>(candidate.path_candidate);
123+
updateRTCStatus(
124+
candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change);
125+
} else {
126+
clearWaitingApproval();
127+
}
119128

120129
return output;
121130
}

planning/behavior_path_lane_change_module/src/scene.cpp

+33-26
Original file line numberDiff line numberDiff line change
@@ -144,6 +144,19 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
144144
{
145145
BehaviorModuleOutput output;
146146

147+
if (isAbortState() && abort_path_) {
148+
output.path = abort_path_->path;
149+
output.reference_path = prev_module_reference_path_;
150+
output.turn_signal_info = prev_turn_signal_info_;
151+
extendOutputDrivableArea(output);
152+
const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path.points);
153+
output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal(
154+
output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info,
155+
planner_data_->parameters.ego_nearest_dist_threshold,
156+
planner_data_->parameters.ego_nearest_yaw_threshold);
157+
return output;
158+
}
159+
147160
const auto current_lanes = getCurrentLanes();
148161
if (current_lanes.empty()) {
149162
RCLCPP_WARN(logger_, "Current lanes not found!!! Something wrong.");
@@ -157,7 +170,7 @@ BehaviorModuleOutput NormalLaneChange::getTerminalLaneChangePath() const
157170
const auto terminal_path =
158171
calcTerminalLaneChangePath(current_lanes, getLaneChangeLanes(current_lanes, direction_));
159172
if (!terminal_path) {
160-
RCLCPP_WARN(logger_, "Terminal path not found!!!");
173+
RCLCPP_DEBUG(logger_, "Terminal path not found!!!");
161174
output.path = prev_module_path_;
162175
output.reference_path = prev_module_reference_path_;
163176
output.drivable_area_info = prev_drivable_area_info_;
@@ -1528,7 +1541,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
15281541
-(lane_change_buffer + next_lane_change_buffer + distance_to_terminal_from_goal));
15291542

15301543
if (!lane_changing_start_pose) {
1531-
RCLCPP_WARN(logger_, "Reject: lane changing start pose not found!!!");
1544+
RCLCPP_DEBUG(logger_, "Reject: lane changing start pose not found!!!");
15321545
return {};
15331546
}
15341547

@@ -1537,7 +1550,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
15371550

15381551
// Check if the lane changing start point is not on the lanes next to target lanes,
15391552
if (target_length_from_lane_change_start_pose > 0.0) {
1540-
RCLCPP_WARN(logger_, "lane change start getEgoPose() is behind target lanelet!");
1553+
RCLCPP_DEBUG(logger_, "lane change start getEgoPose() is behind target lanelet!");
15411554
return {};
15421555
}
15431556

@@ -1555,7 +1568,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
15551568
minimum_lane_changing_velocity, next_lane_change_buffer);
15561569

15571570
if (target_segment.points.empty()) {
1558-
RCLCPP_WARN(logger_, "Reject: target segment is empty!! something wrong...");
1571+
RCLCPP_DEBUG(logger_, "Reject: target segment is empty!! something wrong...");
15591572
return {};
15601573
}
15611574

@@ -1584,7 +1597,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
15841597
lane_change_info.terminal_lane_changing_velocity = minimum_lane_changing_velocity;
15851598

15861599
if (!is_valid_start_point) {
1587-
RCLCPP_WARN(
1600+
RCLCPP_DEBUG(
15881601
logger_,
15891602
"Reject: lane changing points are not inside of the target preferred lanes or its "
15901603
"neighbors");
@@ -1599,7 +1612,7 @@ std::optional<LaneChangePath> NormalLaneChange::calcTerminalLaneChangePath(
15991612
next_lane_change_buffer);
16001613

16011614
if (target_lane_reference_path.points.empty()) {
1602-
RCLCPP_WARN(logger_, "Reject: target_lane_reference_path is empty!!");
1615+
RCLCPP_DEBUG(logger_, "Reject: target_lane_reference_path is empty!!");
16031616
return {};
16041617
}
16051618

@@ -1857,31 +1870,25 @@ bool NormalLaneChange::calcAbortPath()
18571870

18581871
ShiftedPath shifted_path;
18591872
// offset front side
1860-
// bool offset_back = false;
18611873
if (!path_shifter.generate(&shifted_path)) {
18621874
RCLCPP_ERROR(logger_, "failed to generate abort shifted path.");
18631875
}
18641876

1865-
const auto arc_position = lanelet::utils::getArcCoordinates(
1866-
reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose);
1867-
const PathWithLaneId reference_lane_segment = std::invoke([&]() {
1868-
const double s_start = arc_position.length;
1869-
double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start);
1870-
1871-
if (
1872-
!reference_lanelets.empty() &&
1873-
route_handler->isInGoalRouteSection(reference_lanelets.back())) {
1874-
const auto goal_arc_coordinates =
1875-
lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose());
1876-
const double forward_length = std::max(goal_arc_coordinates.length, s_start);
1877-
s_end = std::min(s_end, forward_length);
1877+
PathWithLaneId reference_lane_segment = prev_module_path_;
1878+
{
1879+
const auto terminal_path =
1880+
calcTerminalLaneChangePath(reference_lanelets, selected_path.info.target_lanes);
1881+
if (terminal_path) {
1882+
reference_lane_segment = terminal_path->path;
18781883
}
1879-
PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true);
1880-
ref.points.back().point.longitudinal_velocity_mps = std::min(
1881-
ref.points.back().point.longitudinal_velocity_mps,
1882-
static_cast<float>(lane_change_parameters_->minimum_lane_changing_velocity));
1883-
return ref;
1884-
});
1884+
const auto return_pose = shifted_path.path.points.at(abort_return_idx).point.pose;
1885+
const auto seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
1886+
reference_lane_segment.points, return_pose, common_param.ego_nearest_dist_threshold,
1887+
common_param.ego_nearest_yaw_threshold);
1888+
reference_lane_segment.points = motion_utils::cropPoints(
1889+
reference_lane_segment.points, return_pose.position, seg_idx,
1890+
common_param.forward_path_length, 0.0);
1891+
}
18851892

18861893
auto abort_path = selected_path;
18871894
abort_path.shifted_path = shifted_path;

0 commit comments

Comments
 (0)