diff --git a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml index 68000747b7410..27bd9624c3cc1 100644 --- a/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml +++ b/planning/behavior_path_planner/config/pull_out/pull_out.param.yaml @@ -9,8 +9,8 @@ # shift pull out enable_shift_pull_out: true shift_pull_out_velocity: 2.0 - pull_out_sampling_num: 4 minimum_shift_pull_out_distance: 20.0 + pull_out_sampling_num: 4 maximum_lateral_jerk: 2.0 minimum_lateral_jerk: 0.5 deceleration_interval: 15.0 diff --git a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp index 59b903e1995e5..b682d28efeaa5 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/data_manager.hpp @@ -57,6 +57,7 @@ using nav_msgs::msg::Odometry; using route_handler::RouteHandler; using tier4_planning_msgs::msg::LateralOffset; using PlanResult = PathWithLaneId::SharedPtr; +using unique_identifier_msgs::msg::UUID; struct BoolStamped { @@ -140,6 +141,7 @@ struct PlannerData PathWithLaneId::SharedPtr reference_path{std::make_shared()}; PathWithLaneId::SharedPtr prev_output_path{std::make_shared()}; std::optional prev_modified_goal{}; + std::optional prev_route_id{}; lanelet::ConstLanelets current_lanes{}; std::shared_ptr route_handler{std::make_shared()}; BehaviorPathPlannerParameters parameters{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp index 63720996782f1..3c3885e7c7782 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/pull_out/pull_out_module.hpp @@ -47,18 +47,20 @@ using lane_departure_checker::LaneDepartureChecker; struct PullOutStatus { - PullOutPath pull_out_path; - size_t current_path_idx = 0; - PlannerType planner_type = PlannerType::NONE; - PathWithLaneId backward_path; - lanelet::ConstLanelets current_lanes; - lanelet::ConstLanelets pull_out_lanes; - std::vector lanes; - std::vector lane_follow_lane_ids; - std::vector pull_out_lane_ids; - bool is_safe = false; - bool back_finished = false; - Pose pull_out_start_pose; + PullOutPath pull_out_path{}; + size_t current_path_idx{0}; + PlannerType planner_type{PlannerType::NONE}; + PathWithLaneId backward_path{}; + lanelet::ConstLanelets current_lanes{}; + lanelet::ConstLanelets pull_out_lanes{}; + std::vector lanes{}; + std::vector lane_follow_lane_ids{}; + std::vector pull_out_lane_ids{}; + bool is_safe{false}; + bool back_finished{false}; + Pose pull_out_start_pose{}; + + PullOutStatus() {} }; class PullOutModule : public SceneModuleInterface @@ -107,13 +109,14 @@ class PullOutModule : public SceneModuleInterface vehicle_info_util::VehicleInfo vehicle_info_; std::vector> pull_out_planners_; - PullOutStatus status_; + mutable PullOutStatus status_; std::deque odometry_buffer_; std::unique_ptr last_route_received_time_; std::unique_ptr last_pull_out_start_update_time_; std::unique_ptr last_approved_pose_; + mutable bool has_received_new_route_{false}; std::shared_ptr getCurrentPlanner() const; PathWithLaneId getFullPath() const; @@ -141,6 +144,11 @@ class PullOutModule : public SceneModuleInterface bool hasFinishedCurrentPath(); void setDebugData() const; + +// tempolary for old architecture +#ifdef USE_OLD_ARCHITECTURE + mutable bool is_executed_{false}; +#endif }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 269e50dfa5182..ece09d1c8b297 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1264,6 +1264,8 @@ void BehaviorPathPlannerNode::run() modified_goal_publisher_->publish(modified_goal); } + planner_data_->prev_route_id = planner_data_->route_handler->getRouteUuid(); + if (planner_data_->parameters.visualize_maximum_drivable_area) { const auto maximum_drivable_area = marker_utils::createFurthestLineStringMarkerArray( utils::getMaximumDrivableArea(planner_data_)); diff --git a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp index a384d61fb9d63..eed016afdc37c 100644 --- a/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/pull_out/pull_out_module.cpp @@ -104,13 +104,37 @@ void PullOutModule::processOnExit() bool PullOutModule::isExecutionRequested() const { + has_received_new_route_ = + !planner_data_->prev_route_id || + *planner_data_->prev_route_id != planner_data_->route_handler->getRouteUuid(); + + if (has_received_new_route_) { + status_ = PullOutStatus(); + } + +#ifdef USE_OLD_ARCHITECTURE + if (is_executed_) { + return true; + } +#endif + if (current_state_ == ModuleStatus::RUNNING) { return true; } + if (!has_received_new_route_) { +#ifdef USE_OLD_ARCHITECTURE + is_executed_ = false; +#endif + return false; + } + const bool is_stopped = utils::l2Norm(planner_data_->self_odometry->twist.twist.linear) < parameters_->th_arrived_distance; if (!is_stopped) { +#ifdef USE_OLD_ARCHITECTURE + is_executed_ = false; +#endif return false; } @@ -126,19 +150,15 @@ bool PullOutModule::isExecutionRequested() const auto lanes = current_lanes; lanes.insert(lanes.end(), pull_out_lanes.begin(), pull_out_lanes.end()); if (LaneDepartureChecker::isOutOfLane(lanes, vehicle_footprint)) { +#ifdef USE_OLD_ARCHITECTURE + is_executed_ = false; +#endif return false; } - // Check if any of the footprint points are in the shoulder lane - lanelet::Lanelet closest_shoulder_lanelet; - if (!lanelet::utils::query::getClosestLanelet( - pull_out_lanes, planner_data_->self_odometry->pose.pose, &closest_shoulder_lanelet)) { - return false; - } - if (!isOverlappedWithLane(closest_shoulder_lanelet, vehicle_footprint)) { - return false; - } - +#ifdef USE_OLD_ARCHITECTURE + is_executed_ = true; +#endif return true; } @@ -532,19 +552,8 @@ PathWithLaneId PullOutModule::generateStopPath() const void PullOutModule::updatePullOutStatus() { - // if new route is received, reset status - const bool has_received_new_route = - last_route_received_time_ == nullptr || - *last_route_received_time_ != planner_data_->route_handler->getRouteHeader().stamp; - if (has_received_new_route) { - RCLCPP_INFO(getLogger(), "Receive new route, so reset status"); - resetStatus(); - } - last_route_received_time_ = - std::make_unique(planner_data_->route_handler->getRouteHeader().stamp); - // skip updating if enough time has not passed for preventing chattering between back and pull_out - if (!has_received_new_route && !status_.back_finished) { + if (!has_received_new_route_ && !status_.back_finished) { if (last_pull_out_start_update_time_ == nullptr) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } @@ -711,7 +720,14 @@ bool PullOutModule::hasFinishedPullOut() const lanelet::utils::getArcCoordinates(status_.current_lanes, current_pose); const auto arclength_pull_out_end = lanelet::utils::getArcCoordinates(status_.current_lanes, status_.pull_out_path.end_pose); - return arclength_current.length - arclength_pull_out_end.length > 0.0; + + const bool has_finished = arclength_current.length - arclength_pull_out_end.length > 0.0; + +#ifdef USE_OLD_ARCHITECTURE + is_executed_ = !has_finished; +#endif + + return has_finished; } void PullOutModule::checkBackFinished() diff --git a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp index 0a8866a6978ef..f29f68a4b36a6 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp +++ b/planning/behavior_path_planner/src/utils/pull_out/shift_pull_out.cpp @@ -166,6 +166,10 @@ std::vector ShiftPullOut::calcPullOutPaths( // calculate after/before shifted pull out distance // lateral distance from road center to start pose const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; + std::cerr << "shift_length: " << shift_length << std::endl; + + // const bool use_minimum_shift_length = shift_length < 1.0; + const double pull_out_distance = std::max( PathShifter::calcLongitudinalDistFromJerk( abs(shift_length), lateral_jerk, shift_pull_out_velocity), diff --git a/planning/behavior_path_planner/src/utils/pull_out/util.cpp b/planning/behavior_path_planner/src/utils/pull_out/util.cpp index 1502d3831e681..d46b980327a29 100644 --- a/planning/behavior_path_planner/src/utils/pull_out/util.cpp +++ b/planning/behavior_path_planner/src/utils/pull_out/util.cpp @@ -96,23 +96,22 @@ Pose getBackedPose( return backed_pose; } -// getShoulderLanesOnCurrentPose lanelet::ConstLanelets getPullOutLanes(const std::shared_ptr & planner_data) { - const double pull_out_lane_length = 200.0; const double & vehicle_width = planner_data->parameters.vehicle_width; const auto & route_handler = planner_data->route_handler; - const auto current_pose = planner_data->self_odometry->pose.pose; + const auto & current_pose = planner_data->self_odometry->pose.pose; lanelet::ConstLanelet current_shoulder_lane; lanelet::ConstLanelets shoulder_lanes; if (route_handler->getPullOutStartLane( route_handler->getShoulderLanelets(), current_pose, vehicle_width, ¤t_shoulder_lane)) { - shoulder_lanes = route_handler->getShoulderLaneletSequence( - current_shoulder_lane, current_pose, pull_out_lane_length, pull_out_lane_length); + // pull out from shoulder lane + return route_handler->getShoulderLaneletSequence(current_shoulder_lane, current_pose); } - return shoulder_lanes; + // pull out from road lane + return utils::getExtendedCurrentLanes(planner_data); } } // namespace behavior_path_planner::pull_out_utils