Skip to content

Commit 4165c5b

Browse files
feat(start_planner): allow lane departure check override (autowarefoundation#6512)
* small refactor Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * another refactor Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * further refactoring Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * add param to override lane_departure_check when starting outside lane Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> * update documentation Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com> --------- Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
1 parent 71882fb commit 4165c5b

File tree

6 files changed

+120
-105
lines changed

6 files changed

+120
-105
lines changed

planning/behavior_path_start_planner_module/config/start_planner.param.yaml

+3-1
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,9 @@
1212
center_line_path_interval: 1.0
1313
# shift pull out
1414
enable_shift_pull_out: true
15-
check_shift_path_lane_departure: false
15+
check_shift_path_lane_departure: true
16+
allow_check_shift_path_lane_departure_override: false
17+
shift_collision_check_distance_from_end: -10.0
1618
minimum_shift_pull_out_distance: 0.0
1719
deceleration_interval: 15.0
1820
lateral_jerk: 0.5

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp

+15
Original file line numberDiff line numberDiff line change
@@ -154,6 +154,21 @@ class StartPlannerModule : public SceneModuleInterface
154154

155155
bool requiresDynamicObjectsCollisionDetection() const;
156156

157+
uint16_t getSteeringFactorDirection(
158+
const behavior_path_planner::BehaviorModuleOutput & output) const
159+
{
160+
switch (output.turn_signal_info.turn_signal.command) {
161+
case TurnIndicatorsCommand::ENABLE_LEFT:
162+
return SteeringFactor::LEFT;
163+
164+
case TurnIndicatorsCommand::ENABLE_RIGHT:
165+
return SteeringFactor::RIGHT;
166+
167+
default:
168+
return SteeringFactor::STRAIGHT;
169+
}
170+
};
171+
157172
/**
158173
* @brief Check if there are no moving objects around within a certain radius.
159174
*

planning/behavior_path_start_planner_module/include/behavior_path_start_planner_module/start_planner_parameters.hpp

+2
Original file line numberDiff line numberDiff line change
@@ -52,6 +52,8 @@ struct StartPlannerParameters
5252
// shift pull out
5353
bool enable_shift_pull_out{false};
5454
bool check_shift_path_lane_departure{false};
55+
bool allow_check_shift_path_lane_departure_override{false};
56+
double shift_collision_check_distance_from_end{0.0};
5557
double minimum_shift_pull_out_distance{0.0};
5658
int lateral_acceleration_sampling_num{0};
5759
double lateral_jerk{0.0};

planning/behavior_path_start_planner_module/src/manager.cpp

+4
Original file line numberDiff line numberDiff line change
@@ -53,6 +53,10 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
5353
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
5454
p.check_shift_path_lane_departure =
5555
node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
56+
p.allow_check_shift_path_lane_departure_override =
57+
node->declare_parameter<bool>(ns + "allow_check_shift_path_lane_departure_override");
58+
p.shift_collision_check_distance_from_end =
59+
node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
5660
p.minimum_shift_pull_out_distance =
5761
node->declare_parameter<double>(ns + "minimum_shift_pull_out_distance");
5862
p.lateral_acceleration_sampling_num =

planning/behavior_path_start_planner_module/src/shift_pull_out.cpp

+19-5
Original file line numberDiff line numberDiff line change
@@ -97,13 +97,27 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
9797
shift_path.points.begin() + collision_check_end_idx + 1);
9898
}
9999

100-
// check lane departure
101-
// The method for lane departure checking verifies if the footprint of each point on the path is
102-
// contained within a lanelet using `boost::geometry::within`, which incurs a high computational
103-
// cost.
104100
const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();
101+
102+
// if lane departure check override is true, and if the initial pose is not fully within a lane,
103+
// cancel lane departure check
104+
const bool is_lane_departure_check_required = std::invoke([&]() -> bool {
105+
if (!parameters_.allow_check_shift_path_lane_departure_override)
106+
return parameters_.check_shift_path_lane_departure;
107+
108+
PathWithLaneId path_with_only_first_pose{};
109+
path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.at(0));
110+
return !lane_departure_checker_->checkPathWillLeaveLane(
111+
lanelet_map_ptr, path_with_only_first_pose);
112+
});
113+
114+
// check lane departure
115+
// The method for lane departure checking verifies if the footprint of each point on the path
116+
// is contained within a lanelet using `boost::geometry::within`, which incurs a high
117+
// computational cost.
118+
105119
if (
106-
parameters_.check_shift_path_lane_departure &&
120+
is_lane_departure_check_required &&
107121
lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) {
108122
continue;
109123
}

planning/behavior_path_start_planner_module/src/start_planner_module.cpp

+77-99
Original file line numberDiff line numberDiff line change
@@ -176,18 +176,14 @@ void StartPlannerModule::updateData()
176176
status_.first_engaged_and_driving_forward_time = clock_->now();
177177
}
178178

179-
if (hasFinishedBackwardDriving()) {
179+
status_.backward_driving_complete = hasFinishedBackwardDriving();
180+
if (status_.backward_driving_complete) {
180181
updateStatusAfterBackwardDriving();
181182
DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving");
182-
} else {
183-
status_.backward_driving_complete = false;
184183
}
185184

186-
if (requiresDynamicObjectsCollisionDetection()) {
187-
status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects();
188-
} else {
189-
status_.is_safe_dynamic_objects = true;
190-
}
185+
status_.is_safe_dynamic_objects =
186+
(!requiresDynamicObjectsCollisionDetection()) ? true : !hasCollisionWithDynamicObjects();
191187
}
192188

193189
bool StartPlannerModule::hasFinishedBackwardDriving() const
@@ -326,20 +322,17 @@ bool StartPlannerModule::isStopped()
326322

327323
bool StartPlannerModule::isExecutionReady() const
328324
{
329-
bool is_safe = true;
330325
// Evaluate safety. The situation is not safe if any of the following conditions are met:
331326
// 1. pull out path has not been found
332327
// 2. there is a moving objects around ego
333328
// 3. waiting for approval and there is a collision with dynamic objects
334-
if (!status_.found_pull_out_path) {
335-
is_safe = false;
336-
} else if (isWaitingApproval()) {
337-
if (!noMovingObjectsAround()) {
338-
is_safe = false;
339-
} else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) {
340-
is_safe = false;
341-
}
342-
}
329+
330+
const bool is_safe = [&]() -> bool {
331+
if (!status_.found_pull_out_path) return false;
332+
if (!isWaitingApproval()) return true;
333+
if (!noMovingObjectsAround()) return false;
334+
return !(requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects());
335+
}();
343336

344337
if (!is_safe) {
345338
stop_pose_ = planner_data_->self_odometry->pose.pose;
@@ -420,14 +413,7 @@ BehaviorModuleOutput StartPlannerModule::plan()
420413

421414
setDrivableAreaInfo(output);
422415

423-
const uint16_t steering_factor_direction = std::invoke([&output]() {
424-
if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
425-
return SteeringFactor::LEFT;
426-
} else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
427-
return SteeringFactor::RIGHT;
428-
}
429-
return SteeringFactor::STRAIGHT;
430-
});
416+
const auto steering_factor_direction = getSteeringFactorDirection(output);
431417

432418
if (status_.driving_forward) {
433419
const double start_distance = motion_utils::calcSignedArcLength(
@@ -441,15 +427,16 @@ BehaviorModuleOutput StartPlannerModule::plan()
441427
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
442428
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
443429
SteeringFactor::TURNING, "");
444-
} else {
445-
const double distance = motion_utils::calcSignedArcLength(
446-
path.points, planner_data_->self_odometry->pose.pose.position,
447-
status_.pull_out_path.start_pose.position);
448-
updateRTCStatus(0.0, distance);
449-
steering_factor_interface_ptr_->updateSteeringFactor(
450-
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
451-
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
430+
setDebugData();
431+
return output;
452432
}
433+
const double distance = motion_utils::calcSignedArcLength(
434+
path.points, planner_data_->self_odometry->pose.pose.position,
435+
status_.pull_out_path.start_pose.position);
436+
updateRTCStatus(0.0, distance);
437+
steering_factor_interface_ptr_->updateSteeringFactor(
438+
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
439+
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
453440

454441
setDebugData();
455442

@@ -530,14 +517,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
530517

531518
setDrivableAreaInfo(output);
532519

533-
const uint16_t steering_factor_direction = std::invoke([&output]() {
534-
if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
535-
return SteeringFactor::LEFT;
536-
} else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
537-
return SteeringFactor::RIGHT;
538-
}
539-
return SteeringFactor::STRAIGHT;
540-
});
520+
const auto steering_factor_direction = getSteeringFactorDirection(output);
541521

542522
if (status_.driving_forward) {
543523
const double start_distance = motion_utils::calcSignedArcLength(
@@ -551,15 +531,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
551531
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
552532
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
553533
SteeringFactor::APPROACHING, "");
554-
} else {
555-
const double distance = motion_utils::calcSignedArcLength(
556-
stop_path.points, planner_data_->self_odometry->pose.pose.position,
557-
status_.pull_out_path.start_pose.position);
558-
updateRTCStatus(0.0, distance);
559-
steering_factor_interface_ptr_->updateSteeringFactor(
560-
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
561-
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
534+
setDebugData();
535+
536+
return output;
562537
}
538+
const double distance = motion_utils::calcSignedArcLength(
539+
stop_path.points, planner_data_->self_odometry->pose.pose.position,
540+
status_.pull_out_path.start_pose.position);
541+
updateRTCStatus(0.0, distance);
542+
steering_factor_interface_ptr_->updateSteeringFactor(
543+
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
544+
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
563545

564546
setDebugData();
565547

@@ -618,10 +600,11 @@ PriorityOrder StartPlannerModule::determinePriorityOrder(
618600
order_priority.emplace_back(i, planner);
619601
}
620602
}
621-
} else {
622-
RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str());
623-
throw std::domain_error("[start_planner] invalid search_priority");
603+
return order_priority;
624604
}
605+
606+
RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str());
607+
throw std::domain_error("[start_planner] invalid search_priority");
625608
return order_priority;
626609
}
627610

@@ -990,12 +973,7 @@ bool StartPlannerModule::isStuck()
990973
return false;
991974
}
992975

993-
if (status_.planner_type == PlannerType::STOP) {
994-
return true;
995-
}
996-
997-
// not found safe path
998-
if (!status_.found_pull_out_path) {
976+
if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) {
999977
return true;
1000978
}
1001979

@@ -1088,30 +1066,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
10881066
return false;
10891067
});
10901068

1091-
if (is_near_intersection) {
1092-
// offset required end pose with ration to activate turn signal for intersection
1093-
turn_signal.required_end_point = std::invoke([&]() {
1094-
const double length_start_to_end =
1095-
motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
1096-
const auto ratio = std::clamp(
1097-
parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
1098-
1099-
const double required_end_length = length_start_to_end * ratio;
1100-
double accumulated_length = 0.0;
1101-
const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
1102-
for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
1103-
accumulated_length +=
1104-
tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
1105-
if (accumulated_length > required_end_length) {
1106-
return path.points.at(i).point.pose;
1107-
}
1069+
turn_signal.required_end_point = std::invoke([&]() {
1070+
if (is_near_intersection) return end_pose;
1071+
const double length_start_to_end =
1072+
motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
1073+
const auto ratio = std::clamp(
1074+
parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);
1075+
1076+
const double required_end_length = length_start_to_end * ratio;
1077+
double accumulated_length = 0.0;
1078+
const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
1079+
for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
1080+
accumulated_length +=
1081+
tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
1082+
if (accumulated_length > required_end_length) {
1083+
return path.points.at(i).point.pose;
11081084
}
1109-
// not found required end point
1110-
return end_pose;
1111-
});
1112-
} else {
1113-
turn_signal.required_end_point = end_pose;
1114-
}
1085+
}
1086+
// not found required end point
1087+
return end_pose;
1088+
});
11151089

11161090
return turn_signal;
11171091
}
@@ -1279,23 +1253,27 @@ bool StartPlannerModule::planFreespacePath()
12791253

12801254
void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
12811255
{
1282-
if (status_.planner_type == PlannerType::FREESPACE) {
1283-
std::cerr << "Freespace planner updated drivable area." << std::endl;
1284-
const double drivable_area_margin = planner_data_->parameters.vehicle_width;
1285-
output.drivable_area_info.drivable_margin =
1286-
planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
1287-
} else {
1288-
const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
1289-
output.path, generateDrivableLanes(output.path),
1290-
planner_data_->drivable_area_expansion_parameters);
1291-
1292-
DrivableAreaInfo current_drivable_area_info;
1293-
current_drivable_area_info.drivable_lanes = target_drivable_lanes;
1294-
output.drivable_area_info =
1295-
status_.driving_forward
1296-
? utils::combineDrivableAreaInfo(
1297-
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info)
1298-
: current_drivable_area_info;
1256+
switch (status_.planner_type) {
1257+
case PlannerType::FREESPACE: {
1258+
const double drivable_area_margin = planner_data_->parameters.vehicle_width;
1259+
output.drivable_area_info.drivable_margin =
1260+
planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
1261+
break;
1262+
}
1263+
default: {
1264+
const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
1265+
output.path, generateDrivableLanes(output.path),
1266+
planner_data_->drivable_area_expansion_parameters);
1267+
1268+
DrivableAreaInfo current_drivable_area_info;
1269+
current_drivable_area_info.drivable_lanes = target_drivable_lanes;
1270+
output.drivable_area_info =
1271+
status_.driving_forward
1272+
? utils::combineDrivableAreaInfo(
1273+
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info)
1274+
: current_drivable_area_info;
1275+
break;
1276+
}
12991277
}
13001278
}
13011279

0 commit comments

Comments
 (0)