diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp index ce2552a41d380..1061e07e516a2 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/base_class.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_lane_change_module/structs/debug.hpp" #include "autoware/behavior_path_lane_change_module/structs/path.hpp" #include "autoware/behavior_path_lane_change_module/utils/utils.hpp" +#include "autoware/behavior_path_planner_common/data_manager.hpp" #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp" #include "autoware/behavior_path_planner_common/turn_signal_decider.hpp" #include "autoware/behavior_path_planner_common/utils/path_shifter/path_shifter.hpp" @@ -38,6 +39,7 @@ class TestNormalLaneChange; namespace autoware::behavior_path_planner { +using autoware::behavior_path_planner::PoseWithDetailOpt; using autoware::route_handler::Direction; using autoware::universe_utils::StopWatch; using geometry_msgs::msg::Point; @@ -222,7 +224,7 @@ class LaneChangeBase return direction_; } - std::optional getStopPose() const { return lane_change_stop_pose_; } + PoseWithDetailOpt getStopPose() const { return lane_change_stop_pose_; } void resetStopPose() { lane_change_stop_pose_ = std::nullopt; } @@ -282,7 +284,7 @@ class LaneChangeBase lane_change::CommonDataPtr common_data_ptr_; FilteredLanesObjects filtered_objects_{}; BehaviorModuleOutput prev_module_output_{}; - std::optional lane_change_stop_pose_{std::nullopt}; + PoseWithDetailOpt lane_change_stop_pose_{std::nullopt}; PathWithLaneId prev_approved_path_; diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp index 6367caaea6829..5f1da79bc7ea0 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/include/autoware/behavior_path_lane_change_module/scene.hpp @@ -18,6 +18,7 @@ #include "autoware/behavior_path_lane_change_module/structs/data.hpp" #include +#include #include #include @@ -157,7 +158,8 @@ class NormalLaneChange : public LaneChangeBase bool check_prepare_phase() const; - void set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path); + void set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason = ""); void updateStopTime(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp index d65e51997eb32..f80aad721a07c 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp @@ -111,9 +111,7 @@ BehaviorModuleOutput LaneChangeInterface::plan() path_reference_ = std::make_shared(output.reference_path); *prev_approved_path_ = getPreviousModuleOutput().path; - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + stop_pose_ = module_type_->getStopPose(); const auto & lane_change_debug = module_type_->getDebugData(); for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) { @@ -171,9 +169,7 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval() } path_reference_ = std::make_shared(out.reference_path); - const auto stop_pose_opt = module_type_->getStopPose(); - stop_pose_ = stop_pose_opt.has_value() ? PoseWithDetailOpt(PoseWithDetail(stop_pose_opt.value())) - : PoseWithDetailOpt(); + stop_pose_ = module_type_->getStopPose(); if (!module_type_->isValidPath()) { path_candidate_ = std::make_shared(); diff --git a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp index f8a200a9685a7..7f0e6d7f575f6 100644 --- a/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp +++ b/planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp @@ -408,7 +408,7 @@ BehaviorModuleOutput NormalLaneChange::generateOutput() 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)); - set_stop_pose(stop_dist + current_dist, output.path); + set_stop_pose(stop_dist + current_dist, output.path, "incoming rear object"); } else { insert_stop_point(get_target_lanes(), output.path); } @@ -471,7 +471,7 @@ void NormalLaneChange::insert_stop_point( if (!is_current_lane) { const auto arc_length_to_stop_pose = motion_utils::calcArcLength(path.points) - common_data_ptr_->transient_data.next_dist_buffer.min; - set_stop_pose(arc_length_to_stop_pose, path); + set_stop_pose(arc_length_to_stop_pose, path, "next lc terminal"); return; } @@ -514,8 +514,9 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto dist_to_terminal_stop = std::min(dist_to_terminal_start, distance_to_last_fit_width); + const auto terminal_stop_reason = status_.is_valid_path ? "no safe path" : "no valid path"; if (filtered_objects_.current_lane.empty()) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -541,12 +542,12 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) const auto stop_arc_length_behind_obj = arc_length_to_current_obj - stop_margin; if (stop_arc_length_behind_obj < dist_to_target_lane_start) { - set_stop_pose(dist_to_target_lane_start, path); + set_stop_pose(dist_to_target_lane_start, path, "maintain distance to front object"); return; } if (stop_arc_length_behind_obj > dist_to_terminal_stop) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } @@ -562,11 +563,11 @@ void NormalLaneChange::insert_stop_point_on_current_lanes(PathWithLaneId & path) filtered_objects_.target_lane_leading, stop_arc_length_behind_obj, path); if (has_blocking_target_lane_obj || stop_arc_length_behind_obj <= 0.0) { - set_stop_pose(dist_to_terminal_stop, path); + set_stop_pose(dist_to_terminal_stop, path, terminal_stop_reason); return; } - set_stop_pose(stop_arc_length_behind_obj, path); + set_stop_pose(stop_arc_length_behind_obj, path, "maintain distance to front object"); } PathWithLaneId NormalLaneChange::getReferencePath() const @@ -1688,10 +1689,11 @@ bool NormalLaneChange::is_ego_stuck() const return has_object_blocking; } -void NormalLaneChange::set_stop_pose(const double arc_length_to_stop_pose, PathWithLaneId & path) +void NormalLaneChange::set_stop_pose( + const double arc_length_to_stop_pose, PathWithLaneId & path, const std::string & reason) { const auto stop_point = utils::insertStopPoint(arc_length_to_stop_pose, path); - lane_change_stop_pose_ = stop_point.point.pose; + lane_change_stop_pose_ = PoseWithDetailOpt(PoseWithDetail(stop_point.point.pose, reason)); } void NormalLaneChange::updateStopTime()