refactor(lane_change): combine all debug data into single struct (aut…

…owarefoundation#6173) * refactor(lane_change): combine all debug data into single struct Signed-off-by: Zulfaqar Azmi <[email protected]> * Rename valid path → valid paths Signed-off-by: Zulfaqar Azmi <[email protected]> * move the marker creation to marker_utils Signed-off-by: Zulfaqar Azmi <[email protected]> * Remove alias Signed-off-by: Zulfaqar Azmi <[email protected]> --------- Signed-off-by: Zulfaqar Azmi <[email protected]>
kyoichi-sugahara · Jan 29, 2024 · 4b22a5a · 4b22a5a
1 parent 00974c3
commit 4b22a5a
Show file tree

Hide file tree

Showing 7 changed files with 129 additions and 76 deletions.
diff --git a/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp b/...g/behavior_path_lane_change_module/include/behavior_path_lane_change_module/interface.hpp
@@ -128,7 +128,7 @@ class LaneChangeInterface : public SceneModuleInterface
 
   bool canTransitIdleToRunningState() override;
 
-  void setObjectDebugVisualization() const;
+  void updateDebugMarker() const;
 
   void updateSteeringFactorPtr(const BehaviorModuleOutput & output);
 

diff --git a/...ior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp b/...ior_path_lane_change_module/include/behavior_path_lane_change_module/utils/base_class.hpp
@@ -15,6 +15,7 @@
 #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__BASE_CLASS_HPP_
 
 #include "behavior_path_lane_change_module/utils/data_structs.hpp"
+#include "behavior_path_lane_change_module/utils/debug_structs.hpp"
 #include "behavior_path_lane_change_module/utils/path.hpp"
 #include "behavior_path_lane_change_module/utils/utils.hpp"
 #include "behavior_path_planner_common/interface/scene_module_interface.hpp"
@@ -36,7 +37,6 @@
 namespace behavior_path_planner
 {
 using autoware_auto_planning_msgs::msg::PathWithLaneId;
-using behavior_path_planner::utils::path_safety_checker::CollisionCheckDebugMap;
 using data::lane_change::PathSafetyStatus;
 using geometry_msgs::msg::Point;
 using geometry_msgs::msg::Pose;
@@ -137,19 +137,7 @@ class LaneChangeBase
 
   const LaneChangeStatus & getLaneChangeStatus() const { return status_; }
 
-  const LaneChangePaths & getDebugValidPath() const { return debug_valid_path_; }
-
-  const CollisionCheckDebugMap & getDebugData() const { return object_debug_; }
-
-  const CollisionCheckDebugMap & getAfterApprovalDebugData() const
-  {
-    return object_debug_after_approval_;
-  }
-
-  const LaneChangeTargetObjects & getDebugFilteredObjects() const
-  {
-    return debug_filtered_objects_;
-  }
+  const data::lane_change::Debug & getDebugData() const { return lane_change_debug_; }
 
   const Pose & getEgoPose() const { return planner_data_->self_odometry->pose.pose; }
 
@@ -257,12 +245,8 @@ class LaneChangeBase
   Direction direction_{Direction::NONE};
   LaneChangeModuleType type_{LaneChangeModuleType::NORMAL};
 
-  mutable LaneChangePaths debug_valid_path_{};
-  mutable CollisionCheckDebugMap object_debug_{};
-  mutable CollisionCheckDebugMap object_debug_after_approval_{};
-  mutable LaneChangeTargetObjects debug_filtered_objects_{};
-  mutable double object_debug_lifetime_{0.0};
   mutable StopWatch<std::chrono::milliseconds> stop_watch_;
+  mutable data::lane_change::Debug lane_change_debug_;
 
   rclcpp::Logger logger_ = utils::lane_change::getLogger(getModuleTypeStr());
   mutable rclcpp::Clock clock_{RCL_ROS_TIME};

diff --git a/..._path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp b/..._path_lane_change_module/include/behavior_path_lane_change_module/utils/debug_structs.hpp
@@ -0,0 +1,49 @@
+// Copyright 2024 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.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+#ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_
+#define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_
+
+#include "behavior_path_lane_change_module/utils/data_structs.hpp"
+#include "behavior_path_lane_change_module/utils/path.hpp"
+
+#include <behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp>
+
+#include <string>
+
+namespace behavior_path_planner::data::lane_change
+{
+using utils::path_safety_checker::CollisionCheckDebugMap;
+struct Debug
+{
+  std::string module_type;
+  LaneChangePaths valid_paths;
+  CollisionCheckDebugMap collision_check_objects;
+  CollisionCheckDebugMap collision_check_objects_after_approval;
+  LaneChangeTargetObjects filtered_objects;
+  double collision_check_object_debug_lifetime{0.0};
+
+  void reset()
+  {
+    valid_paths.clear();
+    collision_check_objects.clear();
+    collision_check_objects_after_approval.clear();
+    filtered_objects.current_lane.clear();
+    filtered_objects.target_lane.clear();
+    filtered_objects.other_lane.clear();
+    collision_check_object_debug_lifetime = 0.0;
+  }
+};
+}  // namespace behavior_path_planner::data::lane_change
+
+#endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__DEBUG_STRUCTS_HPP_
diff --git a/...havior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp b/...havior_path_lane_change_module/include/behavior_path_lane_change_module/utils/markers.hpp
@@ -15,9 +15,11 @@
 #ifndef BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_
 #define BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_
 
+#include "behavior_path_lane_change_module/utils/debug_structs.hpp"
 #include "behavior_path_lane_change_module/utils/path.hpp"
 #include "behavior_path_planner_common/utils/path_safety_checker/path_safety_checker_parameters.hpp"
 
+#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 #include <visualization_msgs/msg/marker_array.hpp>
 
 #include <string>
@@ -26,6 +28,7 @@
 namespace marker_utils::lane_change_markers
 {
 using behavior_path_planner::LaneChangePath;
+using behavior_path_planner::data::lane_change::Debug;
 using behavior_path_planner::utils::path_safety_checker::ExtendedPredictedObjects;
 using visualization_msgs::msg::MarkerArray;
 MarkerArray showAllValidLaneChangePath(
@@ -37,5 +40,6 @@ MarkerArray showFilteredObjects(
   const ExtendedPredictedObjects & current_lane_objects,
   const ExtendedPredictedObjects & target_lane_objects,
   const ExtendedPredictedObjects & other_lane_objects, const std::string & ns);
+MarkerArray createDebugMarkerArray(const Debug & debug_data);
 }  // namespace marker_utils::lane_change_markers
 #endif  // BEHAVIOR_PATH_LANE_CHANGE_MODULE__UTILS__MARKERS_HPP_
diff --git a/planning/behavior_path_lane_change_module/src/interface.cpp b/planning/behavior_path_lane_change_module/src/interface.cpp
@@ -83,7 +83,7 @@ void LaneChangeInterface::updateData()
   if (isWaitingApproval()) {
     module_type_->updateLaneChangeStatus();
   }
-  setObjectDebugVisualization();
+  updateDebugMarker();
 
   module_type_->updateSpecialData();
   module_type_->resetStopPose();
@@ -109,7 +109,8 @@ BehaviorModuleOutput LaneChangeInterface::plan()
 
   stop_pose_ = module_type_->getStopPose();
 
-  for (const auto & [uuid, data] : module_type_->getAfterApprovalDebugData()) {
+  const auto & lane_change_debug = module_type_->getDebugData();
+  for (const auto & [uuid, data] : lane_change_debug.collision_check_objects_after_approval) {
     const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
     setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
   }
@@ -133,7 +134,8 @@ BehaviorModuleOutput LaneChangeInterface::planWaitingApproval()
   out.drivable_area_info = getPreviousModuleOutput().drivable_area_info;
   out.turn_signal_info = getCurrentTurnSignalInfo(out.path, out.turn_signal_info);
 
-  for (const auto & [uuid, data] : module_type_->getDebugData()) {
+  const auto & lane_change_debug = module_type_->getDebugData();
+  for (const auto & [uuid, data] : lane_change_debug.collision_check_objects) {
     const auto color = data.is_safe ? ColorName::GREEN : ColorName::RED;
     setObjectsOfInterestData(data.current_obj_pose, data.obj_shape, color);
   }
@@ -291,7 +293,7 @@ bool LaneChangeInterface::canTransitFailureState()
 
 bool LaneChangeInterface::canTransitIdleToRunningState()
 {
-  setObjectDebugVisualization();
+  updateDebugMarker();
 
   auto log_debug_throttled = [&](std::string_view message) -> void {
     RCLCPP_DEBUG(getLogger(), "%s", message.data());
@@ -312,43 +314,15 @@ bool LaneChangeInterface::canTransitIdleToRunningState()
   return true;
 }
 
-void LaneChangeInterface::setObjectDebugVisualization() const
+void LaneChangeInterface::updateDebugMarker() const
 {
-  debug_marker_.markers.clear();
   if (!parameters_->publish_debug_marker) {
     return;
   }
-  using marker_utils::showPolygon;
-  using marker_utils::showPredictedPath;
-  using marker_utils::showSafetyCheckInfo;
-  using marker_utils::lane_change_markers::showAllValidLaneChangePath;
-  using marker_utils::lane_change_markers::showFilteredObjects;
-
-  const auto debug_data = module_type_->getDebugData();
-  const auto debug_after_approval = module_type_->getAfterApprovalDebugData();
-  const auto debug_valid_path = module_type_->getDebugValidPath();
-  const auto debug_filtered_objects = module_type_->getDebugFilteredObjects();
 
   debug_marker_.markers.clear();
-  const auto add = [this](const MarkerArray & added) {
-    tier4_autoware_utils::appendMarkerArray(added, &debug_marker_);
-  };
-
-  add(showAllValidLaneChangePath(debug_valid_path, "lane_change_valid_paths"));
-  add(showFilteredObjects(
-    debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
-    debug_filtered_objects.other_lane, "object_filtered"));
-  if (!debug_data.empty()) {
-    add(showSafetyCheckInfo(debug_data, "object_debug_info"));
-    add(showPredictedPath(debug_data, "ego_predicted_path"));
-    add(showPolygon(debug_data, "ego_and_target_polygon_relation"));
-  }
-
-  if (!debug_after_approval.empty()) {
-    add(showSafetyCheckInfo(debug_after_approval, "object_debug_info_after_approval"));
-    add(showPredictedPath(debug_after_approval, "ego_predicted_path_after_approval"));
-    add(showPolygon(debug_after_approval, "ego_and_target_polygon_relation_after_approval"));
-  }
+  using marker_utils::lane_change_markers::createDebugMarkerArray;
+  debug_marker_ = createDebugMarkerArray(module_type_->getDebugData());
 }
 
 MarkerArray LaneChangeInterface::getModuleVirtualWall()

diff --git a/planning/behavior_path_lane_change_module/src/scene.cpp b/planning/behavior_path_lane_change_module/src/scene.cpp
@@ -97,7 +97,7 @@ std::pair<bool, bool> NormalLaneChange::getSafePath(LaneChangePath & safe_path)
       lane_change_parameters_->rss_params_for_stuck, is_stuck);
   }
 
-  debug_valid_path_ = valid_paths;
+  lane_change_debug_.valid_paths = valid_paths;
 
   if (valid_paths.empty()) {
     safe_path.info.current_lanes = current_lanes;
@@ -421,13 +421,8 @@ void NormalLaneChange::resetParameters()
   current_lane_change_state_ = LaneChangeStates::Normal;
   abort_path_ = nullptr;
   status_ = {};
+  lane_change_debug_.reset();
 
-  object_debug_.clear();
-  object_debug_after_approval_.clear();
-  debug_filtered_objects_.current_lane.clear();
-  debug_filtered_objects_.target_lane.clear();
-  debug_filtered_objects_.other_lane.clear();
-  debug_valid_path_.clear();
   RCLCPP_DEBUG(logger_, "reset all flags and debug information.");
 }
 
@@ -1112,7 +1107,7 @@ bool NormalLaneChange::getLaneChangePaths(
   const utils::path_safety_checker::RSSparams rss_params, const bool is_stuck,
   const bool check_safety) const
 {
-  object_debug_.clear();
+  lane_change_debug_.collision_check_objects.clear();
   if (current_lanes.empty() || target_lanes.empty()) {
     RCLCPP_WARN(logger_, "target_neighbor_preferred_lane_poly_2d is empty. Not expected.");
     return false;
@@ -1158,7 +1153,7 @@ bool NormalLaneChange::getLaneChangePaths(
   }
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
-  debug_filtered_objects_ = target_objects;
+  lane_change_debug_.filtered_objects = target_objects;
 
   const auto prepare_durations = calcPrepareDuration(current_lanes, target_lanes);
 
@@ -1385,9 +1380,10 @@ bool NormalLaneChange::getLaneChangePaths(
         std::vector<ExtendedPredictedObject> filtered_objects =
           filterObjectsInTargetLane(target_objects, target_lanes);
         if (
-          !is_stuck && utils::lane_change::passParkedObject(
-                         route_handler, *candidate_path, filtered_objects, lane_change_buffer,
-                         is_goal_in_route, *lane_change_parameters_, object_debug_)) {
+          !is_stuck &&
+          utils::lane_change::passParkedObject(
+            route_handler, *candidate_path, filtered_objects, lane_change_buffer, is_goal_in_route,
+            *lane_change_parameters_, lane_change_debug_.collision_check_objects)) {
           debug_print(
             "Reject: parking vehicle exists in the target lane, and the ego is not in stuck. Skip "
             "lane change.");
@@ -1400,7 +1396,8 @@ bool NormalLaneChange::getLaneChangePaths(
         }
 
         const auto [is_safe, is_object_coming_from_rear] = isLaneChangePathSafe(
-          *candidate_path, target_objects, rss_params, is_stuck, object_debug_);
+          *candidate_path, target_objects, rss_params, is_stuck,
+          lane_change_debug_.collision_check_objects);
 
         if (is_safe) {
           debug_print("ACCEPT!!!: it is valid and safe!");
@@ -1423,24 +1420,25 @@ PathSafetyStatus NormalLaneChange::isApprovedPathSafe() const
   const auto & target_lanes = status_.target_lanes;
 
   const auto target_objects = getTargetObjects(current_lanes, target_lanes);
-  debug_filtered_objects_ = target_objects;
+  lane_change_debug_.filtered_objects = target_objects;
 
   CollisionCheckDebugMap debug_data;
   const bool is_stuck = isVehicleStuck(current_lanes);
   const auto safety_status = isLaneChangePathSafe(
     path, target_objects, lane_change_parameters_->rss_params_for_abort, is_stuck, debug_data);
   {
     // only for debug purpose
-    object_debug_.clear();
-    object_debug_lifetime_ += (stop_watch_.toc(getModuleTypeStr()) / 1000);
-    if (object_debug_lifetime_ > 2.0) {
+    lane_change_debug_.collision_check_objects.clear();
+    lane_change_debug_.collision_check_object_debug_lifetime +=
+      (stop_watch_.toc(getModuleTypeStr()) / 1000);
+    if (lane_change_debug_.collision_check_object_debug_lifetime > 2.0) {
       stop_watch_.toc(getModuleTypeStr(), true);
-      object_debug_lifetime_ = 0.0;
-      object_debug_after_approval_.clear();
+      lane_change_debug_.collision_check_object_debug_lifetime = 0.0;
+      lane_change_debug_.collision_check_objects_after_approval.clear();
     }
 
     if (!safety_status.is_safe) {
-      object_debug_after_approval_ = debug_data;
+      lane_change_debug_.collision_check_objects_after_approval = debug_data;
     }
   }
 
@@ -1802,7 +1800,7 @@ bool NormalLaneChange::isVehicleStuck(
   using lanelet::utils::getArcCoordinates;
   const auto base_distance = getArcCoordinates(current_lanes, getEgoPose()).length;
 
-  for (const auto & object : debug_filtered_objects_.current_lane) {
+  for (const auto & object : lane_change_debug_.filtered_objects.current_lane) {
     const auto & p = object.initial_pose.pose;  // TODO(Horibe): consider footprint point
 
     // Note: it needs chattering prevention.

diff --git a/planning/behavior_path_lane_change_module/src/utils/markers.cpp b/planning/behavior_path_lane_change_module/src/utils/markers.cpp
@@ -22,6 +22,7 @@
 #include <autoware_auto_perception_msgs/msg/predicted_objects.hpp>
 #include <autoware_auto_planning_msgs/msg/path_with_lane_id.hpp>
 #include <visualization_msgs/msg/detail/marker__struct.hpp>
+#include <visualization_msgs/msg/detail/marker_array__struct.hpp>
 
 #include <cstdint>
 #include <cstdlib>
@@ -120,4 +121,47 @@ MarkerArray showFilteredObjects(
     marker_array.markers.end(), other_marker.markers.begin(), other_marker.markers.end());
   return marker_array;
 }
+
+MarkerArray createDebugMarkerArray(const Debug & debug_data)
+{
+  using marker_utils::showPolygon;
+  using marker_utils::showPredictedPath;
+  using marker_utils::showSafetyCheckInfo;
+  using marker_utils::lane_change_markers::showAllValidLaneChangePath;
+  using marker_utils::lane_change_markers::showFilteredObjects;
+
+  const auto & debug_collision_check_object = debug_data.collision_check_objects;
+  const auto & debug_collision_check_object_after_approval =
+    debug_data.collision_check_objects_after_approval;
+  const auto & debug_valid_paths = debug_data.valid_paths;
+  const auto & debug_filtered_objects = debug_data.filtered_objects;
+
+  MarkerArray debug_marker;
+  const auto add = [&debug_marker](const MarkerArray & added) {
+    tier4_autoware_utils::appendMarkerArray(added, &debug_marker);
+  };
+
+  add(showAllValidLaneChangePath(debug_valid_paths, "lane_change_valid_paths"));
+  add(showFilteredObjects(
+    debug_filtered_objects.current_lane, debug_filtered_objects.target_lane,
+    debug_filtered_objects.other_lane, "object_filtered"));
+
+  if (!debug_collision_check_object.empty()) {
+    add(showSafetyCheckInfo(debug_collision_check_object, "collision_check_object_info"));
+    add(showPredictedPath(debug_collision_check_object, "ego_predicted_path"));
+    add(showPolygon(debug_collision_check_object, "ego_and_target_polygon_relation"));
+  }
+
+  if (!debug_collision_check_object_after_approval.empty()) {
+    add(showSafetyCheckInfo(
+      debug_collision_check_object_after_approval, "object_debug_info_after_approval"));
+    add(showPredictedPath(
+      debug_collision_check_object_after_approval, "ego_predicted_path_after_approval"));
+    add(showPolygon(
+      debug_collision_check_object_after_approval,
+      "ego_and_target_polygon_relation_after_approval"));
+  }
+
+  return debug_marker;
+}
 }  // namespace marker_utils::lane_change_markers