Skip to content

Commit

Permalink
apply changes based on comments
Browse files Browse the repository at this point in the history
Signed-off-by: kyoichi-sugahara <[email protected]>
  • Loading branch information
kyoichi-sugahara committed Oct 25, 2023
1 parent 6d6d40c commit 791145e
Show file tree
Hide file tree
Showing 2 changed files with 14 additions and 13 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -132,17 +132,18 @@ class StartPlannerModule : public SceneModuleInterface

void initializeSafetyCheckParameters();

PriorityOrder determinePriorityOrder(const std::string & search_priority, size_t candidates_size);
PriorityOrder determinePriorityOrder(
const std::string & search_priority, const size_t candidates_size);
bool findPullOutPath(
const std::vector<Pose> & start_pose_candidates, const size_t index,
const std::shared_ptr<PullOutPlannerBase> & planner, const Pose & refined_start_pose,
const Pose & goal_pose);
void updateStatusWithCurrentPath(
const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
const std::shared_ptr<PullOutPlannerBase> & planner);
const behavior_path_planner::PlannerType & planner_type);
void updateStatusWithNextPath(
const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
const std::shared_ptr<PullOutPlannerBase> & planner);
const behavior_path_planner::PlannerType & planner_type);
void updateStatusIfNoSafePathFound();

std::shared_ptr<StartPlannerParameters> parameters_;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -497,17 +497,16 @@ void StartPlannerModule::planWithPriority(
const PriorityOrder order_priority =
determinePriorityOrder(search_priority, start_pose_candidates.size());

for (const auto & pair : order_priority) {
if (findPullOutPath(
start_pose_candidates, pair.first, pair.second, refined_start_pose, goal_pose))
for (const auto & [index, planner] : order_priority) {
if (findPullOutPath(start_pose_candidates, index, planner, refined_start_pose, goal_pose))
return;
}

updateStatusIfNoSafePathFound();
}

PriorityOrder StartPlannerModule::determinePriorityOrder(
const std::string & search_priority, size_t candidates_size)
const std::string & search_priority, const size_t candidates_size)
{
PriorityOrder order_priority;
if (search_priority == "efficient_path") {
Expand Down Expand Up @@ -551,7 +550,7 @@ bool StartPlannerModule::findPullOutPath(

// If driving forward, update status with the current path and return true
if (is_driving_forward) {
updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner);
updateStatusWithCurrentPath(*pull_out_path, pull_out_start_pose, planner->getPlannerType());
return true;
}

Expand All @@ -565,32 +564,33 @@ bool StartPlannerModule::findPullOutPath(
if (!next_pull_out_path) return false;

// Update status with the next path and return true
updateStatusWithNextPath(*next_pull_out_path, next_pull_out_start_pose, planner);
updateStatusWithNextPath(
*next_pull_out_path, next_pull_out_start_pose, planner->getPlannerType());
return true;
}

void StartPlannerModule::updateStatusWithCurrentPath(
const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
const std::shared_ptr<PullOutPlannerBase> & planner)
const behavior_path_planner::PlannerType & planner_type)
{
const std::lock_guard<std::mutex> lock(mutex_);
status_.driving_forward = true;
status_.found_pull_out_path = true;
status_.pull_out_path = path;
status_.pull_out_start_pose = start_pose;
status_.planner_type = planner->getPlannerType();
status_.planner_type = planner_type;
}

void StartPlannerModule::updateStatusWithNextPath(
const behavior_path_planner::PullOutPath & path, const Pose & start_pose,
const std::shared_ptr<PullOutPlannerBase> & planner)
const behavior_path_planner::PlannerType & planner_type)
{
const std::lock_guard<std::mutex> lock(mutex_);
status_.driving_forward = false;
status_.found_pull_out_path = true;
status_.pull_out_path = path;
status_.pull_out_start_pose = start_pose;
status_.planner_type = planner->getPlannerType();
status_.planner_type = planner_type;
}

void StartPlannerModule::updateStatusIfNoSafePathFound()
Expand Down

0 comments on commit 791145e

Please sign in to comment.