diff --git a/localization/yabloc/COLCON_IGNORE b/localization/yabloc/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index f30bf31d4e010..02774f2e2ce93 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -107,7 +107,7 @@ class LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const double threshold) const = 0; - virtual bool getAbortPath() = 0; + virtual bool calcAbortPath() = 0; virtual bool specialRequiredCheck() const { return false; } @@ -209,6 +209,10 @@ class LaneChangeBase return direction_; } + boost::optional getStopPose() const { return lane_change_stop_pose_; } + + void resetStopPose() { lane_change_stop_pose_ = boost::none; } + protected: virtual lanelet::ConstLanelets getCurrentLanes() const = 0; @@ -244,6 +248,7 @@ class LaneChangeBase PathWithLaneId prev_module_path_{}; DrivableAreaInfo prev_drivable_area_info_{}; TurnSignalInfo prev_turn_signal_info_{}; + boost::optional lane_change_stop_pose_{boost::none}; PathWithLaneId prev_approved_path_{}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp index d1476abb63e27..0dc9e8adb0ac9 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/normal.hpp @@ -72,7 +72,7 @@ class NormalLaneChange : public LaneChangeBase TurnSignalInfo updateOutputTurnSignal() override; - bool getAbortPath() override; + bool calcAbortPath() override; PathSafetyStatus isApprovedPathSafe() const override; @@ -147,6 +147,8 @@ class NormalLaneChange : public LaneChangeBase const lanelet::ConstLanelets & current_lanes, const lanelet::ConstLanelets & target_lanes, const lanelet::ConstLanelets & target_backward_lanes) const; + void setStopPose(const Pose & stop_pose); + rclcpp::Logger logger_ = rclcpp::get_logger("lane_change").get_child(getModuleTypeStr()); }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp index a31eeed88a275..16527c2886617 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp @@ -172,7 +172,7 @@ ModuleStatus LaneChangeInterface::updateState() return ModuleStatus::RUNNING; } - const auto found_abort_path = module_type_->getAbortPath(); + const auto found_abort_path = module_type_->calcAbortPath(); if (!found_abort_path) { RCLCPP_WARN_STREAM_THROTTLE( getLogger().get_child(module_type_->getModuleTypeStr()), *clock_, 5000, @@ -204,6 +204,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateSpecialData(); + module_type_->resetStopPose(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -220,7 +221,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() auto output = module_type_->generateOutput(); path_reference_ = output.reference_path; *prev_approved_path_ = *getPreviousModuleOutput().path; - module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); + + stop_pose_ = module_type_->getStopPose(); updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -250,6 +252,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() path_reference_ = getPreviousModuleOutput().reference_path; + stop_pose_ = module_type_->getStopPose(); + if (!module_type_->isValidPath()) { removeRTCStatus(); path_candidate_ = std::make_shared(); @@ -288,6 +292,7 @@ void LaneChangeInterface::updateModuleParams( void LaneChangeInterface::setData(const std::shared_ptr & data) { + planner_data_ = data; module_type_->setData(data); } diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index e8d86aac999b2..fa6119713770f 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -111,37 +111,43 @@ bool NormalLaneChange::isLaneChangeRequired() const LaneChangePath NormalLaneChange::getLaneChangePath() const { - return isAbortState() ? *abort_path_ : status_.lane_change_path; + return status_.lane_change_path; } BehaviorModuleOutput NormalLaneChange::generateOutput() { BehaviorModuleOutput output; - output.path = std::make_shared(getLaneChangePath().path); - const auto found_extended_path = extendPath(); - if (found_extended_path) { - *output.path = utils::combinePath(*output.path, *found_extended_path); - } - extendOutputDrivableArea(output); - output.reference_path = std::make_shared(getReferencePath()); - output.turn_signal_info = updateOutputTurnSignal(); - - if (isAbortState()) { + if (isAbortState() && abort_path_) { + output.path = std::make_shared(abort_path_->path); output.reference_path = std::make_shared(prev_module_reference_path_); output.turn_signal_info = prev_turn_signal_info_; - return output; - } + insertStopPoint(status_.current_lanes, *output.path); + } else { + output.path = std::make_shared(getLaneChangePath().path); - if (isStopState()) { - const auto current_velocity = getEgoVelocity(); - const auto current_dist = motion_utils::calcSignedArcLength( - output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); - const auto stop_dist = - -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); - const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + const auto found_extended_path = extendPath(); + if (found_extended_path) { + *output.path = utils::combinePath(*output.path, *found_extended_path); + } + output.reference_path = std::make_shared(getReferencePath()); + output.turn_signal_info = updateOutputTurnSignal(); + + if (isStopState()) { + const auto current_velocity = getEgoVelocity(); + const auto current_dist = motion_utils::calcSignedArcLength( + output.path->points, output.path->points.front().point.pose.position, getEgoPosition()); + const auto stop_dist = + -(current_velocity * current_velocity / (2.0 * planner_data_->parameters.min_acc)); + const auto stop_point = utils::insertStopPoint(stop_dist + current_dist, *output.path); + setStopPose(stop_point.point.pose); + } else { + insertStopPoint(status_.target_lanes, *output.path); + } } + extendOutputDrivableArea(output); + const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); output.turn_signal_info = planner_data_->turn_signal_decider.use_prior_turn_signal( *output.path, getEgoPose(), current_seg_idx, prev_turn_signal_info_, output.turn_signal_info, @@ -200,6 +206,7 @@ void NormalLaneChange::insertStopPoint( const double stopping_distance = distance_to_terminal - lane_change_buffer - stop_point_buffer; if (stopping_distance > 0.0) { const auto stop_point = utils::insertStopPoint(stopping_distance, path); + setStopPose(stop_point.point.pose); } } @@ -1176,7 +1183,7 @@ bool NormalLaneChange::isRequiredStop(const bool is_object_coming_from_rear) con isAbleToStopSafely() && is_object_coming_from_rear; } -bool NormalLaneChange::getAbortPath() +bool NormalLaneChange::calcAbortPath() { const auto & route_handler = getRouteHandler(); const auto & common_param = getCommonParam(); @@ -1284,14 +1291,12 @@ bool NormalLaneChange::getAbortPath() reference_lanelets, shifted_path.path.points.at(abort_return_idx).point.pose); const PathWithLaneId reference_lane_segment = std::invoke([&]() { const double s_start = arc_position.length; - double s_end = std::max( - lanelet::utils::getLaneletLength2d(reference_lanelets) - minimum_lane_change_length, s_start); + double s_end = std::max(lanelet::utils::getLaneletLength2d(reference_lanelets), s_start); if (route_handler->isInGoalRouteSection(selected_path.info.target_lanes.back())) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(reference_lanelets, route_handler->getGoalPose()); - const double forward_length = - std::max(goal_arc_coordinates.length - minimum_lane_change_length, s_start); + const double forward_length = std::max(goal_arc_coordinates.length, s_start); s_end = std::min(s_end, forward_length); } PathWithLaneId ref = route_handler->getCenterLinePath(reference_lanelets, s_start, s_end, true); @@ -1301,20 +1306,23 @@ bool NormalLaneChange::getAbortPath() return ref; }); - PathWithLaneId start_to_abort_return_pose; - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), shifted_path.path.points.begin(), - shifted_path.path.points.begin() + abort_return_idx); - if (reference_lane_segment.points.size() > 1) { - start_to_abort_return_pose.points.insert( - start_to_abort_return_pose.points.end(), (reference_lane_segment.points.begin() + 1), - reference_lane_segment.points.end()); - } - auto abort_path = selected_path; abort_path.shifted_path = shifted_path; - abort_path.path = start_to_abort_return_pose; abort_path.info.shift_line = shift_line; + + { + PathWithLaneId aborting_path; + aborting_path.points.insert( + aborting_path.points.begin(), shifted_path.path.points.begin(), + shifted_path.path.points.begin() + abort_return_idx); + + if (!reference_lane_segment.points.empty()) { + abort_path.path = utils::combinePath(aborting_path, reference_lane_segment); + } else { + abort_path.path = aborting_path; + } + } + abort_path_ = std::make_shared(abort_path); return true; } @@ -1381,4 +1389,10 @@ PathSafetyStatus NormalLaneChange::isLaneChangePathSafe( return path_safety_status; } + +void NormalLaneChange::setStopPose(const Pose & stop_pose) +{ + lane_change_stop_pose_ = stop_pose; +} + } // namespace behavior_path_planner