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..1a7350d82aa4e 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 @@ -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 664f547edb92d..747c75babb573 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 @@ -153,6 +153,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 0ceab21573316..5e47116117103 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 @@ -203,6 +203,7 @@ void LaneChangeInterface::updateData() module_type_->setPreviousModulePaths( getPreviousModuleOutput().reference_path, getPreviousModuleOutput().path); module_type_->updateSpecialData(); + module_type_->resetStopPose(); } BehaviorModuleOutput LaneChangeInterface::plan() @@ -221,6 +222,8 @@ BehaviorModuleOutput LaneChangeInterface::plan() *prev_approved_path_ = *getPreviousModuleOutput().path; module_type_->insertStopPoint(module_type_->getLaneChangeStatus().target_lanes, *output.path); + stop_pose_ = module_type_->getStopPose(); + updateSteeringFactorPtr(output); clearWaitingApproval(); @@ -249,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(); @@ -286,6 +291,7 @@ void LaneChangeInterface::updateModuleParams(const std::any & parameters) 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 f2c068f07d2fa..dea6073654ae2 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 @@ -138,6 +138,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() 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); } const auto current_seg_idx = planner_data_->findEgoSegmentIndex(output.path->points); @@ -198,6 +199,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); } } @@ -1455,4 +1457,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