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.

Sign up for GitHub

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

feat(goal_planner): divide Planners to isolated threads #9514

Merged
merged 10 commits into from
Dec 15, 2024
Merged

feat(goal_planner): divide Planners to isolated threads #9514

Show file tree
Hide file tree
Changes from 6 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
4a98ca9
feat(goal_planner): disable freespace feature temporarily
soblin Nov 25, 2024
cdac350
revert SelectedPullOverPath
soblin Dec 2, 2024
362d70a
does not do pullover
soblin Dec 12, 2024
4edb7a1
seems working
soblin Dec 12, 2024
b4c7d79
fix
soblin Dec 13, 2024
d42fc0d
reflect comment
soblin Dec 13, 2024
bc1eb5f
remove ogm-based-collision-detecor from Freespace in the future
soblin Dec 13, 2024
181415e
reflect comment2
soblin Dec 13, 2024
f0f215c
Merge branch 'main' into feat/goal-planner/divide-thread
soblin Dec 15, 2024
83e3f8f
fix ci
soblin Dec 15, 2024
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
1 change: 1 addition & 0 deletions planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ ament_auto_add_library(${PROJECT_NAME} SHARED
src/util.cpp
src/goal_planner_module.cpp
src/manager.cpp
src/thread_data.cpp
src/decision_state.cpp
)

Expand Down
194 changes: 107 additions & 87 deletions ...planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -18,9 +18,7 @@
#include "autoware/behavior_path_goal_planner_module/decision_state.hpp"
#include "autoware/behavior_path_goal_planner_module/fixed_goal_planner_base.hpp"
#include "autoware/behavior_path_goal_planner_module/goal_planner_parameters.hpp"
#include "autoware/behavior_path_goal_planner_module/goal_searcher.hpp"
#include "autoware/behavior_path_goal_planner_module/thread_data.hpp"
#include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
#include "autoware/behavior_path_planner_common/utils/parking_departure/common_module_data.hpp"
#include "autoware/behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"

Expand Down Expand Up @@ -89,27 +87,44 @@ struct PullOverContextData
PullOverContextData() = delete;
explicit PullOverContextData(
const bool is_stable_safe_path, const PredictedObjects & static_objects,
const PredictedObjects & dynamic_objects, std::optional<PullOverPath> && pull_over_path_opt,
const std::vector<PullOverPath> & pull_over_path_candidates,
const PathDecisionState & prev_state)
const PredictedObjects & dynamic_objects, const PathDecisionState & prev_state,
const bool is_stopped, const LaneParkingResponse & lane_parking_response,
const FreespaceParkingResponse & freespace_parking_response)
: is_stable_safe_path(is_stable_safe_path),
static_target_objects(static_objects),
dynamic_target_objects(dynamic_objects),
pull_over_path_opt(pull_over_path_opt),
pull_over_path_candidates(pull_over_path_candidates),
prev_state_for_debug(prev_state),
last_path_idx_increment_time(std::nullopt)
is_stopped(is_stopped),
lane_parking_response(lane_parking_response),
freespace_parking_response(freespace_parking_response)
{
}
const bool is_stable_safe_path;
const PredictedObjects static_target_objects;
const PredictedObjects dynamic_target_objects;
// TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it)
// TODO(soblin): make following variables private
bool is_stable_safe_path;
PredictedObjects static_target_objects;
PredictedObjects dynamic_target_objects;
PathDecisionState prev_state_for_debug;
bool is_stopped;
LaneParkingResponse lane_parking_response;
FreespaceParkingResponse freespace_parking_response;
std::optional<PullOverPath> pull_over_path_opt;
const std::vector<PullOverPath> pull_over_path_candidates;
// TODO(soblin): goal_candidate_from_thread, modifed_goal_pose_from_thread, closest_start_pose
const PathDecisionState prev_state_for_debug;
std::optional<rclcpp::Time> last_path_update_time;
std::optional<rclcpp::Time> last_path_idx_increment_time;

void update(
const bool is_stable_safe_path_, const PredictedObjects static_target_objects_,
const PredictedObjects dynamic_target_objects_, const PathDecisionState prev_state_for_debug_,
const bool is_stopped_, const LaneParkingResponse & lane_parking_response_,
const FreespaceParkingResponse & freespace_parking_response_)
{
is_stable_safe_path = is_stable_safe_path_;
static_target_objects = static_target_objects_;
dynamic_target_objects = dynamic_target_objects_;
prev_state_for_debug = prev_state_for_debug_;
is_stopped = is_stopped_;
lane_parking_response = lane_parking_response_;
freespace_parking_response = freespace_parking_response_;
}
};

bool isOnModifiedGoal(
Expand All @@ -133,6 +148,11 @@ bool checkOccupancyGridCollision(
const PathWithLaneId & path,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map);

// freespace parking
std::optional<PullOverPath> planFreespacePath(
const FreespaceParkingRequest & req, const PredictedObjects & static_target_objects,
std::shared_ptr<PullOverPlannerBase> freespace_planner);

bool isStopped(
std::deque<nav_msgs::msg::Odometry::ConstSharedPtr> & odometry_buffer,
const nav_msgs::msg::Odometry::ConstSharedPtr self_odometry, const double duration_lower,
Expand All @@ -150,6 +170,61 @@ class ScopedFlag
std::atomic<bool> & flag_;
};

class LaneParkingPlanner
{
public:
LaneParkingPlanner(
rclcpp::Node & node, std::mutex & lane_parking_mutex,
const std::optional<LaneParkingRequest> & request, LaneParkingResponse & response,
std::atomic<bool> & is_lane_parking_cb_running, const rclcpp::Logger & logger,
const GoalPlannerParameters & parameters);
rclcpp::Logger getLogger() const { return logger_; }
void onTimer();

private:
std::mutex & mutex_;
const std::optional<LaneParkingRequest> & request_;
LaneParkingResponse & response_;
std::atomic<bool> & is_lane_parking_cb_running_;
rclcpp::Logger logger_;

std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

nits:
(I know this is just using previous code. but we can use unique_ptr or just vector. I want refactor in the future)

};

class FreespaceParkingPlanner
{
public:
FreespaceParkingPlanner(
std::mutex & freespace_parking_mutex, const std::optional<FreespaceParkingRequest> & request,
FreespaceParkingResponse & response, std::atomic<bool> & is_freespace_parking_cb_running,
const rclcpp::Logger & logger, const rclcpp::Clock::SharedPtr clock,
const std::shared_ptr<PullOverPlannerBase> freespace_planner)
: mutex_(freespace_parking_mutex),
request_(request),
response_(response),
is_freespace_parking_cb_running_(is_freespace_parking_cb_running),
logger_(logger),
clock_(clock),
freespace_planner_(freespace_planner)
{
}
void onTimer();

private:
std::mutex & mutex_;
const std::optional<FreespaceParkingRequest> & request_;
FreespaceParkingResponse & response_;
std::atomic<bool> & is_freespace_parking_cb_running_;
rclcpp::Logger logger_;
rclcpp::Clock::SharedPtr clock_;

std::shared_ptr<PullOverPlannerBase> freespace_planner_;

bool isStuck(
const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
const FreespaceParkingRequest & req) const;
};

class GoalPlannerModule : public SceneModuleInterface
{
public:
Expand Down Expand Up @@ -217,48 +292,16 @@ class GoalPlannerModule : public SceneModuleInterface
CandidateOutput planCandidate() const override { return CandidateOutput{}; }

private:
/**
* @brief shared data for onTimer(onTimer/onFreespaceParkingTimer just read this)
*/
struct GoalPlannerData
{
GoalPlannerData(
const PlannerData & planner_data, const GoalPlannerParameters & parameters,
const BehaviorModuleOutput & previous_module_output_)
{
initializeOccupancyGridMap(planner_data, parameters);
previous_module_output = previous_module_output_;
last_previous_module_output = previous_module_output_;
};
GoalPlannerParameters parameters;
autoware::universe_utils::LinearRing2d vehicle_footprint;

PlannerData planner_data;
ModuleStatus current_status;
BehaviorModuleOutput previous_module_output;
BehaviorModuleOutput last_previous_module_output; //<! previous "previous_module_output"
GoalCandidates goal_candidates; //<! only the positional information of goal_candidates

// collision detector
// need to be shared_ptr to be used in planner and goal searcher
std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map;

const BehaviorModuleOutput & getPreviousModuleOutput() const { return previous_module_output; }
const ModuleStatus & getCurrentStatus() const { return current_status; }
void updateOccupancyGrid();
GoalPlannerData clone() const;
void update(
const GoalPlannerParameters & parameters, const PlannerData & planner_data,
const ModuleStatus & current_status, const BehaviorModuleOutput & previous_module_output,
const autoware::universe_utils::LinearRing2d & vehicle_footprint,
const GoalCandidates & goal_candidates);

private:
void initializeOccupancyGridMap(
const PlannerData & planner_data, const GoalPlannerParameters & parameters);
};
std::optional<GoalPlannerData> gp_planner_data_{std::nullopt};
std::mutex gp_planner_data_mutex_;
Comment on lines -260 to -261
Copy link
Contributor

@kosuke55 kosuke55 Dec 13, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

gp_planner_data is deleted but still remain in comments. please modify or remove comments if they are not necessary.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

delete in d42fc0d

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

// NOTE: never access to following variables except in updateData()!!!
std::mutex lane_parking_mutex_;
std::optional<LaneParkingRequest> lane_parking_request_;
LaneParkingResponse lane_parking_response_;

std::mutex freespace_parking_mutex_;
std::optional<FreespaceParkingRequest> freespace_parking_request_;
FreespaceParkingResponse freespace_parking_response_;

/*
* state transitions and plan function used in each state
Expand Down Expand Up @@ -297,9 +340,6 @@ class GoalPlannerModule : public SceneModuleInterface

autoware::vehicle_info_utils::VehicleInfo vehicle_info_{};

// planner
std::vector<std::shared_ptr<PullOverPlannerBase>> pull_over_planners_;
std::unique_ptr<PullOverPlannerBase> freespace_planner_;
std::unique_ptr<FixedGoalPlannerBase> fixed_goal_planner_;

// goal searcher
Expand All @@ -317,11 +357,6 @@ class GoalPlannerModule : public SceneModuleInterface

autoware::universe_utils::LinearRing2d vehicle_footprint_;

std::recursive_mutex mutex_;
mutable ThreadSafeData thread_safe_data_;

// TODO(soblin): organize part of thread_safe_data and previous data to PullOverContextData
// context_data_ is initialized in updateData(), used in plan() and refreshed in postProcess()
std::optional<PullOverContextData> context_data_{std::nullopt};
// path_decision_controller is updated in updateData(), and used in plan()
PathDecisionStateController path_decision_controller_{getLogger()};
Expand Down Expand Up @@ -369,12 +404,7 @@ class GoalPlannerModule : public SceneModuleInterface
// status
bool hasFinishedCurrentPath(const PullOverContextData & ctx_data);
double calcModuleRequestLength() const;
bool isStuck(
const PredictedObjects & static_target_objects, const PredictedObjects & dynamic_target_objects,
const std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const GoalPlannerParameters & parameters);
void decideVelocity();
void decideVelocity(PullOverPath & pull_over_path);
void updateStatus(const BehaviorModuleOutput & output);

// validation
Expand All @@ -385,21 +415,13 @@ class GoalPlannerModule : public SceneModuleInterface
bool isCrossingPossible(
const Pose & start_pose, const Pose & end_pose, const lanelet::ConstLanelets lanes) const;
bool isCrossingPossible(const PullOverPath & pull_over_path) const;
bool hasEnoughTimePassedSincePathUpdate(const double duration) const;

// freespace parking
bool planFreespacePath(
std::shared_ptr<const PlannerData> planner_data,
const std::shared_ptr<GoalSearcherBase> goal_searcher, const GoalCandidates & goal_candidates,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
const PredictedObjects & static_target_objects);
bool canReturnToLaneParking(const PullOverContextData & context_data);

// plan pull over path
BehaviorModuleOutput planPullOver(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsOutput(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOver(PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsOutput(PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsCandidate(
const PullOverContextData & context_data, const std::string & detail);
PullOverContextData & context_data, const std::string & detail);
std::optional<PullOverPath> selectPullOverPath(
const PullOverContextData & context_data,
const std::vector<PullOverPath> & pull_over_path_candidates,
Expand All @@ -411,7 +433,9 @@ class GoalPlannerModule : public SceneModuleInterface
const PullOverContextData & context_data, BehaviorModuleOutput & output) const;

// output setter
void setOutput(const PullOverContextData & context_data, BehaviorModuleOutput & output);
void setOutput(
const std::optional<PullOverPath> selected_pull_over_path_with_velocity_opt,
const PullOverContextData & context_data, BehaviorModuleOutput & output);

void setModifiedGoal(
const PullOverContextData & context_data, BehaviorModuleOutput & output) const;
Expand All @@ -421,10 +445,6 @@ class GoalPlannerModule : public SceneModuleInterface
TurnSignalInfo calcTurnSignalInfo(const PullOverContextData & context_data);
std::optional<lanelet::Id> ignore_signal_{std::nullopt};

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

// steering factor
void updateSteeringFactor(
const PullOverContextData & context_data, const std::array<Pose, 2> & pose,
Expand Down
Loading
Loading