From 9018b214c8361c0f975479509678882faebdf5a8 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 8 Nov 2023 22:02:32 +0900 Subject: [PATCH] fix style(pre-commit): autofix --- .../goal_planner/goal_planner_module.hpp | 16 ++++++++++------ .../goal_planner/goal_planner_module.cpp | 8 ++++---- 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp index ccd1da462f8ca..e4044d7191805 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/goal_planner/goal_planner_module.hpp @@ -101,12 +101,10 @@ class PullOverStatus pull_over_lanes_.clear(); lanes_.clear(); has_decided_path_ = false; - is_safe_static_objects_ = false; is_safe_dynamic_objects_ = false; prev_found_path_ = false; prev_is_safe_dynamic_objects_ = false; has_decided_velocity_ = false; - has_requested_approval_ = false; } DEFINE_SETTER_GETTER(std::shared_ptr, lane_parking_pull_over_path) @@ -118,12 +116,10 @@ class PullOverStatus DEFINE_SETTER_GETTER(lanelet::ConstLanelets, pull_over_lanes) DEFINE_SETTER_GETTER(std::vector, lanes) DEFINE_SETTER_GETTER(bool, has_decided_path) - DEFINE_SETTER_GETTER(bool, is_safe_static_objects) DEFINE_SETTER_GETTER(bool, is_safe_dynamic_objects) DEFINE_SETTER_GETTER(bool, prev_found_path) DEFINE_SETTER_GETTER(bool, prev_is_safe_dynamic_objects) DEFINE_SETTER_GETTER(bool, has_decided_velocity) - DEFINE_SETTER_GETTER(bool, has_requested_approval) DEFINE_SETTER_GETTER(Pose, refined_goal_pose) DEFINE_SETTER_GETTER(Pose, closest_goal_candidate_pose) @@ -137,12 +133,10 @@ class PullOverStatus lanelet::ConstLanelets pull_over_lanes_{}; std::vector lanes_{}; bool has_decided_path_{false}; - bool is_safe_static_objects_{false}; bool is_safe_dynamic_objects_{false}; bool prev_found_path_{false}; bool prev_is_safe_dynamic_objects_{false}; bool has_decided_velocity_{false}; - bool has_requested_approval_{false}; Pose refined_goal_pose_{}; Pose closest_goal_candidate_pose_{}; @@ -220,6 +214,16 @@ class ThreadSafeData return pull_over_path_->isValidPath(); } + PullOverPlannerType getPullOverPlannerType() const + { + const std::lock_guard lock(mutex_); + if (!pull_over_path_) { + return PullOverPlannerType::NONE; + } + + return pull_over_path_->type; + }; + void reset() { const std::lock_guard lock(mutex_); diff --git a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp index 6259408ffaf5a..adc77ac49d58c 100644 --- a/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/goal_planner/goal_planner_module.cpp @@ -802,7 +802,7 @@ void GoalPlannerModule::setStopPathFromCurrentPath(BehaviorModuleOutput & output void GoalPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) const { - if (thread_safe_data_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::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; @@ -953,7 +953,7 @@ BehaviorModuleOutput GoalPlannerModule::planWithGoalModification() path_reference_ = getPreviousModuleOutput().reference_path; // return to lane parking if it is possible - if (thread_safe_data_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { returnToLaneParking(); } @@ -1010,7 +1010,7 @@ BehaviorModuleOutput GoalPlannerModule::planWaitingApprovalWithGoalModification( const auto distance_to_path_change = calcDistanceToPathChange(); // generate drivable area info for new architecture - if (thread_safe_data_.get_pull_over_path()->type == PullOverPlannerType::FREESPACE) { + if (thread_safe_data_.getPullOverPlannerType() == PullOverPlannerType::FREESPACE) { const double drivable_area_margin = planner_data_->parameters.vehicle_width; out.drivable_area_info.drivable_margin = planner_data_->parameters.vehicle_width / 2.0 + drivable_area_margin; @@ -1811,7 +1811,7 @@ void GoalPlannerModule::setDebugData() marker.pose = thread_safe_data_.get_modified_goal_pose() ? thread_safe_data_.get_modified_goal_pose()->goal_pose : planner_data_->self_odometry->pose.pose; - marker.text = magic_enum::enum_name(thread_safe_data_.get_pull_over_path()->type); + marker.text = magic_enum::enum_name(thread_safe_data_.getPullOverPlannerType()); marker.text += " " + std::to_string(status_.get_current_path_idx()) + "/" + std::to_string(thread_safe_data_.get_pull_over_path()->partial_paths.size() - 1); if (isStuck()) {