feat(lane_change): add info text to virtual wall

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<Pose> 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<Pose> lane_change_stop_pose_{std::nullopt};
+  PoseWithDetailOpt lane_change_stop_pose_{std::nullopt};
 
   PathWithLaneId prev_approved_path_;
 

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 <memory>
+#include <string>
 #include <utility>
 #include <vector>
 
@@ -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();
 

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/interface.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -111,9 +111,7 @@
   path_reference_ = std::make_shared<PathWithLaneId>(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 @@
   }
 
   path_reference_ = std::make_shared<PathWithLaneId>(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<PathWithLaneId>();

planning/behavior_path_planner/autoware_behavior_path_lane_change_module/src/scene.cpp

@@ -1,4 +1,4 @@
 // Copyright 2023 TIER IV, Inc.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -408,7 +408,7 @@
         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 @@
   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,59 +514,60 @@
 
   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;
   }
 
   const auto dist_to_target_lane_start = std::invoke([&]() -> double {
     const auto & front_lane = lanes_ptr->target_neighbor.front();
     const auto target_front =
       utils::to_geom_msg_pose(front_lane.centerline2d().front(), front_lane);
     return get_arc_length_along_lanelet(target_front);
   });
 
   const auto arc_length_to_current_obj = utils::lane_change::get_min_dist_to_current_lanes_obj(
     common_data_ptr_, filtered_objects_, dist_to_target_lane_start, path);
 
   // margin with leading vehicle
   // consider rss distance when the LC need to avoid obstacles
   const auto rss_dist = calcRssDistance(
     0.0, lc_param_ptr->trajectory.min_lane_changing_velocity,
     lc_param_ptr->safety.rss_params_for_parked);
 
   const auto stop_margin = transient_data.lane_changing_length.min +
                            lc_param_ptr->backward_length_buffer_for_blocking_object + rss_dist +
                            bpp_param_ptr->base_link2front;
   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;
   }
 
   //  If the target lane in the lane change section is blocked by a stationary obstacle, there
   //  is no reason for stopping with a lane change margin. Instead, stop right behind the
   //  obstacle.
   //  ----------------------------------------------------------
   //                            [obj]>
   //  ----------------------------------------------------------
   //    [ego]>          | <--- stop margin --->  [obj]>
   //  ----------------------------------------------------------
   const auto has_blocking_target_lane_obj = utils::lane_change::has_blocking_target_object(
     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 @@
   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()