Skip to content

Commit

Permalink
further refactoring
Browse files Browse the repository at this point in the history 
Signed-off-by: Daniel Sanchez <danielsanchezaran@gmail.com>
  • Loading branch information
@danielsanchezaran
danielsanchezaran committed Feb 29, 2024
1 parent b4764e2 commit a073d2f
Show file tree
Hide file tree
Showing 2 changed files with 85 additions and 81 deletions.
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
151 changes: 70 additions & 81 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 @@ -452,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 @@ -473,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 @@ -562,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 @@ -583,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 @@ -651,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 @@ -1092,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 @@ -1190,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 @@ -1382,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 a073d2f

Please sign in to comment.