feat(avoidance): make it selectable output debug marker from yaml (#7097)

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>

Author: satoshi-ota

planning/behavior_path_avoidance_module/config/avoidance.param.yaml

@@ -293,4 +293,11 @@
 
       # for debug
       debug:
-        marker: false
+        enable_other_objects_marker: false
+        enable_other_objects_info: false
+        enable_detection_area_marker: false
+        enable_drivable_bound_marker: false
+        enable_safety_check_marker: false
+        enable_shift_line_marker: false
+        enable_lane_marker: false
+        enable_misc_marker: false

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp

@@ -337,7 +337,14 @@ struct AvoidanceParameters
   bool enable_bound_clipping{false};
 
   // debug
-  bool publish_debug_marker = false;
+  bool enable_other_objects_marker{false};
+  bool enable_other_objects_info{false};
+  bool enable_detection_area_marker{false};
+  bool enable_drivable_bound_marker{false};
+  bool enable_safety_check_marker{false};
+  bool enable_shift_line_marker{false};
+  bool enable_lane_marker{false};
+  bool enable_misc_marker{false};
 };
 
 struct ObjectData  // avoidance target

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/debug.hpp

@@ -18,11 +18,13 @@
 #include "behavior_path_avoidance_module/data_structs.hpp"
 #include "behavior_path_avoidance_module/type_alias.hpp"
 
+#include <memory>
 #include <string>
 
 namespace behavior_path_planner::utils::avoidance
 {
 
+using behavior_path_planner::AvoidanceParameters;
 using behavior_path_planner::AvoidancePlanningData;
 using behavior_path_planner::AvoidLineArray;
 using behavior_path_planner::DebugData;
@@ -42,10 +44,12 @@ MarkerArray createPredictedVehiclePositions(const PathWithLaneId & path, std::st
 
 MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, const std::string & ns);
 
-MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info);
+MarkerArray createOtherObjectsMarkerArray(
+  const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose);
 
 MarkerArray createDebugMarkerArray(
-  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug);
+  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
+  const std::shared_ptr<AvoidanceParameters> & parameters);
 }  // namespace behavior_path_planner::utils::avoidance
 
 std::string toStrInfo(const behavior_path_planner::ShiftLineArray & sl_arr);

planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp

@@ -383,7 +383,20 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   // debug
   {
     const std::string ns = "avoidance.debug.";
-    p.publish_debug_marker = getOrDeclareParameter<bool>(*node, ns + "marker");
+    p.enable_other_objects_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_other_objects_marker");
+    p.enable_other_objects_info =
+      getOrDeclareParameter<bool>(*node, ns + "enable_other_objects_info");
+    p.enable_detection_area_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_detection_area_marker");
+    p.enable_drivable_bound_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_drivable_bound_marker");
+    p.enable_safety_check_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_safety_check_marker");
+    p.enable_shift_line_marker =
+      getOrDeclareParameter<bool>(*node, ns + "enable_shift_line_marker");
+    p.enable_lane_marker = getOrDeclareParameter<bool>(*node, ns + "enable_lane_marker");
+    p.enable_misc_marker = getOrDeclareParameter<bool>(*node, ns + "enable_misc_marker");
   }
 
   return p;

planning/behavior_path_avoidance_module/schema/avoidance.schema.json

@@ -1388,13 +1388,57 @@
         "debug": {
           "type": "object",
           "properties": {
-            "marker": {
+            "enable_other_objects_marker": {
               "type": "boolean",
-              "description": "Publish debug marker.",
+              "description": "Publish other objects marker.",
+              "default": "false"
+            },
+            "enable_other_objects_info": {
+              "type": "boolean",
+              "description": "Publish other objects detail information.",
+              "default": "false"
+            },
+            "enable_detection_area_marker": {
+              "type": "boolean",
+              "description": "Publish detection area.",
+              "default": "false"
+            },
+            "enable_drivable_bound_marker": {
+              "type": "boolean",
+              "description": "Publish drivable area boundary.",
+              "default": "false"
+            },
+            "enable_safety_check_marker": {
+              "type": "boolean",
+              "description": "Publish safety check information.",
+              "default": "false"
+            },
+            "enable_shift_line_marker": {
+              "type": "boolean",
+              "description": "Publish shift line information.",
+              "default": "false"
+            },
+            "enable_lane_marker": {
+              "type": "boolean",
+              "description": "Publish lane information.",
+              "default": "false"
+            },
+            "enable_misc_marker": {
+              "type": "boolean",
+              "description": "Publish misc markers.",
               "default": "false"
             }
           },
-          "required": ["marker"],
+          "required": [
+            "enable_other_objects_marker",
+            "enable_other_objects_info",
+            "enable_detection_area_marker",
+            "enable_drivable_bound_marker",
+            "enable_safety_check_marker",
+            "enable_shift_line_marker",
+            "enable_lane_marker",
+            "enable_misc_marker"
+          ],
           "additionalProperties": false
         }
       },

planning/behavior_path_avoidance_module/src/debug.cpp

@@ -130,7 +130,8 @@ MarkerArray createToDrivableBoundDistance(const ObjectDataArray & objects, std::
   return msg;
 }
 
-MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::string && ns)
+MarkerArray createObjectInfoMarkerArray(
+  const ObjectDataArray & objects, std::string && ns, const bool verbose)
 {
   MarkerArray msg;
 
@@ -139,7 +140,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
     createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(1.0, 1.0, 0.0, 1.0));
 
   for (const auto & object : objects) {
-    {
+    if (verbose) {
       marker.id = uuidToInt32(object.object.object_id);
       marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
       std::ostringstream string_stream;
@@ -160,6 +161,7 @@ MarkerArray createObjectInfoMarkerArray(const ObjectDataArray & objects, std::st
 
     {
       marker.id = uuidToInt32(object.object.object_id);
+      marker.pose = object.object.kinematics.initial_pose_with_covariance.pose;
       marker.pose.position.z += 2.0;
       std::ostringstream string_stream;
       string_stream << magic_enum::enum_name(object.info) << (object.is_parked ? "(PARKED)" : "");
@@ -201,7 +203,7 @@ MarkerArray avoidableObjectsMarkerArray(const ObjectDataArray & objects, std::st
       createMarkerColor(1.0, 1.0, 0.0, 0.8)),
     &msg);
 
-  appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
+  appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg);
   appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
   appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
   appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
@@ -220,7 +222,7 @@ MarkerArray unAvoidableObjectsMarkerArray(const ObjectDataArray & objects, std::
       createMarkerColor(1.0, 0.0, 0.0, 0.8)),
     &msg);
 
-  appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info"), &msg);
+  appendMarkerArray(createObjectInfoMarkerArray(objects, ns + "_info", true), &msg);
   appendMarkerArray(createObjectPolygonMarkerArray(objects, ns + "_envelope_polygon"), &msg);
   appendMarkerArray(createToDrivableBoundDistance(objects, ns + "_to_drivable_bound"), &msg);
   appendMarkerArray(createOverhangLaneletMarkerArray(objects, ns + "_overhang_lanelet"), &msg);
@@ -428,7 +430,8 @@ MarkerArray createTargetObjectsMarkerArray(const ObjectDataArray & objects, cons
   return msg;
 }
 
-MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const ObjectInfo & info)
+MarkerArray createOtherObjectsMarkerArray(
+  const ObjectDataArray & objects, const ObjectInfo & info, const bool verbose)
 {
   const auto filtered_objects = [&objects, &info]() {
     ObjectDataArray ret{};
@@ -456,7 +459,8 @@ MarkerArray createOtherObjectsMarkerArray(const ObjectDataArray & objects, const
       filtered_objects, "others_" + ns + "_cube", createMarkerScale(3.0, 1.5, 1.5),
       createMarkerColor(0.0, 1.0, 0.0, 0.8)),
     &msg);
-  appendMarkerArray(createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info"), &msg);
+  appendMarkerArray(
+    createObjectInfoMarkerArray(filtered_objects, "others_" + ns + "_info", verbose), &msg);
   appendMarkerArray(
     createOverhangLaneletMarkerArray(filtered_objects, "others_" + ns + "_overhang_lanelet"), &msg);
 
@@ -500,7 +504,8 @@ MarkerArray createDrivableBounds(
 }
 
 MarkerArray createDebugMarkerArray(
-  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug)
+  const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug,
+  const std::shared_ptr<AvoidanceParameters> & parameters)
 {
   using behavior_path_planner::utils::transformToLanelets;
   using lanelet::visualization::laneletsAsTriangleMarkerArray;
@@ -534,7 +539,7 @@ MarkerArray createDebugMarkerArray(
     };
 
   const auto addObjects = [&](const ObjectDataArray & objects, const auto & ns) {
-    add(createOtherObjectsMarkerArray(objects, ns));
+    add(createOtherObjectsMarkerArray(objects, ns, parameters->enable_other_objects_info));
   };
 
   const auto addShiftLength =
@@ -549,7 +554,7 @@ MarkerArray createDebugMarkerArray(
   };
 
   // ignore objects
-  {
+  if (parameters->enable_other_objects_marker) {
     addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_THRESHOLD);
     addObjects(data.other_objects, ObjectInfo::IS_NOT_TARGET_OBJECT);
     addObjects(data.other_objects, ObjectInfo::FURTHER_THAN_GOAL);
@@ -569,7 +574,7 @@ MarkerArray createDebugMarkerArray(
   }
 
   // shift line pre-process
-  {
+  if (parameters->enable_shift_line_marker) {
     addAvoidLine(debug.step1_registered_shift_line, "step1_registered_shift_line", 0.2, 0.2, 1.0);
     addAvoidLine(debug.step1_current_shift_line, "step1_current_shift_line", 0.2, 0.4, 0.8, 0.3);
     addAvoidLine(debug.step1_merged_shift_line, "step1_merged_shift_line", 0.2, 0.6, 0.6, 0.3);
@@ -578,55 +583,60 @@ MarkerArray createDebugMarkerArray(
   }
 
   // merge process
-  {
+  if (parameters->enable_shift_line_marker) {
     addAvoidLine(debug.step2_merged_shift_line, "step2_merged_shift_line", 0.2, 1.0, 0.0, 0.3);
   }
 
   // trimming process
-  {
+  if (parameters->enable_shift_line_marker) {
     addAvoidLine(debug.step3_grad_filtered_1st, "step3_grad_filtered_1st", 0.2, 0.8, 0.0, 0.3);
     addAvoidLine(debug.step3_grad_filtered_2nd, "step3_grad_filtered_2nd", 0.4, 0.6, 0.0, 0.3);
     addAvoidLine(debug.step3_grad_filtered_3rd, "step3_grad_filtered_3rd", 0.6, 0.4, 0.0, 0.3);
   }
 
   // registering process
-  {
+  if (parameters->enable_shift_line_marker) {
     addShiftLine(shifter.getShiftLines(), "step4_old_shift_line", 1.0, 1.0, 0.0, 0.3);
     addAvoidLine(data.new_shift_line, "step4_new_shift_line", 1.0, 0.0, 0.0, 0.3);
   }
 
   // safety check
-  {
+  if (parameters->enable_safety_check_marker) {
     add(showSafetyCheckInfo(debug.collision_check, "object_debug_info"));
     add(showPredictedPath(debug.collision_check, "ego_predicted_path"));
     add(showPolygon(debug.collision_check, "ego_and_target_polygon_relation"));
   }
 
   // shift length
-  {
+  if (parameters->enable_shift_line_marker) {
     addShiftLength(debug.pos_shift, "merged_length_pos", 0.0, 0.7, 0.5);
     addShiftLength(debug.neg_shift, "merged_length_neg", 0.0, 0.5, 0.7);
     addShiftLength(debug.total_shift, "merged_length_total", 0.99, 0.4, 0.2);
   }
 
   // shift grad
-  {
+  if (parameters->enable_shift_line_marker) {
     addShiftGrad(debug.pos_shift_grad, debug.pos_shift, "merged_grad_pos", 0.0, 0.7, 0.5);
     addShiftGrad(debug.neg_shift_grad, debug.neg_shift, "merged_grad_neg", 0.0, 0.5, 0.7);
     addShiftGrad(debug.total_forward_grad, debug.total_shift, "grad_forward", 0.99, 0.4, 0.2);
     addShiftGrad(debug.total_backward_grad, debug.total_shift, "grad_backward", 0.4, 0.2, 0.9);
   }
 
   // detection area
-  size_t i = 0;
-  for (const auto & detection_area : debug.detection_areas) {
-    add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1));
+  if (parameters->enable_detection_area_marker) {
+    size_t i = 0;
+    for (const auto & detection_area : debug.detection_areas) {
+      add(createPolygonMarkerArray(detection_area, "detection_area", i++, 0.16, 1.0, 0.69, 0.1));
+    }
   }
 
-  // misc
-  {
-    add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
+  // drivable bound
+  if (parameters->enable_drivable_bound_marker) {
     add(createDrivableBounds(data, "drivable_bound", 1.0, 0.0, 0.42));
+  }
+
+  // lane
+  if (parameters->enable_lane_marker) {
     add(laneletsAsTriangleMarkerArray(
       "drivable_lanes", transformToLanelets(data.drivable_lanes),
       createMarkerColor(0.16, 1.0, 0.69, 0.2)));
@@ -636,6 +646,11 @@ MarkerArray createDebugMarkerArray(
       "safety_check_lanes", debug.safety_check_lanes, createMarkerColor(1.0, 0.0, 0.42, 0.2)));
   }
 
+  // misc
+  if (parameters->enable_misc_marker) {
+    add(createPathMarkerArray(path, "centerline_resampled", 0, 0.0, 0.9, 0.5));
+  }
+
   return msg;
 }
 }  // namespace behavior_path_planner::utils::avoidance

planning/behavior_path_avoidance_module/src/manager.cpp

@@ -261,7 +261,17 @@ void AvoidanceModuleManager::updateModuleParams(const std::vector<rclcpp::Parame
 
   {
     const std::string ns = "avoidance.debug.";
-    updateParam<bool>(parameters, ns + "marker", p->publish_debug_marker);
+    updateParam<bool>(
+      parameters, ns + "enable_other_objects_marker", p->enable_other_objects_marker);
+    updateParam<bool>(parameters, ns + "enable_other_objects_info", p->enable_other_objects_info);
+    updateParam<bool>(
+      parameters, ns + "enable_detection_area_marker", p->enable_detection_area_marker);
+    updateParam<bool>(
+      parameters, ns + "enable_drivable_bound_marker", p->enable_drivable_bound_marker);
+    updateParam<bool>(parameters, ns + "enable_safety_check_marker", p->enable_safety_check_marker);
+    updateParam<bool>(parameters, ns + "enable_shift_line_marker", p->enable_shift_line_marker);
+    updateParam<bool>(parameters, ns + "enable_lane_marker", p->enable_lane_marker);
+    updateParam<bool>(parameters, ns + "enable_misc_marker", p->enable_misc_marker);
   }
 
   std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) {

planning/behavior_path_avoidance_module/src/scene.cpp

@@ -1360,12 +1360,7 @@ void AvoidanceModule::updateDebugMarker(
   const AvoidancePlanningData & data, const PathShifter & shifter, const DebugData & debug) const
 {
   debug_marker_.markers.clear();
-
-  if (!parameters_->publish_debug_marker) {
-    return;
-  }
-
-  debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug);
+  debug_marker_ = utils::avoidance::createDebugMarkerArray(data, shifter, debug, parameters_);
 }
 
 void AvoidanceModule::updateAvoidanceDebugData(