From f01e280735ed7a102598245219ae45d94b96f8bc Mon Sep 17 00:00:00 2001 From: Mamoru Sobue Date: Fri, 24 Jan 2025 14:31:11 +0900 Subject: [PATCH] change trigger condition Signed-off-by: Mamoru Sobue --- .../config/goal_planner.param.yaml | 2 +- .../goal_planner_module.hpp | 1 + .../thread_data.hpp | 5 +- .../util.hpp | 4 + .../src/goal_planner_module.cpp | 73 +++++++------------ .../src/thread_data.cpp | 4 +- .../src/util.cpp | 40 ++++++++++ 7 files changed, 80 insertions(+), 49 deletions(-) diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml index f822a6d97647c..95b2095f87164 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/config/goal_planner.param.yaml @@ -69,7 +69,7 @@ maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 - after_shift_straight_distance: 1.5 + after_shift_straight_distance: 1.0 # parallel parking path parallel_parking: 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 4de8fe0873c65..f2cc7e2f4ef65 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 @@ -318,6 +318,7 @@ class GoalPlannerModule : public SceneModuleInterface private: std::pair syncWithThreads(); + bool trigger_thread_on_approach_{false}; // NOTE: never access to following variables except in updateData()!!! std::mutex lane_parking_mutex_; std::optional lane_parking_request_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp index 655cb87ad593c..af20c5f6debe5 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/include/autoware/behavior_path_goal_planner_module/thread_data.hpp @@ -43,7 +43,8 @@ class LaneParkingRequest void update( const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & upstream_module_output, - const std::optional & pull_over_path, const PathDecisionState & prev_data); + const std::optional & pull_over_path, const PathDecisionState & prev_data, + const bool trigger_thread_on_approach); const autoware::universe_utils::LinearRing2d vehicle_footprint_; const GoalCandidates goal_candidates_; @@ -56,6 +57,7 @@ class LaneParkingRequest } const std::optional & get_pull_over_path() const { return pull_over_path_; } const PathDecisionState & get_prev_data() const { return prev_data_; } + bool trigger_thread_on_approach() const { return trigger_thread_on_approach_; } private: std::shared_ptr planner_data_; @@ -63,6 +65,7 @@ class LaneParkingRequest BehaviorModuleOutput upstream_module_output_; std::optional pull_over_path_; // GoalPlannerModule::sync // count is affected lane_parking_request_.value().update( *planner_data_, getCurrentStatus(), getPreviousModuleOutput(), pull_over_path, - path_decision_controller_.get_current_state()); + path_decision_controller_.get_current_state(), trigger_thread_on_approach_); // NOTE: RouteHandler holds several shared pointers in it, so just copying PlannerData as // value does not adds the reference counts of RouteHandler.lanelet_map_ptr_ and others. Since // behavior_path_planner::run() updates @@ -724,15 +724,26 @@ void GoalPlannerModule::updateData() goal_candidates_ = generateGoalCandidates(); } - /* - const Pose & current_pose = planner_data_->self_odometry->pose.pose; - const Pose goal_pose = planner_data_->route_handler->getOriginalGoalPose(); - // check if goal_pose is in current_lanes or neighboring road lanes const lanelet::ConstLanelets current_lanes = utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); - const double self_to_goal_arc_length = - utils::getSignedDistance(current_pose, goal_pose, current_lanes); - */ + + if ( + !trigger_thread_on_approach_ && + utils::isAllowedGoalModification(planner_data_->route_handler) && + goal_planner_utils::is_goal_reachable_on_path( + current_lanes, *(planner_data_->route_handler), left_side_parking_)) { + 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) { + trigger_thread_on_approach_ = true; + [[maybe_unused]] const auto send_only_request = syncWithThreads(); + RCLCPP_INFO( + getLogger(), "start preparing goal candidates once for goal ahead of %f meter", + self_to_goal_arc_length); + return; + } + } if (getCurrentStatus() == ModuleStatus::IDLE && !isExecutionRequested()) { return; @@ -849,39 +860,9 @@ bool GoalPlannerModule::isExecutionRequested() const // check if goal_pose is in current_lanes or neighboring road lanes const lanelet::ConstLanelets current_lanes = utils::getCurrentLanesFromPath(getPreviousModuleOutput().reference_path, planner_data_); - const auto getNeighboringLane = - [&](const lanelet::ConstLanelet & lane) -> std::optional { - return left_side_parking_ ? route_handler->getLeftLanelet(lane, false, true) - : route_handler->getRightLanelet(lane, false, true); - }; - lanelet::ConstLanelets goal_check_lanes = current_lanes; - for (const auto & lane : current_lanes) { - auto neighboring_lane = getNeighboringLane(lane); - while (neighboring_lane) { - goal_check_lanes.push_back(neighboring_lane.value()); - neighboring_lane = getNeighboringLane(neighboring_lane.value()); - } - } - const bool goal_is_in_current_segment_lanes = std::any_of( - goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) { - return lanelet::utils::isInLanelet(goal_pose, lane); - }); - - // check that goal is in current neighbor shoulder lane - const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() { - for (const auto & lane : current_lanes) { - const auto shoulder_lane = left_side_parking_ ? route_handler->getLeftShoulderLanelet(lane) - : route_handler->getRightShoulderLanelet(lane); - if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) { - return true; - } - } - return false; - }); - - // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner, - // because goal arc coordinates cannot be calculated. - if (!goal_is_in_current_segment_lanes && !goal_is_in_current_shoulder_lanes) { + const bool is_goal_reachable = goal_planner_utils::is_goal_reachable_on_path( + current_lanes, *(planner_data_->route_handler), left_side_parking_); + if (!is_goal_reachable) { return false; } @@ -889,7 +870,7 @@ bool GoalPlannerModule::isExecutionRequested() const // 1) goal_pose is in current_lanes, plan path to the original fixed goal // 2) goal_pose is NOT in current_lanes, do not execute goal_planner if (!utils::isAllowedGoalModification(route_handler)) { - return goal_is_in_current_segment_lanes; + return is_goal_reachable; } // if goal arc coordinates can be calculated, check if goal is in request_length diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp index 89458c199c130..c334fec7a1bd9 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/thread_data.cpp @@ -22,7 +22,8 @@ namespace autoware::behavior_path_planner void LaneParkingRequest::update( const PlannerData & planner_data, const ModuleStatus & current_status, const BehaviorModuleOutput & upstream_module_output, - const std::optional & pull_over_path, const PathDecisionState & prev_data) + const std::optional & pull_over_path, const PathDecisionState & prev_data, + const bool trigger_thread_on_approach) { planner_data_ = std::make_shared(planner_data); planner_data_->route_handler = std::make_shared(*(planner_data.route_handler)); @@ -30,6 +31,7 @@ void LaneParkingRequest::update( upstream_module_output_ = upstream_module_output; pull_over_path_ = pull_over_path; prev_data_ = prev_data; + trigger_thread_on_approach_ = trigger_thread_on_approach; } void FreespaceParkingRequest::initializeOccupancyGridMap( diff --git a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp index 34aa50ba041a4..fb39a34c9ac91 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_goal_planner_module/src/util.cpp @@ -856,4 +856,44 @@ autoware_perception_msgs::msg::PredictedObjects extract_dynamic_objects( return dynamic_target_objects; } +bool is_goal_reachable_on_path( + const lanelet::ConstLanelets current_lanes, const route_handler::RouteHandler & route_handler, + const bool left_side_parking) +{ + const Pose goal_pose = route_handler.getOriginalGoalPose(); + const auto getNeighboringLane = + [&](const lanelet::ConstLanelet & lane) -> std::optional { + return left_side_parking ? route_handler.getLeftLanelet(lane, false, true) + : route_handler.getRightLanelet(lane, false, true); + }; + lanelet::ConstLanelets goal_check_lanes = current_lanes; + for (const auto & lane : current_lanes) { + auto neighboring_lane = getNeighboringLane(lane); + while (neighboring_lane) { + goal_check_lanes.push_back(neighboring_lane.value()); + neighboring_lane = getNeighboringLane(neighboring_lane.value()); + } + } + const bool goal_is_in_current_segment_lanes = std::any_of( + goal_check_lanes.begin(), goal_check_lanes.end(), [&](const lanelet::ConstLanelet & lane) { + return lanelet::utils::isInLanelet(goal_pose, lane); + }); + + // check that goal is in current neighbor shoulder lane + const bool goal_is_in_current_shoulder_lanes = std::invoke([&]() { + for (const auto & lane : current_lanes) { + const auto shoulder_lane = left_side_parking ? route_handler.getLeftShoulderLanelet(lane) + : route_handler.getRightShoulderLanelet(lane); + if (shoulder_lane && lanelet::utils::isInLanelet(goal_pose, *shoulder_lane)) { + return true; + } + } + return false; + }); + + // if goal is not in current_lanes and current_shoulder_lanes, do not execute goal_planner, + // because goal arc coordinates cannot be calculated. + return goal_is_in_current_segment_lanes || goal_is_in_current_shoulder_lanes; +} + } // namespace autoware::behavior_path_planner::goal_planner_utils