From 8def93ce52788424ed5e44ab5285cf00aa690598 Mon Sep 17 00:00:00 2001 From: kosuke55 Date: Wed, 24 May 2023 21:11:29 +0900 Subject: [PATCH] feat(pull_out): support pull out normal lane Signed-off-by: kosuke55 --- .../config/pull_out/pull_out.param.yaml | 3 +- .../behavior_path_planner_pull_out_design.md | 1 + .../behavior_path_planner/data_manager.hpp | 2 + .../scene_module/pull_out/pull_out_module.hpp | 34 ++++--- .../goal_planner/goal_planner_parameters.hpp | 1 + .../utils/pull_out/pull_out_parameters.hpp | 1 + .../src/behavior_path_planner_node.cpp | 3 + .../scene_module/pull_out/pull_out_module.cpp | 94 ++++++++++++------- .../src/utils/pull_out/shift_pull_out.cpp | 13 +++ .../src/utils/pull_out/util.cpp | 11 +-- 10 files changed, 110 insertions(+), 53 deletions(-) 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..43cc972c8c8d1 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 @@ -4,13 +4,14 @@ th_arrived_distance: 1.0 th_stopped_velocity: 0.01 th_stopped_time: 1.0 + th_blinker_on_lateral_offset: 1.0 collision_check_margin: 1.0 collision_check_distance_from_end: 1.0 # 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/docs/behavior_path_planner_pull_out_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_pull_out_design.md index 78c359a395697..ee138740d0fc1 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_pull_out_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_pull_out_design.md @@ -54,6 +54,7 @@ PullOutPath --o PullOutPlannerBase | th_arrived_distance_m | [m] | double | distance threshold for arrival of path termination | 1.0 | | th_stopped_velocity_mps | [m/s] | double | velocity threshold for arrival of path termination | 0.01 | | th_stopped_time_sec | [s] | double | time threshold for arrival of path termination | 1.0 | +| th_blinker_on_lateral_offset | [m] | double | lateral distance threshold for turning on blinker | 1.0 | | collision_check_margin | [m] | double | Obstacle collision check margin | 1.0 | | collision_check_distance_from_end | [m] | double | collision check distance from end point. currently only for pull out | 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..ca0c21b56d002 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; + +// temporary for old architecture +#ifdef USE_OLD_ARCHITECTURE + mutable bool is_executed_{false}; +#endif }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp index 6da7c131a281f..f4a3a2bc5a88b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/goal_planner/goal_planner_parameters.hpp @@ -44,6 +44,7 @@ struct GoalPlannerParameters double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; + double th_blinker_on_lateral_offset; // goal search std::string search_priority; // "efficient_path" or "close_goal" diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp index 0fc7d367718ec..bc12812424561 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/pull_out/pull_out_parameters.hpp @@ -28,6 +28,7 @@ struct PullOutParameters double th_arrived_distance; double th_stopped_velocity; double th_stopped_time; + double th_blinker_on_lateral_offset; double collision_check_margin; double collision_check_distance_from_end; // shift pull out 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 c747aac3de6ec..e535571f6c16f 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -1034,6 +1034,7 @@ PullOutParameters BehaviorPathPlannerNode::getPullOutParam() p.th_arrived_distance = declare_parameter(ns + "th_arrived_distance"); p.th_stopped_velocity = declare_parameter(ns + "th_stopped_velocity"); p.th_stopped_time = declare_parameter(ns + "th_stopped_time"); + p.th_blinker_on_lateral_offset = declare_parameter(ns + "th_blinker_on_lateral_offset"); p.collision_check_margin = declare_parameter(ns + "collision_check_margin"); p.collision_check_distance_from_end = declare_parameter(ns + "collision_check_distance_from_end"); @@ -1267,6 +1268,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..41a7d4c664d8b 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 @@ -31,6 +31,7 @@ using motion_utils::calcLongitudinalOffsetPose; using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::inverseTransformPoint; namespace behavior_path_planner { @@ -104,13 +105,33 @@ 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(); + +#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 +147,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; } @@ -390,6 +407,9 @@ void PullOutModule::incrementPathIndex() PathWithLaneId PullOutModule::getCurrentPath() const { + if (status_.pull_out_path.partial_paths.size() <= status_.current_path_idx) { + return PathWithLaneId{}; + } return status_.pull_out_path.partial_paths.at(status_.current_path_idx); } @@ -532,28 +552,21 @@ 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(); + if (has_received_new_route_) { + status_ = PullOutStatus(); } - 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 (last_pull_out_start_update_time_ == nullptr) { + if (!has_received_new_route_ && !last_pull_out_start_update_time_ && !status_.back_finished) { + if (!last_pull_out_start_update_time_) { last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } const auto elapsed_time = (clock_->now() - *last_pull_out_start_update_time_).seconds(); if (elapsed_time < parameters_->backward_path_update_duration) { return; } - last_pull_out_start_update_time_ = std::make_unique(clock_->now()); } + last_pull_out_start_update_time_ = std::make_unique(clock_->now()); const auto & route_handler = planner_data_->route_handler; const auto & current_pose = planner_data_->self_odometry->pose.pose; @@ -711,7 +724,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() @@ -776,7 +796,10 @@ bool PullOutModule::hasFinishedCurrentPath() TurnSignalInfo PullOutModule::calcTurnSignalInfo() const { TurnSignalInfo turn_signal{}; // output - const auto & current_pose = planner_data_->self_odometry->pose.pose; + + const Pose & current_pose = planner_data_->self_odometry->pose.pose; + const Pose & start_pose = status_.pull_out_path.start_pose; + const Pose & end_pose = status_.pull_out_path.end_pose; // turn on hazard light when backward driving if (!status_.back_finished) { @@ -785,26 +808,31 @@ TurnSignalInfo PullOutModule::calcTurnSignalInfo() const turn_signal.desired_start_point = back_start_pose; turn_signal.required_start_point = back_start_pose; // pull_out start_pose is same to backward driving end_pose - turn_signal.required_end_point = status_.pull_out_path.start_pose; - turn_signal.desired_end_point = status_.pull_out_path.start_pose; + turn_signal.required_end_point = start_pose; + turn_signal.desired_end_point = start_pose; return turn_signal; } // turn on right signal until passing pull_out end point const auto path = getFullPath(); // pull out path does not overlap - const double distance_from_end = motion_utils::calcSignedArcLength( - path.points, status_.pull_out_path.end_pose.position, current_pose.position); - if (distance_from_end < 0.0) { + const double distance_from_end = + motion_utils::calcSignedArcLength(path.points, end_pose.position, current_pose.position); + const double lateral_offset = inverseTransformPoint(end_pose.position, start_pose).y; + + if (distance_from_end < 0.0 && lateral_offset > parameters_->th_blinker_on_lateral_offset) { + turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_LEFT; + } else if ( + distance_from_end < 0.0 && lateral_offset < -parameters_->th_blinker_on_lateral_offset) { turn_signal.turn_signal.command = TurnIndicatorsCommand::ENABLE_RIGHT; } else { turn_signal.turn_signal.command = TurnIndicatorsCommand::DISABLE; } - turn_signal.desired_start_point = status_.pull_out_path.start_pose; - turn_signal.required_start_point = status_.pull_out_path.start_pose; - turn_signal.required_end_point = status_.pull_out_path.end_pose; - turn_signal.desired_end_point = status_.pull_out_path.end_pose; + turn_signal.desired_start_point = start_pose; + turn_signal.required_start_point = start_pose; + turn_signal.required_end_point = end_pose; + turn_signal.desired_end_point = end_pose; return turn_signal; } 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 7be5d2693bb7b..f71b2c65f3078 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 @@ -158,6 +158,7 @@ std::vector ShiftPullOut::calcPullOutPaths( PathWithLaneId road_lane_reference_path = utils::resamplePathWithSpline(route_handler.getCenterLinePath(road_lanes, s_start, s_end), 1.0); + bool has_non_shifted_path = false; for (double lateral_jerk = minimum_lateral_jerk; lateral_jerk <= maximum_lateral_jerk; lateral_jerk += jerk_resolution) { PathShifter path_shifter{}; @@ -165,7 +166,19 @@ std::vector ShiftPullOut::calcPullOutPaths( // calculate after/before shifted pull out distance // lateral distance from road center to start pose + constexpr double minimum_shift_length = 0.01; const double shift_length = getArcCoordinates(road_lanes, start_pose).distance; + // if shift length is too short, add non sifted path + if (shift_length < minimum_shift_length && !has_non_shifted_path) { + PullOutPath non_shifted_path{}; + non_shifted_path.partial_paths.push_back(road_lane_reference_path); + non_shifted_path.start_pose = start_pose; + non_shifted_path.end_pose = start_pose; + candidate_paths.push_back(non_shifted_path); + has_non_shifted_path = true; + continue; + } + 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