feat(lane_change): add info text to virtual wall (autowarefoundation#…

…9783)

* specify reason for lane change stop line

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

* add stop reason for incoming rear object

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

---------

Signed-off-by: mohammad alqudah <alqudah.mohammad@tier4.jp>

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

@@ -111,9 +111,7 @@ BehaviorModuleOutput LaneChangeInterface::plan()
   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 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   }
 
   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

@@ -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()