Skip to content

Commit

Permalink
Merge pull request #1790 from tier4/cherry-pick/bus_stop_area
Browse files Browse the repository at this point in the history 
chore: cherry-pick/bus_stop_area
  • Loading branch information
@rej55
rej55 authored Feb 7, 2025
2 parents e7dda0f + 0d87929 commit 3f791c5
Show file tree
Hide file tree
Showing 6 changed files with 214 additions and 282 deletions.
129 changes: 53 additions & 76 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,69 +298,48 @@ 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
std::shared_ptr<GoalSearcherBase> goal_searcher_;
GoalCandidates goal_candidates_{};

bool use_bus_stop_area_{false};

// NOTE: this is latest occupancy_grid_map pointer which the local planner_data on
// onFreespaceParkingTimer thread storage may point to while calculation.
// onTimer/onFreespaceParkingTimer and their callees MUST NOT use this
Expand All @@ -380,8 +349,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,24 +360,40 @@ 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;
GoalCandidates generateGoalCandidates(const bool use_bus_stop_area) 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;
Expand All @@ -427,7 +410,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 +463,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 +474,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
3 changes: 2 additions & 1 deletion ..._goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,8 @@ class GoalSearcher : public GoalSearcherBase
public:
GoalSearcher(const GoalPlannerParameters & parameters, const LinearRing2d & vehicle_footprint);

GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) override;
GoalCandidates search(
const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area) override;
void update(
GoalCandidates & goal_candidates,
const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
Expand Down
3 changes: 2 additions & 1 deletion ..._planner_module/include/autoware/behavior_path_goal_planner_module/goal_searcher_base.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -47,7 +47,8 @@ class GoalSearcherBase
virtual ~GoalSearcherBase() = default;

MultiPolygon2d getAreaPolygons() const { return area_polygons_; }
virtual GoalCandidates search(const std::shared_ptr<const PlannerData> & planner_data) = 0;
virtual GoalCandidates search(
const std::shared_ptr<const PlannerData> & planner_data, const bool use_bus_stop_area) = 0;
virtual void update(
[[maybe_unused]] GoalCandidates & goal_candidates,
[[maybe_unused]] const std::shared_ptr<OccupancyGridBasedCollisionDetector> occupancy_grid_map,
Expand Down
5 changes: 4 additions & 1 deletion ...th_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,9 +33,11 @@ class LaneParkingRequest
public:
LaneParkingRequest(
const autoware::universe_utils::LinearRing2d & vehicle_footprint,
const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output)
const GoalCandidates & goal_candidates, const BehaviorModuleOutput & upstream_module_output,
const bool use_bus_stop_area)
: vehicle_footprint_(vehicle_footprint),
goal_candidates_(goal_candidates),
use_bus_stop_area_(use_bus_stop_area),
upstream_module_output_(upstream_module_output)
{
}
Expand All @@ -48,6 +50,7 @@ class LaneParkingRequest

const autoware::universe_utils::LinearRing2d vehicle_footprint_;
const GoalCandidates goal_candidates_;
const bool use_bus_stop_area_;

const std::shared_ptr<PlannerData> & get_planner_data() const { return planner_data_; }
const ModuleStatus & get_current_status() const { return current_status_; }
Expand Down
Loading

0 comments on commit 3f791c5

Please sign in to comment.