refactor(bpp, avoidance): remove unnecessary verbose flag (autowarefo…

…undation#6822) * refactor(avoidance): logger small change Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> * refactor(bpp): remove verbose flag Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com> --------- Signed-off-by: satoshi-ota <satoshi.ota928@gmail.com>
satoshi-ota · Apr 22, 2024 · c09bc59 · c09bc59
1 parent 75885a5
commit c09bc59
Show file tree

Hide file tree

Showing 14 changed files with 33 additions and 46 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.");
         }
       }
     };

diff --git a/planning/behavior_path_planner/config/behavior_path_planner.param.yaml b/planning/behavior_path_planner/config/behavior_path_planner.param.yaml
@@ -1,6 +1,5 @@
 /**:
   ros__parameters:
-    verbose: false
     max_iteration_num: 100
     traffic_light_signal_timeout: 1.0
     planning_hz: 10.0

diff --git a/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/planner_manager.hpp
@@ -96,7 +96,7 @@ struct SceneModuleStatus
 class PlannerManager
 {
 public:
-  PlannerManager(rclcpp::Node & node, const size_t max_iteration_num, const bool verbose);
+  PlannerManager(rclcpp::Node & node, const size_t max_iteration_num);
 
   /**
    * @brief run all candidate and approved modules.
@@ -471,8 +471,6 @@ class PlannerManager
   mutable std::shared_ptr<SceneModuleVisitor> debug_msg_ptr_;
 
   size_t max_iteration_num_{100};
-
-  bool verbose_{false};
 };
 }  // namespace behavior_path_planner
 

diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp
@@ -133,7 +133,7 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     const std::lock_guard<std::mutex> lock(mutex_manager_);  // for planner_manager_
 
     const auto & p = planner_data_->parameters;
-    planner_manager_ = std::make_shared<PlannerManager>(*this, p.max_iteration_num, p.verbose);
+    planner_manager_ = std::make_shared<PlannerManager>(*this, p.max_iteration_num);
 
     for (const auto & name : declare_parameter<std::vector<std::string>>("launch_modules")) {
       // workaround: Since ROS 2 can't get empty list, launcher set [''] on the parameter.
@@ -208,7 +208,6 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam()
 {
   BehaviorPathPlannerParameters p{};
 
-  p.verbose = declare_parameter<bool>("verbose");
   p.max_iteration_num = declare_parameter<int>("max_iteration_num");
   p.traffic_light_signal_timeout = declare_parameter<double>("traffic_light_signal_timeout");
 

diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp
@@ -30,13 +30,11 @@
 
 namespace behavior_path_planner
 {
-PlannerManager::PlannerManager(
-  rclcpp::Node & node, const size_t max_iteration_num, const bool verbose)
+PlannerManager::PlannerManager(rclcpp::Node & node, const size_t max_iteration_num)
 : plugin_loader_("behavior_path_planner", "behavior_path_planner::SceneModuleManagerInterface"),
   logger_(node.get_logger().get_child("planner_manager")),
   clock_(*node.get_clock()),
-  max_iteration_num_{max_iteration_num},
-  verbose_{verbose}
+  max_iteration_num_{max_iteration_num}
 {
   processing_time_.emplace("total_time", 0.0);
   debug_publisher_ptr_ = std::make_unique<DebugPublisher>(&node, "~/debug");
@@ -946,11 +944,7 @@ void PlannerManager::print() const
 
   state_publisher_ptr_->publish<DebugStringMsg>("internal_state", string_stream.str());
 
-  if (!verbose_) {
-    return;
-  }
-
-  RCLCPP_INFO_STREAM(logger_, string_stream.str());
+  RCLCPP_DEBUG_STREAM(logger_, string_stream.str());
 }
 
 void PlannerManager::publishProcessingTime() const

diff --git a/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp b/planning/behavior_path_planner_common/include/behavior_path_planner_common/parameters.hpp
@@ -30,7 +30,6 @@ struct ModuleConfigParameters
 
 struct BehaviorPathPlannerParameters
 {
-  bool verbose;
   size_t max_iteration_num{100};
   double traffic_light_signal_timeout{1.0};