Skip to content

Commit

Permalink
refactor(goal_planner): make parameters const (#10043)
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin authored Jan 30, 2025
1 parent 04ed652 commit 0d030d7
Show file tree
Hide file tree
Showing 2 changed files with 182 additions and 271 deletions.
125 changes: 50 additions & 75 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -288,17 +288,7 @@ class GoalPlannerModule : public SceneModuleInterface
getLogger(), *clock_, 1000, "lane parking and freespace parking callbacks finished");
}

void updateModuleParams(const std::any & parameters) override
{
parameters_ = std::any_cast<std::shared_ptr<GoalPlannerParameters>>(parameters);
if (parameters_->safety_check_params.enable_safety_check) {
ego_predicted_path_params_ =
std::make_shared<EgoPredictedPathParams>(parameters_->ego_predicted_path_params);
objects_filtering_params_ =
std::make_shared<ObjectsFilteringParams>(parameters_->objects_filtering_params);
safety_check_params_ = std::make_shared<SafetyCheckParams>(parameters_->safety_check_params);
}
}
void updateModuleParams([[maybe_unused]] const std::any & parameters) override {}

BehaviorModuleOutput plan() override;
BehaviorModuleOutput planWaitingApproval() override;
Expand All @@ -308,63 +298,40 @@ class GoalPlannerModule : public SceneModuleInterface
void updateData() override;

void postProcess() override;
void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
void acceptVisitor(
[[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
{
}
CandidateOutput planCandidate() const override { return CandidateOutput{}; }

private:
std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();
const GoalPlannerParameters parameters_;
const EgoPredictedPathParams & ego_predicted_path_params_ = parameters_.ego_predicted_path_params;
const ObjectsFilteringParams & objects_filtering_params_ = parameters_.objects_filtering_params;
const SafetyCheckParams safety_check_params_ = parameters_.safety_check_params;

const autoware::vehicle_info_utils::VehicleInfo vehicle_info_;
const autoware::universe_utils::LinearRing2d vehicle_footprint_;

const bool left_side_parking_;

bool trigger_thread_on_approach_{false};
// pre-generate lane parking paths in a separate thread
rclcpp::TimerBase::SharedPtr lane_parking_timer_;
rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
std::atomic<bool> is_lane_parking_cb_running_;
// NOTE: never access to following variables except in updateData()!!!
std::mutex lane_parking_mutex_;
std::optional<LaneParkingRequest> lane_parking_request_;
LaneParkingResponse lane_parking_response_;

// generate freespace parking paths in a separate thread
rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
std::atomic<bool> is_freespace_parking_cb_running_;
std::mutex freespace_parking_mutex_;
std::optional<FreespaceParkingRequest> freespace_parking_request_;
FreespaceParkingResponse freespace_parking_response_;

/*
* state transitions and plan function used in each state
*
* +--------------------------+
* | RUNNING |
* | planPullOverAsCandidate()|
* +------------+-------------+
* | hasDecidedPath()
* 2 v
* +--------------------------+
* | WAITING_APPROVAL |
* | planPullOverAsCandidate()|
* +------------+-------------+
* | isActivated()
* 3 v
* +--------------------------+
* | RUNNING |
* | planPullOverAsOutput() |
* +--------------------------+
*/

// The start_planner activates when it receives a new route,
// so there is no need to terminate the goal planner.
// If terminating it, it may switch to lane following and could generate an inappropriate path.
bool canTransitSuccessState() override { return false; }
bool canTransitFailureState() override { return false; }

mutable StartGoalPlannerData goal_planner_data_;

std::shared_ptr<GoalPlannerParameters> parameters_;

mutable std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
mutable std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
mutable std::shared_ptr<SafetyCheckParams> safety_check_params_;

autoware::vehicle_info_utils::VehicleInfo vehicle_info_{};

std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;

// goal searcher
Expand All @@ -380,8 +347,6 @@ class GoalPlannerModule : public SceneModuleInterface
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stopped_;
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> odometry_buffer_stuck_;

autoware::universe_utils::LinearRing2d vehicle_footprint_;

std::optional<PullOverContextData> context_data_{std::nullopt};
// path_decision_controller is updated in updateData(), and used in plan()
PathDecisionStateController path_decision_controller_{getLogger()};
Expand All @@ -393,25 +358,41 @@ class GoalPlannerModule : public SceneModuleInterface
// ego may exceed the stop distance, so add a buffer
const double stop_distance_buffer_{2.0};

// for parking policy
bool left_side_parking_{true};

// pre-generate lane parking paths in a separate thread
rclcpp::TimerBase::SharedPtr lane_parking_timer_;
rclcpp::CallbackGroup::SharedPtr lane_parking_timer_cb_group_;
std::atomic<bool> is_lane_parking_cb_running_;

// generate freespace parking paths in a separate thread
rclcpp::TimerBase::SharedPtr freespace_parking_timer_;
rclcpp::CallbackGroup::SharedPtr freespace_parking_timer_cb_group_;
std::atomic<bool> is_freespace_parking_cb_running_;

// debug
mutable GoalPlannerDebugData debug_data_;

// goal seach
GoalCandidates generateGoalCandidates() const;

/*
* state transitions and plan function used in each state
*
* +--------------------------+
* | RUNNING |
* | planPullOverAsCandidate()|
* +------------+-------------+
* | hasDecidedPath()
* 2 v
* +--------------------------+
* | WAITING_APPROVAL |
* | planPullOverAsCandidate()|
* +------------+-------------+
* | isActivated()
* 3 v
* +--------------------------+
* | RUNNING |
* | planPullOverAsOutput() |
* +--------------------------+
*/

// The start_planner activates when it receives a new route,
// so there is no need to terminate the goal planner.
// If terminating it, it may switch to lane following and could generate an inappropriate path.
bool canTransitSuccessState() override { return false; }
bool canTransitFailureState() override { return false; }

std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();

// stop or decelerate
void deceleratePath(PullOverPath & pull_over_path) const;
void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const;
Expand All @@ -427,7 +408,7 @@ class GoalPlannerModule : public SceneModuleInterface
double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;

// status
bool hasFinishedCurrentPath(const PullOverContextData & ctx_data);
bool hasFinishedCurrentPath(const PullOverContextData & ctx_data) const;
double calcModuleRequestLength() const;
void decideVelocity(PullOverPath & pull_over_path);
void updateStatus(const BehaviorModuleOutput & output);
Expand Down Expand Up @@ -480,9 +461,6 @@ class GoalPlannerModule : public SceneModuleInterface
std::pair<double, double> calcDistanceToPathChange(
const PullOverContextData & context_data) const;

// safety check
void initializeSafetyCheckParameters();
SafetyCheckParams createSafetyCheckParams() const;
/*
void updateSafetyCheckTargetObjectsData(
const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane,
Expand All @@ -494,11 +472,8 @@ class GoalPlannerModule : public SceneModuleInterface
*/
std::pair<bool, utils::path_safety_checker::CollisionCheckDebugMap> isSafePath(
const std::shared_ptr<const PlannerData> planner_data, const bool found_pull_over_path,
const std::optional<PullOverPath> & pull_over_path_opt, const PathDecisionState & prev_data,
const GoalPlannerParameters & parameters,
const std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<SafetyCheckParams> & safety_check_params) const;
const std::optional<PullOverPath> & pull_over_path_opt,
const PathDecisionState & prev_data) const;

// debug
void setDebugData(const PullOverContextData & context_data);
Expand Down
Loading

0 comments on commit 0d030d7

Please sign in to comment.