Skip to content

Commit

Permalink
feat(goal_planner): divide Planners to isolated threads (autowarefoun…
Browse files Browse the repository at this point in the history 
…dation#9514)

Signed-off-by: Mamoru Sobue <mamoru.sobue@tier4.jp>
  • Loading branch information
@soblin
soblin authored Dec 15, 2024
1 parent 9ff1c38 commit 657c894
Show file tree
Hide file tree
Showing 7 changed files with 568 additions and 535 deletions.
1 change: 1 addition & 0 deletions planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/CMakeLists.txt
View file
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
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_;
};

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_;
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

0 comments on commit 657c894

Please sign in to comment.