Skip to content

Commit

Permalink
compiles
Browse files Browse the repository at this point in the history 
Signed-off-by: Mamoru Sobue <[email protected]>
  • Loading branch information
@soblin
soblin committed Nov 28, 2024
1 parent 2afa115 commit 6161574
Show file tree
Hide file tree
Showing 6 changed files with 953 additions and 938 deletions.
2 changes: 2 additions & 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,8 @@ 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
)

if(BUILD_EXAMPLES)
Expand Down
229 changes: 134 additions & 95 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,7 +18,6 @@
#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/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 @@ -106,32 +105,136 @@ struct PoseWithString
}
};

struct SelectedPullOverPath
{
PullOverPath path;
rclcpp::Time selected_time;
std::optional<rclcpp::Time> last_path_idx_increment_time;
};

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, LaneParkingResponse && lane_parking_response,
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)
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_idx_increment_time;
const bool is_stopped;
const LaneParkingResponse lane_parking_response;
const FreespaceParkingResponse freespace_parking_response;
// TODO(soblin): due to deceleratePath(), this cannot be const member(velocity is modified by it)
std::optional<SelectedPullOverPath> selected_pull_over_path_opt;
};

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 PullOverPath & pull_over_path, const rclcpp::Time & selected_time,
const GoalPlannerParameters & parameters);

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_odomemtry, const double duration_lower);

Check warning on line 171 in planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp

View workflow job for this annotation

GitHub Actions / spell-check-differential 

Unknown word (odomemtry)

// 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 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);

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
Expand All @@ -144,7 +247,7 @@ class GoalPlannerModule : public SceneModuleInterface
std::unordered_map<std::string, std::shared_ptr<ObjectsOfInterestMarkerInterface>> &
objects_of_interest_marker_interface_ptr_map);

~GoalPlannerModule()
~GoalPlannerModule() override
{
if (lane_parking_timer_) {
lane_parking_timer_->cancel();
Expand Down Expand Up @@ -192,18 +295,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 All @@ -213,20 +304,15 @@ class GoalPlannerModule : public SceneModuleInterface
CandidateOutput planCandidate() const override { return CandidateOutput{}; }

private:
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); }
std::pair<LaneParkingResponse, FreespaceParkingResponse> syncWithThreads();

~ScopedFlag() { flag_.store(false); }
std::mutex lane_parking_mutex_;
std::optional<LaneParkingRequest> lane_parking_request_;
LaneParkingResponse lane_parking_response_;

private:
std::atomic<bool> & flag_;
};
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 @@ -259,15 +345,12 @@ class GoalPlannerModule : public SceneModuleInterface

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_;
std::shared_ptr<EgoPredictedPathParams> ego_predicted_path_params_;
std::shared_ptr<ObjectsFilteringParams> objects_filtering_params_;
std::shared_ptr<SafetyCheckParams> safety_check_params_;

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 @@ -285,11 +368,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 @@ -318,11 +396,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 @@ -339,24 +412,9 @@ 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,
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 @@ -367,20 +425,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 planPullOverAsCandidate(const PullOverContextData & context_data);
BehaviorModuleOutput planPullOver(PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsOutput(PullOverContextData & context_data);
BehaviorModuleOutput planPullOverAsCandidate(PullOverContextData & context_data);
std::optional<PullOverPath> selectPullOverPath(
const PullOverContextData & context_data,
const std::vector<PullOverPath> & pull_over_path_candidates,
Expand All @@ -392,7 +443,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 @@ -402,20 +455,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();

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

0 comments on commit 6161574

Please sign in to comment.