From 0d030d7cc7fbe0543b45d5f996b5259d6876ab9c Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Thu, 30 Jan 2025 14:14:11 +0900 Subject: [PATCH] refactor(goal_planner): make parameters const (#10043) Signed-off-by: Mamoru Sobue --- .../goal_planner_module.hpp | 125 +++---- .../src/goal_planner_module.cpp | 328 +++++++----------- 2 files changed, 182 insertions(+), 271 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp index f2cc7e2f4ef65..faeadf32ffd5f 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/goal_planner_module.hpp @@ -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>(parameters); - if (parameters_->safety_check_params.enable_safety_check) { - ego_predicted_path_params_ = - std::make_shared(parameters_->ego_predicted_path_params); - objects_filtering_params_ = - std::make_shared(parameters_->objects_filtering_params); - safety_check_params_ = std::make_shared(parameters_->safety_check_params); - } - } + void updateModuleParams([[maybe_unused]] const std::any & parameters) override {} BehaviorModuleOutput plan() override; BehaviorModuleOutput planWaitingApproval() override; @@ -308,7 +298,6 @@ class GoalPlannerModule : public SceneModuleInterface void updateData() override; void postProcess() override; - void setParameters(const std::shared_ptr & parameters); void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override { @@ -316,55 +305,33 @@ class GoalPlannerModule : public SceneModuleInterface CandidateOutput planCandidate() const override { return CandidateOutput{}; } private: - std::pair 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 is_lane_parking_cb_running_; // NOTE: never access to following variables except in updateData()!!! std::mutex lane_parking_mutex_; std::optional 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 is_freespace_parking_cb_running_; std::mutex freespace_parking_mutex_; std::optional 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 parameters_; - - mutable std::shared_ptr ego_predicted_path_params_; - mutable std::shared_ptr objects_filtering_params_; - mutable std::shared_ptr safety_check_params_; - - autoware::vehicle_info_utils::VehicleInfo vehicle_info_{}; - std::unique_ptr fixed_goal_planner_; // goal searcher @@ -380,8 +347,6 @@ class GoalPlannerModule : public SceneModuleInterface std::deque odometry_buffer_stopped_; std::deque odometry_buffer_stuck_; - autoware::universe_utils::LinearRing2d vehicle_footprint_; - std::optional context_data_{std::nullopt}; // path_decision_controller is updated in updateData(), and used in plan() PathDecisionStateController path_decision_controller_{getLogger()}; @@ -393,25 +358,41 @@ 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 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 is_freespace_parking_cb_running_; - // debug mutable GoalPlannerDebugData debug_data_; // goal seach GoalCandidates generateGoalCandidates() 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 syncWithThreads(); + // stop or decelerate void deceleratePath(PullOverPath & pull_over_path) const; void decelerateForTurnSignal(const Pose & stop_pose, PathWithLaneId & path) const; @@ -427,7 +408,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); @@ -480,9 +461,6 @@ class GoalPlannerModule : public SceneModuleInterface std::pair 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, @@ -494,11 +472,8 @@ class GoalPlannerModule : public SceneModuleInterface */ std::pair isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params) const; + const std::optional & pull_over_path_opt, + const PathDecisionState & prev_data) const; // debug void setDebugData(const PullOverContextData & context_data); diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp index 448429b9a3b0b..fa2cef68ed3e7 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/goal_planner_module.cpp @@ -72,23 +72,26 @@ GoalPlannerModule::GoalPlannerModule( objects_of_interest_marker_interface_ptr_map, const std::shared_ptr planning_factor_interface) : SceneModuleInterface{name, node, rtc_interface_ptr_map, objects_of_interest_marker_interface_ptr_map, planning_factor_interface}, // NOLINT - parameters_{parameters}, + parameters_{*parameters}, vehicle_info_{autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo()}, + vehicle_footprint_{vehicle_info_.createFootprint()}, + left_side_parking_{parameters_.parking_policy == ParkingPolicy::LEFT_SIDE}, is_lane_parking_cb_running_{false}, is_freespace_parking_cb_running_{false} { - occupancy_grid_map_ = std::make_shared(); + // TODO(soblin): remove safety_check_params.enable_safety_check because it does not make sense + if (!parameters_.safety_check_params.enable_safety_check) { + throw std::domain_error("goal_planner never works without safety check"); + } - left_side_parking_ = parameters_->parking_policy == ParkingPolicy::LEFT_SIDE; + occupancy_grid_map_ = std::make_shared(); // planner when goal modification is not allowed fixed_goal_planner_ = std::make_unique(); // set selected goal searcher // currently there is only one goal_searcher_type - const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo(); - vehicle_footprint_ = vehicle_info.createFootprint(); - goal_searcher_ = std::make_shared(*parameters, vehicle_footprint_); + goal_searcher_ = std::make_shared(parameters_, vehicle_footprint_); // timer callback for generating lane parking candidate paths const auto lane_parking_period_ns = rclcpp::Rate(1.0).period(); @@ -98,13 +101,13 @@ GoalPlannerModule::GoalPlannerModule( &node, clock_, lane_parking_period_ns, [lane_parking_executor = std::make_unique( node, lane_parking_mutex_, lane_parking_request_, lane_parking_response_, - is_lane_parking_cb_running_, getLogger(), *parameters_)]() { + is_lane_parking_cb_running_, getLogger(), parameters_)]() { lane_parking_executor->onTimer(); }, lane_parking_timer_cb_group_); // freespace parking - if (parameters_->enable_freespace_parking) { + if (parameters_.enable_freespace_parking) { auto freespace_planner = std::make_shared(node, *parameters); const auto freespace_parking_period_ns = rclcpp::Rate(1.0).period(); freespace_parking_timer_cb_group_ = @@ -118,18 +121,6 @@ GoalPlannerModule::GoalPlannerModule( }, freespace_parking_timer_cb_group_); } - - // Initialize safety checker - if (parameters_->safety_check_params.enable_safety_check) { - initializeSafetyCheckParameters(); - } - - /** - * NOTE: Add `universe_utils::ScopedTimeTrack st(__func__, *time_keeper_);` to functions called - * from the main thread only. - * If you want to see the processing time tree in console, uncomment the following line - */ - // time_keeper_->add_reporter(&std::cerr); } bool isOnModifiedGoal( @@ -698,14 +689,14 @@ std::pair GoalPlannerModule::sync std::lock_guard guard(freespace_parking_mutex_); if (!freespace_parking_request_) { freespace_parking_request_.emplace( - *parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); + parameters_, vehicle_footprint_, goal_candidates_, *planner_data_); } constexpr double stuck_time = 5.0; freespace_parking_request_.value().update( *planner_data_, getCurrentStatus(), pull_over_path, last_path_update_time, isStopped( odometry_buffer_stuck_, planner_data_->self_odometry, stuck_time, - parameters_->th_stopped_velocity)); + parameters_.th_stopped_velocity)); // TODO(soblin): currently, ogm-based-collision-detector is updated to latest in // freespace_parking_request_.value().update, and it is shared with goal_planner_module. Next, // goal_planner_module update it and pass it to freespace_parking_request. @@ -736,7 +727,7 @@ void GoalPlannerModule::updateData() const double self_to_goal_arc_length = utils::getSignedDistance( planner_data_->self_odometry->pose.pose, planner_data_->route_handler->getOriginalGoalPose(), current_lanes); - if (self_to_goal_arc_length < parameters_->pull_over_prepare_length) { + if (self_to_goal_arc_length < parameters_.pull_over_prepare_length) { trigger_thread_on_approach_ = true; [[maybe_unused]] const auto send_only_request = syncWithThreads(); RCLCPP_INFO( @@ -770,21 +761,20 @@ void GoalPlannerModule::updateData() // save "old" state const auto prev_decision_state = path_decision_controller_.get_current_state(); - const auto [is_current_safe, collision_check_map] = isSafePath( - planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state, *parameters_, - ego_predicted_path_params_, objects_filtering_params_, safety_check_params_); + const auto [is_current_safe, collision_check_map] = + isSafePath(planner_data_, found_pull_over_path, pull_over_path_recv, prev_decision_state); debug_data_.collision_check = collision_check_map; // update to latest state // extract static and dynamic objects in extraction polygon for path collision check const auto dynamic_target_objects = goal_planner_utils::extract_dynamic_objects( - *(planner_data_->dynamic_object), *(planner_data_->route_handler), *parameters_, + *(planner_data_->dynamic_object), *(planner_data_->route_handler), parameters_, planner_data_->parameters.vehicle_width); const auto static_target_objects = utils::path_safety_checker::filterObjectsByVelocity( - dynamic_target_objects, parameters_->th_moving_object_velocity); + dynamic_target_objects, parameters_.th_moving_object_velocity); path_decision_controller_.transit_state( found_pull_over_path, clock_->now(), static_target_objects, dynamic_target_objects, - modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, *parameters_, + modified_goal_pose, planner_data_, occupancy_grid_map_, is_current_safe, parameters_, goal_searcher_, isActivated(), pull_over_path_recv, debug_data_.ego_polygons_expanded); auto [lane_parking_response, freespace_parking_response] = syncWithThreads(); @@ -793,16 +783,16 @@ void GoalPlannerModule::updateData() path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, dynamic_target_objects, prev_decision_state, isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity), + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, + parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } else { context_data_.emplace( path_decision_controller_.get_current_state().is_stable_safe, static_target_objects, dynamic_target_objects, prev_decision_state, isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity), + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, + parameters_.th_stopped_velocity), std::move(lane_parking_response), std::move(freespace_parking_response)); } auto & ctx_data = context_data_.value(); @@ -828,15 +818,6 @@ void GoalPlannerModule::updateData() } } -void GoalPlannerModule::initializeSafetyCheckParameters() -{ - ego_predicted_path_params_ = - std::make_shared(parameters_->ego_predicted_path_params); - safety_check_params_ = std::make_shared(parameters_->safety_check_params); - objects_filtering_params_ = - std::make_shared(parameters_->objects_filtering_params); -} - void GoalPlannerModule::processOnExit() { resetPathCandidate(); @@ -879,7 +860,7 @@ bool GoalPlannerModule::isExecutionRequested() const utils::getSignedDistance(current_pose, goal_pose, current_lanes); const double request_length = utils::isAllowedGoalModification(route_handler) ? calcModuleRequestLength() - : parameters_->pull_over_minimum_request_length; + : parameters_.pull_over_minimum_request_length; if (self_to_goal_arc_length < 0.0 || self_to_goal_arc_length > request_length) { // if current position is far from goal or behind goal, do not execute goal_planner return false; @@ -889,8 +870,8 @@ bool GoalPlannerModule::isExecutionRequested() const // (A) target lane is `road` and same to the current lanes // (B) target lane is `road_shoulder` and neighboring to the current lanes const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + *(route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); lanelet::ConstLanelet target_lane{}; lanelet::utils::query::getClosestLanelet(pull_over_lanes, goal_pose, &target_lane); @@ -918,7 +899,7 @@ bool GoalPlannerModule::isExecutionReady() const // NOTE(soblin): at least in goal_planner, isExecutionReady is called via super::updateRTCStatus // from self::postProcess, so returning cached member variable like // path_decision_controller.get_current_state() is valid - if (parameters_->safety_check_params.enable_safety_check && isWaitingApproval()) { + if (isWaitingApproval()) { if (!path_decision_controller_.get_current_state().is_stable_safe) { RCLCPP_INFO_THROTTLE( getLogger(), *clock_, 5000, @@ -932,9 +913,9 @@ bool GoalPlannerModule::isExecutionReady() const double GoalPlannerModule::calcModuleRequestLength() const { const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (!min_stop_distance) { - return parameters_->pull_over_minimum_request_length; + return parameters_.pull_over_minimum_request_length; } // The module is requested at a distance such that the ego can stop for the pull over start point @@ -943,18 +924,18 @@ double GoalPlannerModule::calcModuleRequestLength() const // min_stop_distance is used as is, so scale is applied to provide a buffer. constexpr double scale_factor_for_buffer = 1.2; const double minimum_request_length = *min_stop_distance * scale_factor_for_buffer + - parameters_->backward_goal_search_length + + parameters_.backward_goal_search_length + approximate_pull_over_distance_; - return std::max(minimum_request_length, parameters_->pull_over_minimum_request_length); + return std::max(minimum_request_length, parameters_.pull_over_minimum_request_length); } bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & context_data) { // return only before starting free space parking if (!isStopped( - odometry_buffer_stopped_, planner_data_->self_odometry, parameters_->th_stopped_time, - parameters_->th_stopped_velocity)) { + odometry_buffer_stopped_, planner_data_->self_odometry, parameters_.th_stopped_time, + parameters_.th_stopped_velocity)) { return false; } @@ -974,19 +955,18 @@ bool GoalPlannerModule::canReturnToLaneParking(const PullOverContextData & conte const auto & path = lane_parking_path.full_path(); const auto & curvatures = lane_parking_path.full_path_curvatures(); if ( - parameters_->use_object_recognition && + parameters_.use_object_recognition && goal_planner_utils::checkObjectsCollision( path, curvatures, context_data.static_target_objects, context_data.dynamic_target_objects, - planner_data_->parameters, - parameters_->object_recognition_collision_check_hard_margins.back(), - /*extract_static_objects=*/false, parameters_->maximum_deceleration, - parameters_->object_recognition_collision_check_max_extra_stopping_margin, + planner_data_->parameters, parameters_.object_recognition_collision_check_hard_margins.back(), + /*extract_static_objects=*/false, parameters_.maximum_deceleration, + parameters_.object_recognition_collision_check_max_extra_stopping_margin, debug_data_.ego_polygons_expanded)) { return false; } if ( - parameters_->use_occupancy_grid_for_path_collision_check && + parameters_.use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(path, occupancy_grid_map_)) { return false; } @@ -1226,7 +1206,7 @@ std::optional GoalPlannerModule::selectPullOverPath( const auto & goal_pose = planner_data_->route_handler->getOriginalGoalPose(); const double backward_length = - parameters_->backward_goal_search_length + parameters_->decide_path_distance; + parameters_.backward_goal_search_length + parameters_.decide_path_distance; const auto & prev_module_output_path = getPreviousModuleOutput().path; // STEP1: Filter valid paths before sorting @@ -1265,7 +1245,7 @@ std::optional GoalPlannerModule::selectPullOverPath( const auto goal_pose_length = lanelet::utils::getArcCoordinates(road_lanes, goal_pose).length; return planner_data_->route_handler->getCenterLinePath( road_lanes, std::max(0.0, goal_pose_length - backward_length), - goal_pose_length + parameters_->forward_goal_search_length); + goal_pose_length + parameters_.forward_goal_search_length); }(); sorted_path_indices.erase( @@ -1277,29 +1257,29 @@ std::optional GoalPlannerModule::selectPullOverPath( sorted_path_indices.end()); sortPullOverPaths( - planner_data_, *parameters_, pull_over_path_candidates, goal_candidates, + planner_data_, parameters_, pull_over_path_candidates, goal_candidates, context_data.static_target_objects, getLogger(), sorted_path_indices); // STEP3: Select the final pull over path by checking collision to make it as high priority as // possible const double collision_check_margin = - parameters_->object_recognition_collision_check_hard_margins.back(); + parameters_.object_recognition_collision_check_hard_margins.back(); for (const size_t i : sorted_path_indices) { const auto & path = pull_over_path_candidates[i]; const PathWithLaneId & parking_path = path.parking_path(); const auto & parking_path_curvatures = path.parking_path_curvatures(); if ( - parameters_->use_object_recognition && + parameters_.use_object_recognition && goal_planner_utils::checkObjectsCollision( parking_path, parking_path_curvatures, context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_->parameters, collision_check_margin, - true, parameters_->maximum_deceleration, - parameters_->object_recognition_collision_check_max_extra_stopping_margin, + true, parameters_.maximum_deceleration, + parameters_.object_recognition_collision_check_max_extra_stopping_margin, debug_data_.ego_polygons_expanded, true)) { continue; } if ( - parameters_->use_occupancy_grid_for_path_collision_check && + parameters_.use_occupancy_grid_for_path_collision_check && checkOccupancyGridCollision(parking_path, occupancy_grid_map_)) { continue; } @@ -1313,12 +1293,11 @@ std::vector GoalPlannerModule::generateDrivableLanes() const universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); const lanelet::ConstLanelets pull_over_lanes = goal_planner_utils::getPullOverLanes( - *(planner_data_->route_handler), left_side_parking_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length); + *(planner_data_->route_handler), left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); return utils::generateDrivableLanesWithShoulderLanes(current_lanes, pull_over_lanes); } @@ -1341,9 +1320,7 @@ void GoalPlannerModule::setOutput( } const auto & pull_over_path = selected_pull_over_path_with_velocity_opt.value(); - if ( - parameters_->safety_check_params.enable_safety_check && !context_data.is_stable_safe_path && - isActivated()) { + if (!context_data.is_stable_safe_path && isActivated()) { // situation : not safe against dynamic objects after approval // insert stop point in current path if ego is able to stop with acceleration and jerk // constraints @@ -1449,7 +1426,7 @@ void GoalPlannerModule::decideVelocity(PullOverPath & pull_over_path) // partial_paths auto & first_path = pull_over_path.partial_paths().front(); const auto vel = - static_cast(std::max(current_vel, parameters_->pull_over_minimum_velocity)); + static_cast(std::max(current_vel, parameters_.pull_over_minimum_velocity)); for (auto & p : first_path.points) { p.point.longitudinal_velocity_mps = std::min(p.point.longitudinal_velocity_mps, vel); } @@ -1538,7 +1515,7 @@ BehaviorModuleOutput GoalPlannerModule::planPullOverAsOutput(PullOverContextData !is_freespace && needPathUpdate( planner_data_->self_odometry->pose.pose, 1.0 /*path_update_duration*/, clock_->now(), - modified_goal_opt, last_path_update_time, *parameters_)) { + modified_goal_opt, last_path_update_time, parameters_)) { // if the final path is not decided and enough time has passed since last path update, // select safe path from lane parking pull over path candidates // and set it to thread_safe_data_ @@ -1708,11 +1685,6 @@ std::pair GoalPlannerModule::calcDistanceToPathChange( return {dist_to_parking_start_pose, dist_to_parking_finish_pose}; } -void GoalPlannerModule::setParameters(const std::shared_ptr & parameters) -{ - parameters_ = parameters; -} - PathWithLaneId GoalPlannerModule::generateStopPath( const PullOverContextData & context_data, const std::string & detail) const { @@ -1722,11 +1694,10 @@ PathWithLaneId GoalPlannerModule::generateStopPath( const auto & current_pose = planner_data_->self_odometry->pose.pose; const auto & common_parameters = planner_data_->parameters; const double current_vel = planner_data_->self_odometry->twist.twist.linear.x; - const double pull_over_velocity = parameters_->pull_over_velocity; + const double pull_over_velocity = parameters_.pull_over_velocity; const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); if (current_lanes.empty()) { @@ -1792,7 +1763,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // if stop pose is closer than min_stop_distance, stop as soon as possible const double ego_to_stop_distance = calcSignedArcLengthFromEgo(extended_prev_path, stop_pose); const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; const double buffer = is_stopped ? stop_distance_buffer_ : 0.0; @@ -1815,8 +1786,7 @@ PathWithLaneId GoalPlannerModule::generateStopPath( // if already passed the decel pose, set pull_over_velocity to stop_path. const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - pull_over_velocity); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, pull_over_velocity); for (auto & p : stop_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(stop_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { @@ -1835,7 +1805,7 @@ PathWithLaneId GoalPlannerModule::generateFeasibleStopPath( // calc minimum stop distance under maximum deceleration const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (!min_stop_distance) { return path; } @@ -1898,7 +1868,7 @@ bool FreespaceParkingPlanner::isStuck( return false; } -bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) +bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_data) const { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); @@ -1941,7 +1911,7 @@ bool GoalPlannerModule::hasFinishedCurrentPath(const PullOverContextData & ctx_d ctx_data.pull_over_path_opt.value().getCurrentPath().points.back(); const auto & self_pose = planner_data_->self_odometry->pose.pose; return autoware::universe_utils::calcDistance2d(current_path_end, self_pose) < - parameters_->th_arrived_distance; + parameters_.th_arrived_distance; } TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & context_data) @@ -1977,8 +1947,7 @@ TurnSignalInfo GoalPlannerModule::calcTurnSignalInfo(const PullOverContextData & }; const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data_, parameters_->backward_goal_search_length, - parameters_->forward_goal_search_length, + planner_data_, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); if (current_lanes.empty()) { @@ -2035,7 +2004,7 @@ bool GoalPlannerModule::hasEnoughDistance( const bool is_separated_path = pull_over_path.partial_paths().size() > 1; const double distance_to_start = calcSignedArcLength( long_tail_reference_path.points, current_pose.position, pull_over_path.start_pose().position); - const double distance_to_restart = parameters_->decide_path_distance / 2; + const double distance_to_restart = parameters_.decide_path_distance / 2; const double eps_vel = 0.01; const bool is_stopped = std::abs(current_vel) < eps_vel; if (is_separated_path && is_stopped && distance_to_start < distance_to_restart) { @@ -2043,7 +2012,7 @@ bool GoalPlannerModule::hasEnoughDistance( } const auto current_to_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (!current_to_stop_distance) { return false; } @@ -2109,15 +2078,15 @@ void GoalPlannerModule::deceleratePath(PullOverPath & pull_over_path) const // if already passed the search start offset pose, set pull_over_velocity to first_path. const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - parameters_->pull_over_velocity); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, + parameters_.pull_over_velocity); for (auto & p : first_path.points) { const double distance_from_ego = calcSignedArcLengthFromEgo(first_path, p.point.pose); if (min_decel_distance && distance_from_ego < *min_decel_distance) { continue; } p.point.longitudinal_velocity_mps = std::min( - p.point.longitudinal_velocity_mps, static_cast(parameters_->pull_over_velocity)); + p.point.longitudinal_velocity_mps, static_cast(parameters_.pull_over_velocity)); } } @@ -2135,7 +2104,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith std::min(point.point.longitudinal_velocity_mps, static_cast(distance_to_stop / time)); const double distance_from_ego = calcSignedArcLengthFromEgo(path, point.point.pose); const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, decel_vel); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, decel_vel); // when current velocity already lower than decel_vel, min_decel_distance will be 0.0, // and do not need to decelerate. @@ -2154,7 +2123,7 @@ void GoalPlannerModule::decelerateForTurnSignal(const Pose & stop_pose, PathWith const double stop_point_length = calcSignedArcLength(path.points, 0, stop_pose.position); const auto min_stop_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, 0.0); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, 0.0); if (min_stop_distance && *min_stop_distance < stop_point_length) { utils::insertStopPoint(stop_point_length, path); @@ -2166,13 +2135,12 @@ void GoalPlannerModule::decelerateBeforeSearchStart( { universe_utils::ScopedTimeTrack st(__func__, *time_keeper_); - const double pull_over_velocity = parameters_->pull_over_velocity; + const double pull_over_velocity = parameters_.pull_over_velocity; const Pose & current_pose = planner_data_->self_odometry->pose.pose; // slow down before the search area. const auto min_decel_distance = calcFeasibleDecelDistance( - planner_data_, parameters_->maximum_deceleration, parameters_->maximum_jerk, - pull_over_velocity); + planner_data_, parameters_.maximum_deceleration, parameters_.maximum_jerk, pull_over_velocity); if (min_decel_distance) { const double distance_to_search_start = calcSignedArcLengthFromEgo(path, search_start_offset_pose); @@ -2271,22 +2239,9 @@ bool GoalPlannerModule::isCrossingPossible(const PullOverPath & pull_over_path) return isCrossingPossible(start_pose, end_pose, lanes); } -/* -void GoalPlannerModule::updateSafetyCheckTargetObjectsData( - const PredictedObjects & filtered_objects, const TargetObjectsOnLane & target_objects_on_lane, - const std::vector & ego_predicted_path) const -{ - goal_planner_data_.filtered_objects = filtered_objects; - goal_planner_data_.target_objects_on_lane = target_objects_on_lane; - goal_planner_data_.ego_predicted_path = ego_predicted_path; -} -*/ - static std::vector filterObjectsByWithinPolicy( const std::shared_ptr & objects, - const lanelet::ConstLanelets & target_lanes, - const std::shared_ptr< - autoware::behavior_path_planner::utils::path_safety_checker::ObjectsFilteringParams> & params) + const lanelet::ConstLanelets & target_lanes, const ObjectsFilteringParams & params) { // implanted part of behavior_path_planner::utils::path_safety_checker::filterObjects() and // createTargetObjectsOnLane() @@ -2296,8 +2251,8 @@ static std::vector filterOb return {}; } - const double ignore_object_velocity_threshold = params->ignore_object_velocity_threshold; - const auto & target_object_types = params->object_types_to_check; + const double ignore_object_velocity_threshold = params.ignore_object_velocity_threshold; + const auto & target_object_types = params.object_types_to_check; PredictedObjects filtered_objects = utils::path_safety_checker::filterObjectsByVelocity( *objects, ignore_object_velocity_threshold, true); @@ -2315,8 +2270,8 @@ static std::vector filterOb } } - const double safety_check_time_horizon = params->safety_check_time_horizon; - const double safety_check_time_resolution = params->safety_check_time_resolution; + const double safety_check_time_horizon = params.safety_check_time_horizon; + const double safety_check_time_resolution = params.safety_check_time_resolution; std::vector refined_filtered_objects; for (const auto & within_filtered_object : within_filtered_objects) { @@ -2328,11 +2283,7 @@ static std::vector filterOb std::pair GoalPlannerModule::isSafePath( const std::shared_ptr planner_data, const bool found_pull_over_path, - const std::optional & pull_over_path_opt, const PathDecisionState & prev_data, - const GoalPlannerParameters & parameters, - const std::shared_ptr & ego_predicted_path_params, - const std::shared_ptr & objects_filtering_params, - const std::shared_ptr & safety_check_params) const + const std::optional & pull_over_path_opt, const PathDecisionState & prev_data) const { using autoware::behavior_path_planner::utils::path_safety_checker::createPredictedPath; using autoware::behavior_path_planner::utils::path_safety_checker:: @@ -2351,25 +2302,26 @@ std::pair GoalPlannerM const auto & dynamic_object = planner_data->dynamic_object; const auto & route_handler = planner_data->route_handler; const lanelet::ConstLanelets current_lanes = utils::getExtendedCurrentLanes( - planner_data, parameters.backward_goal_search_length, parameters.forward_goal_search_length, + planner_data, parameters_.backward_goal_search_length, parameters_.forward_goal_search_length, /*forward_only_in_route*/ false); const auto pull_over_lanes = goal_planner_utils::getPullOverLanes( - *route_handler, left_side_parking_, parameters.backward_goal_search_length, - parameters.forward_goal_search_length); + *route_handler, left_side_parking_, parameters_.backward_goal_search_length, + parameters_.forward_goal_search_length); const std::pair terminal_velocity_and_accel = pull_over_path.getPairsTerminalVelocityAndAccel(); RCLCPP_DEBUG( getLogger(), "pairs_terminal_velocity_and_accel for goal_planner: %f, %f", terminal_velocity_and_accel.first, terminal_velocity_and_accel.second); - auto temp_param = std::make_shared(*ego_predicted_path_params); + auto temp_param = std::make_shared(ego_predicted_path_params_); utils::parking_departure::updatePathProperty(temp_param, terminal_velocity_and_accel); // TODO(Sugahara): shoule judge is_object_front properly const bool is_object_front = true; const bool limit_to_max_velocity = true; const auto ego_seg_idx = planner_data->findEgoIndex(current_pull_over_path.points); const auto ego_predicted_path_from_current_pose = createPredictedPath( - ego_predicted_path_params, current_pull_over_path.points, current_pose, current_velocity, - ego_seg_idx, is_object_front, limit_to_max_velocity); + std::make_shared(ego_predicted_path_params_), + current_pull_over_path.points, current_pose, current_velocity, ego_seg_idx, is_object_front, + limit_to_max_velocity); const auto ego_predicted_path = filterPredictedPathAfterTargetPose( ego_predicted_path_from_current_pose, pull_over_path.start_pose()); @@ -2413,35 +2365,35 @@ std::pair GoalPlannerM const auto expanded_pull_over_lanes_between_ego = goal_planner_utils::generateBetweenEgoAndExpandedPullOverLanes( pull_over_lanes, left_side_parking_, ego_pose_for_expand, - planner_data->parameters.vehicle_info, parameters.outer_road_detection_offset, - parameters.inner_road_detection_offset); + planner_data->parameters.vehicle_info, parameters_.outer_road_detection_offset, + parameters_.inner_road_detection_offset); const auto merged_expanded_pull_over_lanes = lanelet::utils::combineLaneletsShape(expanded_pull_over_lanes_between_ego); debug_data_.expanded_pull_over_lane_between_ego = merged_expanded_pull_over_lanes; const auto filtered_objects = filterObjectsByWithinPolicy( - dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params); + dynamic_object, {merged_expanded_pull_over_lanes}, objects_filtering_params_); const double hysteresis_factor = - prev_data.is_stable_safe ? 1.0 : parameters.hysteresis_factor_expand_rate; + prev_data.is_stable_safe ? 1.0 : parameters_.hysteresis_factor_expand_rate; const bool current_is_safe = std::invoke([&]() { - if (parameters.safety_check_params.method == "RSS") { + if (parameters_.safety_check_params.method == "RSS") { return autoware::behavior_path_planner::utils::path_safety_checker::checkSafetyWithRSS( current_pull_over_path, ego_predicted_path, filtered_objects, collision_check, - planner_data->parameters, safety_check_params->rss_params, - objects_filtering_params->use_all_predicted_path, hysteresis_factor, - safety_check_params->collision_check_yaw_diff_threshold); + planner_data->parameters, safety_check_params_.rss_params, + objects_filtering_params_.use_all_predicted_path, hysteresis_factor, + safety_check_params_.collision_check_yaw_diff_threshold); } - if (parameters.safety_check_params.method == "integral_predicted_polygon") { + if (parameters_.safety_check_params.method == "integral_predicted_polygon") { return utils::path_safety_checker::checkSafetyWithIntegralPredictedPolygon( ego_predicted_path, vehicle_info_, filtered_objects, - objects_filtering_params->check_all_predicted_path, - parameters.safety_check_params.integral_predicted_polygon_params, collision_check); + objects_filtering_params_.check_all_predicted_path, + parameters_.safety_check_params.integral_predicted_polygon_params, collision_check); } RCLCPP_ERROR( getLogger(), " [pull_over] invalid safety check method: %s", - parameters.safety_check_params.method.c_str()); + parameters_.safety_check_params.method.c_str()); throw std::domain_error("[pull_over] invalid safety check method"); }); @@ -2558,14 +2510,12 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } debug_marker_.markers.push_back(marker); - if (parameters_->safety_check_params.enable_safety_check) { - autoware::universe_utils::appendMarkerArray( - goal_planner_utils::createLaneletPolygonMarkerArray( - debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, - "expanded_pull_over_lane_between_ego", - autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), - &debug_marker_); - } + autoware::universe_utils::appendMarkerArray( + goal_planner_utils::createLaneletPolygonMarkerArray( + debug_data_.expanded_pull_over_lane_between_ego.polygon3d(), header, + "expanded_pull_over_lane_between_ego", + autoware::universe_utils::createMarkerColor(1.0, 0.7, 0.0, 0.999)), + &debug_marker_); // Visualize debug poses const auto & debug_poses = pull_over_path.debug_poses; @@ -2575,56 +2525,42 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) } } - // safety check - if (parameters_->safety_check_params.enable_safety_check) { - if (goal_planner_data_.ego_predicted_path.size() > 0) { - const auto & ego_predicted_path = utils::path_safety_checker::convertToPredictedPath( - goal_planner_data_.ego_predicted_path, ego_predicted_path_params_->time_resolution); - add(createPredictedPathMarkerArray( - ego_predicted_path, vehicle_info_, "ego_predicted_path_goal_planner", 0, 0.0, 0.5, 0.9)); - } - if (goal_planner_data_.filtered_objects.objects.size() > 0) { - add(createObjectsMarkerArray( - goal_planner_data_.filtered_objects, "filtered_objects", 0, 0.0, 0.5, 0.9)); - } + auto collision_check = debug_data_.collision_check; + if (parameters_.safety_check_params.method == "RSS") { + add(showSafetyCheckInfo(collision_check, "object_debug_info")); + } + add(showPredictedPath(collision_check, "ego_predicted_path")); + add(showPolygon(collision_check, "ego_and_target_polygon_relation")); - auto collision_check = debug_data_.collision_check; - if (parameters_->safety_check_params.method == "RSS") { - add(showSafetyCheckInfo(collision_check, "object_debug_info")); - } - add(showPredictedPath(collision_check, "ego_predicted_path")); - add(showPolygon(collision_check, "ego_and_target_polygon_relation")); + // set objects of interest + for (const auto & [uuid, data] : collision_check) { + const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; + setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); + } - // set objects of interest - for (const auto & [uuid, data] : collision_check) { - const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED; - setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color); - } + // TODO(Mamoru Sobue): it is not clear where ThreadSafeData::collision_check should be cleared + utils::parking_departure::initializeCollisionCheckDebugMap(collision_check); - // TODO(Mamoru Sobue): it is not clear where ThreadSafeData::collision_check should be cleared - utils::parking_departure::initializeCollisionCheckDebugMap(collision_check); + // visualize safety status maker + { + const auto & prev_data = context_data.prev_state_for_debug; + visualization_msgs::msg::MarkerArray marker_array{}; + const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) + : createMarkerColor(1.0, 0.0, 0.0, 0.99); + auto marker = createDefaultMarker( + header.frame_id, header.stamp, "safety_status", 0, + visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - // visualize safety status maker - { - const auto & prev_data = context_data.prev_state_for_debug; - visualization_msgs::msg::MarkerArray marker_array{}; - const auto color = prev_data.is_stable_safe ? createMarkerColor(1.0, 1.0, 1.0, 0.99) - : createMarkerColor(1.0, 0.0, 0.0, 0.99); - auto marker = createDefaultMarker( - header.frame_id, header.stamp, "safety_status", 0, - visualization_msgs::msg::Marker::TEXT_VIEW_FACING, createMarkerScale(0.0, 0.0, 1.0), color); - - marker.pose = planner_data_->self_odometry->pose.pose; - marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; - if (prev_data.safe_start_time) { - const double elapsed_time_from_safe_start = - (clock_->now() - prev_data.safe_start_time.value()).seconds(); - marker.text += - "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; - } - marker_array.markers.push_back(marker); - add(marker_array); + marker.pose = planner_data_->self_odometry->pose.pose; + marker.text += "is_safe: " + std::to_string(prev_data.is_stable_safe) + "\n"; + if (prev_data.safe_start_time) { + const double elapsed_time_from_safe_start = + (clock_->now() - prev_data.safe_start_time.value()).seconds(); + marker.text += + "elapsed_time_from_safe_start: " + std::to_string(elapsed_time_from_safe_start) + "\n"; } + marker_array.markers.push_back(marker); + add(marker_array); } // Visualize planner type text @@ -2649,7 +2585,7 @@ void GoalPlannerModule::setDebugData(const PullOverContextData & context_data) TODO(soblin): disable until thread safe design is done if (isStuck( context_data.static_target_objects, context_data.dynamic_target_objects, planner_data_, - occupancy_grid_map_, *parameters_)) { + occupancy_grid_map_, parameters_)) { marker.text += " stuck"; } if (isStopped()) {