Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

refactor(goal_planner): move unnecessary member functions #9522

Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
Original file line number Diff line number Diff line change
Expand Up @@ -135,6 +135,43 @@ struct PullOverContextData
std::optional<rclcpp::Time> last_path_idx_increment_time;
};

bool isOnModifiedGoal(
const Pose & current_pose, const std::optional<GoalCandidate> & modified_goal_opt,
const GoalPlannerParameters & parameters);

bool hasPreviousModulePathShapeChanged(
const BehaviorModuleOutput & previous_module_output,
const BehaviorModuleOutput & last_previous_module_output);
bool hasDeviatedFromLastPreviousModulePath(
const PlannerData & planner_data, const BehaviorModuleOutput & last_previous_module_output);
bool hasDeviatedFromCurrentPreviousModulePath(
const PlannerData & planner_data, const BehaviorModuleOutput & previous_module_output);

bool needPathUpdate(
const Pose & current_pose, const double path_update_duration, const rclcpp::Time & now,
const std::optional<GoalCandidate> & modified_goal,
const std::optional<rclcpp::Time> & selected_time, const GoalPlannerParameters & parameters);

bool checkOccupancyGridCollision(
const PathWithLaneId & path,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map);

bool isStopped(
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer,
const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower);

// Flag class for managing whether a certain callback is running in multi-threading
class ScopedFlag
{
public:
explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }

~ScopedFlag() { flag_.store(false); }

private:
std::atomic<bool> & flag_;
};

class GoalPlannerModule : public SceneModuleInterface
{
public:
Expand Down Expand Up @@ -193,18 +230,6 @@ class GoalPlannerModule : public SceneModuleInterface
void processOnExit() override;
void updateData() override;

void updateEgoPredictedPathParams(
std::shared_ptr<EgoPredictedPathParams> & ego_predicted_path_params,
const std::shared_ptr<GoalPlannerParameters> & goal_planner_params);

void updateSafetyCheckParams(
std::shared_ptr<SafetyCheckParams> & safety_check_params,
const std::shared_ptr<GoalPlannerParameters> & goal_planner_params);

void updateObjectsFilteringParams(
std::shared_ptr<ObjectsFilteringParams> & objects_filtering_params,
const std::shared_ptr<GoalPlannerParameters> & goal_planner_params);

void postProcess() override;
void setParameters(const std::shared_ptr<GoalPlannerParameters> & parameters);
void acceptVisitor(
Expand Down Expand Up @@ -257,18 +282,6 @@ class GoalPlannerModule : public SceneModuleInterface
std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
std::mutex gp_planner_data_mutex_;

// Flag class for managing whether a certain callback is running in multi-threading
class ScopedFlag
{
public:
explicit ScopedFlag(std::atomic<bool> & flag) : flag_(flag) { flag_.store(true); }

~ScopedFlag() { flag_.store(false); }

private:
std::atomic<bool> & flag_;
};

/*
* state transitions and plan function used in each state
*
Expand Down Expand Up @@ -359,11 +372,6 @@ class GoalPlannerModule : public SceneModuleInterface
mutable GoalPlannerDebugData debug_data_;
mutable PoseWithString debug_stop_pose_with_info_;

// collision check
bool checkOccupancyGridCollision(
const PathWithLaneId & path,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map) const;

// goal seach
GoalCandidates generateGoalCandidates() const;

Expand All @@ -380,18 +388,8 @@ class GoalPlannerModule : public SceneModuleInterface
double calcSignedArcLengthFromEgo(const PathWithLaneId & path, const Pose & pose) const;

// status
bool isStopped();
bool isStopped(
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer, const double time);
bool hasFinishedCurrentPath(const PullOverContextData & ctx_data);
bool isOnModifiedGoal(
const Pose & current_pose, const std::optional<GoalCandidate> & modified_goal_opt,
const GoalPlannerParameters & parameters) const;
double calcModuleRequestLength() const;
bool needPathUpdate(
const Pose & current_pose, const double path_update_duration,
const std::optional<GoalCandidate> & modified_goal_opt,
const GoalPlannerParameters & parameters) const;
bool isStuck(
const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
const std::shared_ptr<const PlannerData> planner_data,
Expand Down Expand Up @@ -443,16 +441,6 @@ class GoalPlannerModule : public SceneModuleInterface
TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data);
std::optional<lanelet::Id> ignore_signal_{std::nullopt};

bool hasPreviousModulePathShapeChanged(
const BehaviorModuleOutput & previous_module_output,
const BehaviorModuleOutput & last_previous_module_output) const;
bool hasDeviatedFromLastPreviousModulePath(
const std::shared_ptr<const PlannerData> planner_data,
const BehaviorModuleOutput & last_previous_module_output) const;
bool hasDeviatedFromCurrentPreviousModulePath(
const std::shared_ptr<const PlannerData> planner_data,
const BehaviorModuleOutput & previous_module_output) const;

// timer for generating pull over path candidates in a separate thread
void onTimer();
void onFreespaceParkingTimer();
Expand Down
Loading
Loading