fix(lane_change): separate visualization after path approval

Signed-off-by: Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>
Signed-off-by: Muhammad Zulfaqar Azmi <zulfaqar.azmi@tier4.jp>

planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp

@@ -14,13 +14,15 @@
 #ifndef BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_
 #define BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_
 
+#include "behavior_path_planner/marker_util/debug_utilities.hpp"
 #include "behavior_path_planner/marker_util/lane_change/debug.hpp"
 #include "behavior_path_planner/scene_module/scene_module_interface.hpp"
 #include "behavior_path_planner/turn_signal_decider.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_module_data.hpp"
 #include "behavior_path_planner/utils/lane_change/lane_change_path.hpp"
 #include "behavior_path_planner/utils/lane_change/utils.hpp"
 #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp"
+#include "tier4_autoware_utils/system/stop_watch.hpp"
 
 #include <magic_enum.hpp>
 #include <rclcpp/rclcpp.hpp>
@@ -32,6 +34,7 @@
 #include <geometry_msgs/msg/twist.hpp>
 
 #include <algorithm>
+#include <chrono>
 #include <memory>
 #include <string>
 #include <utility>
@@ -46,6 +49,7 @@ using geometry_msgs::msg::Pose;
 using geometry_msgs::msg::Twist;
 using marker_utils::CollisionCheckDebugMap;
 using route_handler::Direction;
+using tier4_autoware_utils::StopWatch;
 using tier4_planning_msgs::msg::LaneChangeDebugMsg;
 using tier4_planning_msgs::msg::LaneChangeDebugMsgArray;
 
@@ -139,6 +143,11 @@ class LaneChangeBase
 
   const CollisionCheckDebugMap & getDebugData() const { return object_debug_; }
 
+  const CollisionCheckDebugMap & getAfterApprovalDebugData() const
+  {
+    return object_debug_after_approval_;
+  }
+
   const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
 
   const Point & getEgoPosition() const { return getEgoPose().position; }
@@ -239,8 +248,11 @@ class LaneChangeBase
   Direction direction_{Direction::NONE};
   LaneChangeModuleType type_{LaneChangeModuleType::NORMAL};
 
-  mutable CollisionCheckDebugMap object_debug_{};
   mutable LaneChangePaths debug_valid_path_{};
+  mutable CollisionCheckDebugMap object_debug_{};
+  mutable CollisionCheckDebugMap object_debug_after_approval_{};
+  mutable double object_debug_lifetime_{0.0};
+  mutable StopWatch<std::chrono::milliseconds> stop_watch_;
 };
 }  // namespace behavior_path_planner
 #endif  // BEHAVIOR_PATH_PLANNER__SCENE_MODULE__LANE_CHANGE__BASE_CLASS_HPP_

planning/behavior_path_planner/src/scene_module/lane_change/interface.cpp

@@ -299,17 +299,26 @@ void LaneChangeInterface::setObjectDebugVisualization() const
   using marker_utils::lane_change_markers::showPolygonPose;
 
   const auto debug_data = module_type_->getDebugData();
+  const auto debug_after_approval = module_type_->getAfterApprovalDebugData();
   const auto debug_valid_path = module_type_->getDebugValidPath();
 
   debug_marker_.markers.clear();
   const auto add = [this](const MarkerArray & added) {
     tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
   };
 
-  add(showObjectInfo(debug_data, "object_debug_info"));
-  add(showLerpedPose(debug_data, "ego_predicted_path"));
-  add(showPolygon(debug_data, "ego_and_target_polygon_relation"));
   add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths"));
+  if (!debug_data.empty()) {
+    add(showObjectInfo(debug_data, "object_debug_info"));
+    add(showLerpedPose(debug_data, "ego_predicted_path"));
+    add(showPolygon(debug_data, "ego_and_target_polygon_relation"));
+  }
+
+  if (!debug_after_approval.empty()) {
+    add(showObjectInfo(debug_after_approval, "object_debug_info_after_approval"));
+    add(showLerpedPose(debug_after_approval, "ego_predicted_path_after_approval"));
+    add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval"));
+  }
 }
 
 std::shared_ptr<LaneChangeDebugMsgArray> LaneChangeInterface::get_debug_msg_array() const

planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp

@@ -36,6 +36,7 @@ NormalLaneChange::NormalLaneChange(
   Direction direction)
 : LaneChangeBase(parameters, type, direction)
 {
+  stop_watch_.tic(getModuleTypeStr());
 }
 
 void NormalLaneChange::updateLaneChangeStatus()
@@ -870,6 +871,20 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   const auto safety_status = isLaneChangePathSafe(
     path, target_objects, common_parameters.expected_front_deceleration_for_abort,
     common_parameters.expected_rear_deceleration_for_abort, debug_data);
+  {
+    // only for debug purpose
+    object_debug_.clear();
+    object_debug_lifetime_ += (stop_watch_.toc(getModuleTypeStr()) / 1000);
+    if (object_debug_lifetime_ > 2.0) {
+      stop_watch_.toc(getModuleTypeStr(), true);
+      object_debug_lifetime_ = 0.0;
+      object_debug_after_approval_.clear();
+    }
+
+    if (!safety_status.is_safe) {
+      object_debug_after_approval_ = debug_data;
+    }
+  }
 
   return safety_status;
 }