Skip to content

Commit

Permalink
feat(start_planner): allow lane departure check override (#6512)
Browse files Browse the repository at this point in the history 
* 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>
  • Loading branch information
@danielsanchezaran
danielsanchezaran authored Feb 29, 2024
1 parent 2d065fc commit c36e2ef
Show file tree
Hide file tree
Showing 7 changed files with 133 additions and 114 deletions.
21 changes: 11 additions & 10 deletions planning/behavior_path_start_planner_module/README.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -433,16 +433,17 @@ Pull out distance is calculated by the speed, lateral deviation, and the lateral

#### parameters for shift pull out

| Name | Unit | Type | Description | Default value |
| :------------------------------ | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true |
| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | false |
| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 |
| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 |
| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 |
| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 |
| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 |
| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 |
| Name | Unit | Type | Description | Default value |
| :--------------------------------------------- | :----- | :----- | :------------------------------------------------------------------------------------------------------------------- | :------------ |
| enable_shift_pull_out | [-] | bool | flag whether to enable shift pull out | true |
| check_shift_path_lane_departure | [-] | bool | flag whether to check if shift path footprints are out of lane | true |
| allow_check_shift_path_lane_departure_override | [-] | bool | flag to override/cancel lane departure check if the ego vehicle's starting pose is already out of lane | false |
| shift_pull_out_velocity | [m/s] | double | velocity of shift pull out | 2.0 |
| pull_out_sampling_num | [-] | int | Number of samplings in the minimum to maximum range of lateral_jerk | 4 |
| maximum_lateral_jerk | [m/s3] | double | maximum lateral jerk | 2.0 |
| minimum_lateral_jerk | [m/s3] | double | minimum lateral jerk | 0.1 |
| minimum_shift_pull_out_distance | [m] | double | minimum shift pull out distance. if calculated pull out distance is shorter than this, use this for path generation. | 0.0 |
| maximum_curvature | [m] | double | maximum curvature. The pull out distance is calculated so that the curvature is smaller than this value. | 0.07 |

### **geometric pull out**

Expand Down
1 change: 1 addition & 0 deletions planning/behavior_path_start_planner_module/config/start_planner.param.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
# shift pull out
enable_shift_pull_out: true
check_shift_path_lane_departure: true
allow_check_shift_path_lane_departure_override: false
shift_collision_check_distance_from_end: -10.0
minimum_shift_pull_out_distance: 0.0
deceleration_interval: 15.0
Expand Down
1 change: 1 addition & 0 deletions ...ior_path_start_planner_module/include/behavior_path_start_planner_module/data_structs.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -72,6 +72,7 @@ struct StartPlannerParameters
// shift pull out
bool enable_shift_pull_out{false};
bool check_shift_path_lane_departure{false};
bool allow_check_shift_path_lane_departure_override{false};
double shift_collision_check_distance_from_end{0.0};
double minimum_shift_pull_out_distance{0.0};
int lateral_acceleration_sampling_num{0};
Expand Down
15 changes: 15 additions & 0 deletions ..._start_planner_module/include/behavior_path_start_planner_module/start_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -178,6 +178,21 @@ class StartPlannerModule : public SceneModuleInterface

bool requiresDynamicObjectsCollisionDetection() const;

uint16_t getSteeringFactorDirection(
const behavior_path_planner::BehaviorModuleOutput & output) const
{
switch (output.turn_signal_info.turn_signal.command) {
case TurnIndicatorsCommand::ENABLE_LEFT:
return SteeringFactor::LEFT;

case TurnIndicatorsCommand::ENABLE_RIGHT:
return SteeringFactor::RIGHT;

default:
return SteeringFactor::STRAIGHT;
}
};

/**
* @brief Check if there are no moving objects around within a certain radius.
*
Expand Down
5 changes: 5 additions & 0 deletions planning/behavior_path_start_planner_module/src/manager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -54,6 +54,8 @@ void StartPlannerModuleManager::init(rclcpp::Node * node)
p.enable_shift_pull_out = node->declare_parameter<bool>(ns + "enable_shift_pull_out");
p.check_shift_path_lane_departure =
node->declare_parameter<bool>(ns + "check_shift_path_lane_departure");
p.allow_check_shift_path_lane_departure_override =
node->declare_parameter<bool>(ns + "allow_check_shift_path_lane_departure_override");
p.shift_collision_check_distance_from_end =
node->declare_parameter<double>(ns + "shift_collision_check_distance_from_end");
p.minimum_shift_pull_out_distance =
Expand Down Expand Up @@ -390,6 +392,9 @@ void StartPlannerModuleManager::updateModuleParams(
p->geometric_collision_check_distance_from_end);
updateParam<bool>(
parameters, ns + "check_shift_path_lane_departure", p->check_shift_path_lane_departure);
updateParam<bool>(
parameters, ns + "allow_check_shift_path_lane_departure_override",
p->allow_check_shift_path_lane_departure_override);
updateParam<std::string>(parameters, ns + "search_priority", p->search_priority);
updateParam<double>(parameters, ns + "max_back_distance", p->max_back_distance);
updateParam<double>(
Expand Down
24 changes: 19 additions & 5 deletions planning/behavior_path_start_planner_module/src/shift_pull_out.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,13 +74,27 @@ std::optional<PullOutPath> ShiftPullOut::plan(const Pose & start_pose, const Pos
shift_path.points.begin() + pull_out_end_idx + 1);
}

// check lane departure
// The method for lane departure checking verifies if the footprint of each point on the path is
// contained within a lanelet using `boost::geometry::within`, which incurs a high computational
// cost.
const auto lanelet_map_ptr = planner_data_->route_handler->getLaneletMapPtr();

// if lane departure check override is true, and if the initial pose is not fully within a lane,
// cancel lane departure check
const bool is_lane_departure_check_required = std::invoke([&]() -> bool {
if (!parameters_.allow_check_shift_path_lane_departure_override)
return parameters_.check_shift_path_lane_departure;

PathWithLaneId path_with_only_first_pose{};
path_with_only_first_pose.points.push_back(path_shift_start_to_end.points.at(0));
return !lane_departure_checker_->checkPathWillLeaveLane(
lanelet_map_ptr, path_with_only_first_pose);
});

// check lane departure
// The method for lane departure checking verifies if the footprint of each point on the path
// is contained within a lanelet using `boost::geometry::within`, which incurs a high
// computational cost.

if (
parameters_.check_shift_path_lane_departure &&
is_lane_departure_check_required &&
lane_departure_checker_->checkPathWillLeaveLane(lanelet_map_ptr, path_shift_start_to_end)) {
continue;
}
Expand Down
180 changes: 81 additions & 99 deletions planning/behavior_path_start_planner_module/src/start_planner_module.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -182,18 +182,14 @@ void StartPlannerModule::updateData()
status_.first_engaged_time = clock_->now();
}

if (hasFinishedBackwardDriving()) {
status_.backward_driving_complete = hasFinishedBackwardDriving();
if (status_.backward_driving_complete) {
updateStatusAfterBackwardDriving();
DEBUG_PRINT("StartPlannerModule::updateData() completed backward driving");
} else {
status_.backward_driving_complete = false;
}

if (requiresDynamicObjectsCollisionDetection()) {
status_.is_safe_dynamic_objects = !hasCollisionWithDynamicObjects();
} else {
status_.is_safe_dynamic_objects = true;
}
status_.is_safe_dynamic_objects =
(!requiresDynamicObjectsCollisionDetection()) ? true : !hasCollisionWithDynamicObjects();
}

bool StartPlannerModule::hasFinishedBackwardDriving() const
Expand Down Expand Up @@ -364,20 +360,17 @@ bool StartPlannerModule::isStopped()

bool StartPlannerModule::isExecutionReady() const
{
bool is_safe = true;
// Evaluate safety. The situation is not safe if any of the following conditions are met:
// 1. pull out path has not been found
// 2. there is a moving objects around ego
// 3. waiting for approval and there is a collision with dynamic objects
if (!status_.found_pull_out_path) {
is_safe = false;
} else if (isWaitingApproval()) {
if (!noMovingObjectsAround()) {
is_safe = false;
} else if (requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects()) {
is_safe = false;
}
}

const bool is_safe = [&]() -> bool {
if (!status_.found_pull_out_path) return false;
if (!isWaitingApproval()) return true;
if (!noMovingObjectsAround()) return false;
return !(requiresDynamicObjectsCollisionDetection() && hasCollisionWithDynamicObjects());
}();

if (!is_safe) {
stop_pose_ = planner_data_->self_odometry->pose.pose;
Expand Down Expand Up @@ -459,14 +452,7 @@ BehaviorModuleOutput StartPlannerModule::plan()

setDrivableAreaInfo(output);

const uint16_t steering_factor_direction = std::invoke([&output]() {
if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
return SteeringFactor::LEFT;
} else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
return SteeringFactor::RIGHT;
}
return SteeringFactor::STRAIGHT;
});
const auto steering_factor_direction = getSteeringFactorDirection(output);

if (status_.driving_forward) {
const double start_distance = motion_utils::calcSignedArcLength(
Expand All @@ -480,15 +466,16 @@ BehaviorModuleOutput StartPlannerModule::plan()
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
SteeringFactor::TURNING, "");
} else {
const double distance = motion_utils::calcSignedArcLength(
path.points, planner_data_->self_odometry->pose.pose.position,
status_.pull_out_path.start_pose.position);
updateRTCStatus(0.0, distance);
steering_factor_interface_ptr_->updateSteeringFactor(
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");
setDebugData();
return output;
}
const double distance = motion_utils::calcSignedArcLength(
path.points, planner_data_->self_odometry->pose.pose.position,
status_.pull_out_path.start_pose.position);
updateRTCStatus(0.0, distance);
steering_factor_interface_ptr_->updateSteeringFactor(
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::TURNING, "");

setDebugData();

Expand Down Expand Up @@ -569,14 +556,7 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()

setDrivableAreaInfo(output);

const uint16_t steering_factor_direction = std::invoke([&output]() {
if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) {
return SteeringFactor::LEFT;
} else if (output.turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_RIGHT) {
return SteeringFactor::RIGHT;
}
return SteeringFactor::STRAIGHT;
});
const auto steering_factor_direction = getSteeringFactorDirection(output);

if (status_.driving_forward) {
const double start_distance = motion_utils::calcSignedArcLength(
Expand All @@ -590,15 +570,17 @@ BehaviorModuleOutput StartPlannerModule::planWaitingApproval()
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose},
{start_distance, finish_distance}, PlanningBehavior::START_PLANNER, steering_factor_direction,
SteeringFactor::APPROACHING, "");
} else {
const double distance = motion_utils::calcSignedArcLength(
stop_path.points, planner_data_->self_odometry->pose.pose.position,
status_.pull_out_path.start_pose.position);
updateRTCStatus(0.0, distance);
steering_factor_interface_ptr_->updateSteeringFactor(
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");
setDebugData();

return output;
}
const double distance = motion_utils::calcSignedArcLength(
stop_path.points, planner_data_->self_odometry->pose.pose.position,
status_.pull_out_path.start_pose.position);
updateRTCStatus(0.0, distance);
steering_factor_interface_ptr_->updateSteeringFactor(
{status_.pull_out_path.start_pose, status_.pull_out_path.end_pose}, {0.0, distance},
PlanningBehavior::START_PLANNER, steering_factor_direction, SteeringFactor::APPROACHING, "");

setDebugData();

Expand Down Expand Up @@ -658,16 +640,20 @@ PriorityOrder StartPlannerModule::determinePriorityOrder(
order_priority.emplace_back(i, planner);
}
}
} else if (search_priority == "short_back_distance") {
return order_priority;
}

if (search_priority == "short_back_distance") {
for (size_t i = 0; i < start_pose_candidates_num; i++) {
for (const auto & planner : start_planners_) {
order_priority.emplace_back(i, planner);
}
}
} else {
RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str());
throw std::domain_error("[start_planner] invalid search_priority");
return order_priority;
}

RCLCPP_ERROR(getLogger(), "Invalid search_priority: %s", search_priority.c_str());
throw std::domain_error("[start_planner] invalid search_priority");
return order_priority;
}

Expand Down Expand Up @@ -1099,12 +1085,7 @@ bool StartPlannerModule::isStuck()
return false;
}

if (status_.planner_type == PlannerType::STOP) {
return true;
}

// not found safe path
if (!status_.found_pull_out_path) {
if (status_.planner_type == PlannerType::STOP || !status_.found_pull_out_path) {
return true;
}

Expand Down Expand Up @@ -1197,30 +1178,26 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const
return false;
});

if (is_near_intersection) {
// offset required end pose with ration to activate turn signal for intersection
turn_signal.required_end_point = std::invoke([&]() {
const double length_start_to_end =
motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
const auto ratio = std::clamp(
parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);

const double required_end_length = length_start_to_end * ratio;
double accumulated_length = 0.0;
const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
accumulated_length +=
tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
if (accumulated_length > required_end_length) {
return path.points.at(i).point.pose;
}
turn_signal.required_end_point = std::invoke([&]() {
if (is_near_intersection) return end_pose;
const double length_start_to_end =
motion_utils::calcSignedArcLength(path.points, start_pose.position, end_pose.position);
const auto ratio = std::clamp(
parameters_->length_ratio_for_turn_signal_deactivation_near_intersection, 0.0, 1.0);

const double required_end_length = length_start_to_end * ratio;
double accumulated_length = 0.0;
const size_t start_idx = motion_utils::findNearestIndex(path.points, start_pose.position);
for (size_t i = start_idx; i < path.points.size() - 1; ++i) {
accumulated_length +=
tier4_autoware_utils::calcDistance2d(path.points.at(i), path.points.at(i + 1));
if (accumulated_length > required_end_length) {
return path.points.at(i).point.pose;
}
// not found required end point
return end_pose;
});
} else {
turn_signal.required_end_point = end_pose;
}
}
// not found required end point
return end_pose;
});

return turn_signal;
}
Expand Down Expand Up @@ -1389,22 +1366,27 @@ bool StartPlannerModule::planFreespacePath()

void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const
{
if (status_.planner_type == PlannerType::FREESPACE) {
const double drivable_area_margin = planner_data_->parameters.vehicle_width;
output.drivable_area_info.drivable_margin =
planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
} else {
const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
output.path, generateDrivableLanes(output.path),
planner_data_->drivable_area_expansion_parameters);

DrivableAreaInfo current_drivable_area_info;
current_drivable_area_info.drivable_lanes = target_drivable_lanes;
output.drivable_area_info =
status_.driving_forward
? utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info)
: current_drivable_area_info;
switch (status_.planner_type) {
case PlannerType::FREESPACE: {
const double drivable_area_margin = planner_data_->parameters.vehicle_width;
output.drivable_area_info.drivable_margin =
planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin;
break;
}
default: {
const auto target_drivable_lanes = utils::getNonOverlappingExpandedLanes(
output.path, generateDrivableLanes(output.path),
planner_data_->drivable_area_expansion_parameters);

DrivableAreaInfo current_drivable_area_info;
current_drivable_area_info.drivable_lanes = target_drivable_lanes;
output.drivable_area_info =
status_.driving_forward
? utils::combineDrivableAreaInfo(
current_drivable_area_info, getPreviousModuleOutput().drivable_area_info)
: current_drivable_area_info;
break;
}
}
}

Expand Down

0 comments on commit c36e2ef

Please sign in to comment.