refactor(avoidance): logger small change

Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
autowarefoundation · Apr 18, 2024 · c5ae54a · c5ae54a
1 parent 3adffc3
commit c5ae54a
Show file tree

Hide file tree

Showing 9 changed files with 28 additions and 30 deletions.
diff --git a/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml b/common/tier4_logging_level_configure_rviz_plugin/config/logger_config.yaml
@@ -37,6 +37,8 @@ Planning:
   behavior_path_planner_avoidance:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
       logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance
+    - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner
+      logger_name: planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils
 
   behavior_path_planner_goal_planner:
     - node_name: /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner

diff --git a/planning/behavior_path_avoidance_module/config/avoidance.param.yaml b/planning/behavior_path_avoidance_module/config/avoidance.param.yaml
@@ -290,4 +290,3 @@
       # for debug
       debug:
         marker: false
-        console: false
diff --git a/...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp b/...ng/behavior_path_avoidance_module/include/behavior_path_avoidance_module/data_structs.hpp
@@ -322,7 +322,6 @@ struct AvoidanceParameters
 
   // debug
   bool publish_debug_marker = false;
-  bool print_debug_info = false;
 };
 
 struct ObjectData  // avoidance target

diff --git a/...ehavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp b/...ehavior_path_avoidance_module/include/behavior_path_avoidance_module/parameter_helper.hpp
@@ -379,7 +379,6 @@ AvoidanceParameters getParameter(rclcpp::Node * node)
   {
     const std::string ns = "avoidance.debug.";
     p.publish_debug_marker = getOrDeclareParameter<bool>(*node, ns + "marker");
-    p.print_debug_info = getOrDeclareParameter<bool>(*node, ns + "console");
   }
 
   return p;

diff --git a/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp b/planning/behavior_path_avoidance_module/include/behavior_path_avoidance_module/utils.hpp
@@ -31,6 +31,9 @@ using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityAndPoly
 using behavior_path_planner::utils::path_safety_checker::PoseWithVelocityStamped;
 using behavior_path_planner::utils::path_safety_checker::PredictedPathWithPolygon;
 
+static constexpr const char * logger_namespace =
+  "planning.scenario_planning.lane_driving.behavior_planning.behavior_path_planner.avoidance.utils";
+
 bool isOnRight(const ObjectData & obj);
 
 double calcShiftLength(

diff --git a/planning/behavior_path_avoidance_module/schema/avoidance.schema.json b/planning/behavior_path_avoidance_module/schema/avoidance.schema.json
@@ -1379,14 +1379,9 @@
               "type": "boolean",
               "description": "Publish debug marker.",
               "default": "false"
-            },
-            "console": {
-              "type": "boolean",
-              "description": "Output debug info on console.",
-              "default": "false"
             }
           },
-          "required": ["marker", "console"],
+          "required": ["marker"],
           "additionalProperties": false
         }
       },

diff --git a/planning/behavior_path_avoidance_module/src/manager.cpp b/planning/behavior_path_avoidance_module/src/manager.cpp
@@ -257,7 +257,6 @@ 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 + "console", p->print_debug_info);
   }
 
   std::for_each(observers_.begin(), observers_.end(), [&p](const auto & observer) {

diff --git a/planning/behavior_path_avoidance_module/src/scene.cpp b/planning/behavior_path_avoidance_module/src/scene.cpp
@@ -40,12 +40,6 @@
 #include <string>
 #include <vector>
 
-// set as macro so that calling function name will be printed.
-// debug print is heavy. turn on only when debugging.
-#define DEBUG_PRINT(...) \
-  RCLCPP_DEBUG_EXPRESSION(getLogger(), parameters_->print_debug_info, __VA_ARGS__)
-#define printShiftLines(p, msg) DEBUG_PRINT("[%s] %s", msg, toStrInfo(p).c_str())
-
 namespace behavior_path_planner
 {
 
@@ -109,7 +103,7 @@ AvoidanceModule::AvoidanceModule(
 
 bool AvoidanceModule::isExecutionRequested() const
 {
-  DEBUG_PRINT("AVOIDANCE isExecutionRequested");
+  RCLCPP_DEBUG(getLogger(), "AVOIDANCE isExecutionRequested");
 
   // Check ego is in preferred lane
   updateInfoMarker(avoid_data_);
@@ -132,7 +126,11 @@ bool AvoidanceModule::isExecutionRequested() const
 
 bool AvoidanceModule::isExecutionReady() const
 {
-  DEBUG_PRINT("AVOIDANCE isExecutionReady");
+  RCLCPP_DEBUG_STREAM(getLogger(), "---Avoidance GO/NO-GO status---");
+  RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "SAFE:" << avoid_data_.safe);
+  RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "COMFORTABLE:" << avoid_data_.comfortable);
+  RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "VALID:" << avoid_data_.valid);
+  RCLCPP_DEBUG_STREAM(getLogger(), std::boolalpha << "READY:" << avoid_data_.ready);
   return avoid_data_.safe && avoid_data_.comfortable && avoid_data_.valid && avoid_data_.ready;
 }
 
@@ -1139,7 +1137,7 @@ void AvoidanceModule::addNewShiftLines(
   const auto new_shift_length = front_new_shift_line.end_shift_length;
   const auto new_shift_end_idx = front_new_shift_line.end_idx;
 
-  DEBUG_PRINT("min_start_idx = %lu", min_start_idx);
+  RCLCPP_DEBUG(getLogger(), "min_start_idx = %lu", min_start_idx);
 
   // Remove shift points that starts later than the new_shift_line from path_shifter.
   //
@@ -1152,8 +1150,9 @@ void AvoidanceModule::addNewShiftLines(
   // farther avoidance.
   for (const auto & sl : current_shift_lines) {
     if (sl.start_idx >= min_start_idx) {
-      DEBUG_PRINT(
-        "sl.start_idx = %lu, this sl starts after new proposal. remove this one.", sl.start_idx);
+      RCLCPP_DEBUG(
+        getLogger(), "sl.start_idx = %lu, this sl starts after new proposal. remove this one.",
+        sl.start_idx);
       continue;
     }
 
@@ -1171,7 +1170,7 @@ void AvoidanceModule::addNewShiftLines(
       }
     }
 
-    DEBUG_PRINT("sl.start_idx = %lu, no conflict. keep this one.", sl.start_idx);
+    RCLCPP_DEBUG(getLogger(), "sl.start_idx = %lu, no conflict. keep this one.", sl.start_idx);
     future.push_back(sl);
   }
 

diff --git a/planning/behavior_path_avoidance_module/src/utils.cpp b/planning/behavior_path_avoidance_module/src/utils.cpp
@@ -563,13 +563,15 @@ bool isNeverAvoidanceTarget(
   if (object.is_within_intersection) {
     if (object.behavior == ObjectData::Behavior::NONE) {
       object.reason = "ParallelToEgoLane";
-      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it.");
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace), "object belongs to ego lane. never avoid it.");
       return true;
     }
 
     if (object.behavior == ObjectData::Behavior::MERGING) {
       object.reason = "MergingToEgoLane";
-      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object belongs to ego lane. never avoid it.");
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace), "object belongs to ego lane. never avoid it.");
       return true;
     }
   }
@@ -581,15 +583,17 @@ bool isNeverAvoidanceTarget(
       planner_data->route_handler->getLeftLanelet(object.overhang_lanelet, true, false);
     if (right_lane.has_value() && left_lane.has_value()) {
       object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
-      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object isn't on the edge lane. never avoid it.");
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace), "object isn't on the edge lane. never avoid it.");
       return true;
     }
   }
 
   if (isCloseToStopFactor(object, data, planner_data, parameters)) {
     if (object.is_on_ego_lane && !object.is_parked) {
       object.reason = AvoidanceDebugFactor::NOT_PARKING_OBJECT;
-      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is close to stop factor. never avoid it.");
+      RCLCPP_DEBUG(
+        rclcpp::get_logger(logger_namespace), "object is close to stop factor. never avoid it.");
       return true;
     }
   }
@@ -604,12 +608,12 @@ bool isObviousAvoidanceTarget(
 {
   if (!object.is_within_intersection) {
     if (object.is_parked && object.behavior == ObjectData::Behavior::NONE) {
-      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is obvious parked vehicle.");
+      RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is obvious parked vehicle.");
       return true;
     }
 
     if (!object.is_on_ego_lane && object.behavior == ObjectData::Behavior::NONE) {
-      RCLCPP_DEBUG(rclcpp::get_logger(__func__), "object is adjacent vehicle.");
+      RCLCPP_DEBUG(rclcpp::get_logger(logger_namespace), "object is adjacent vehicle.");
       return true;
     }
   }
@@ -2187,8 +2191,7 @@ DrivableLanes generateExpandedDrivableLanes(
         }
         if (i == max_recursive_search_num - 1) {
           RCLCPP_ERROR(
-            rclcpp::get_logger("behavior_path_planner").get_child("avoidance"),
-            "Drivable area expansion reaches max iteration.");
+            rclcpp::get_logger(logger_namespace), "Drivable area expansion reaches max iteration.");
         }
       }
     };