feat(planning_factor): add console output option

planning/autoware_core_planning/config/behavior_velocity_planner/behavior_velocity_planner.param.yaml

@@ -4,3 +4,6 @@
     backward_path_length: 5.0
     behavior_output_path_interval: 1.0
     stop_line_extend_length: 5.0
+    planning_factor_console_output:
+      enable: false
+      duration: 1000 # ms

planning/autoware_planning_factor_interface/README.md

@@ -39,6 +39,15 @@ public:
       &node, "avoidance_planner")}
 ```
 
+You can also enable console output for debugging by setting the appropriate parameters:
+
+```cpp
+// Enable console output with a 1000ms throttle duration
+planning_factor_interface_ = std::make_unique<
+  autoware::planning_factor_interface::PlanningFactorInterface>(
+  &node, "avoidance_planner", true, 1000);
+```
+
 ### Adding Planning Factors
 
 ```cpp

planning/autoware_planning_factor_interface/include/autoware/planning_factor_interface/planning_factor_interface.hpp

@@ -27,6 +27,7 @@
 #include <autoware_planning_msgs/msg/trajectory_point.hpp>
 #include <geometry_msgs/msg/pose.hpp>
 
+#include <sstream>
 #include <string>
 #include <vector>
 
@@ -42,11 +43,15 @@
 class PlanningFactorInterface
 {
 public:
-  PlanningFactorInterface(rclcpp::Node * node, const std::string & name)
+  PlanningFactorInterface(
+    rclcpp::Node * node, const std::string & name, bool enable_console_output = false,
+    int throttle_duration_ms = 1000)
   : name_{name},
     pub_factors_{
       node->create_publisher<PlanningFactorArray>("/planning/planning_factors/" + name, 1)},
-    clock_{node->get_clock()}
+    clock_{node->get_clock()},
+    enable_console_output_{enable_console_output},
+    throttle_duration_ms_{throttle_duration_ms}
   {
   }
 
@@ -203,17 +208,40 @@
 
     pub_factors_->publish(msg);
 
+    if (enable_console_output_ && !factors_.empty()) {
+      print_factors_to_console(msg);
+    }
+
     factors_.clear();
   }
 
 private:
+  /**
+   * @brief Print message to console in YAML format
+   * @param msg The message to print
+   */
+  void print_factors_to_console(const PlanningFactorArray & msg)
+  {
+    const std::string output_str =
+      "Planning factor:\n" + autoware_internal_planning_msgs::msg::to_yaml(msg);
+    if (throttle_duration_ms_ > 0) {
+      RCLCPP_INFO_THROTTLE(
+        rclcpp::get_logger(name_), *clock_, throttle_duration_ms_, "%s", output_str.c_str());
+    } else {
+      RCLCPP_INFO(rclcpp::get_logger(name_), "%s", output_str.c_str());
+    }
+  }
+
   std::string name_;
 
   rclcpp::Publisher<PlanningFactorArray>::SharedPtr pub_factors_;
 
   rclcpp::Clock::SharedPtr clock_;
 
   std::vector<PlanningFactor> factors_;
+
+  bool enable_console_output_{false};
+  int throttle_duration_ms_{0};
 };
 
 extern template void

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/config/behavior_velocity_planner.param.yaml

@@ -3,3 +3,6 @@
     forward_path_length: 1000.0
     backward_path_length: 5.0
     behavior_output_path_interval: 1.0
+    planning_factor_console_output:
+      enable: false
+      duration: 1000 # ms

planning/behavior_velocity_planner/autoware_behavior_velocity_planner/schema/behavior_velocity_planner.schema.json

@@ -20,9 +20,32 @@
           "type": "number",
           "default": "1.0",
           "description": "the output path will be interpolated by this interval"
+        },
+        "planning_factor_console_output": {
+          "type": "object",
+          "properties": {
+            "enable": {
+              "type": "boolean",
+              "default": false,
+              "description": "enable console output for planning factors"
+            },
+            "duration": {
+              "type": "number",
+              "default": 1000,
+              "description": "output duration in milliseconds"
+            }
+          },
+          "required": ["enable", "duration"],
+          "additionalProperties": false,
+          "description": "configuration for planning factor console output"
         }
       },
-      "required": ["forward_path_length", "behavior_output_path_interval", "backward_path_length"],
+      "required": [
+        "forward_path_length",
+        "behavior_output_path_interval",
+        "backward_path_length",
+        "planning_factor_console_output"
+      ],
       "additionalProperties": false
     }
   },

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/planner_data.hpp

@@ -66,6 +66,9 @@ struct PlannerData
 
   bool is_simulation = false;
 
+  bool planning_factor_console_output_enabled = false;
+  int planning_factor_console_output_duration = 0;
+
   std::shared_ptr<autoware::velocity_smoother::SmootherBase> velocity_smoother_;
   std::shared_ptr<autoware::route_handler::RouteHandler> route_handler_;
   autoware::vehicle_info_utils::VehicleInfo vehicle_info_;

planning/behavior_velocity_planner/autoware_behavior_velocity_planner_common/include/autoware/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -152,8 +152,15 @@ class SceneModuleManagerInterface
     }
     pub_virtual_wall_ = node.create_publisher<visualization_msgs::msg::MarkerArray>(
       std::string("~/virtual_wall/") + module_name, 5);
+
+    const bool enable_console_output =
+      get_or_declare_parameter<bool>(node, "planning_factor_console_output.enable");
+    const int throttle_duration_ms =
+      get_or_declare_parameter<int>(node, "planning_factor_console_output.duration");
+
     planning_factor_interface_ =
-      std::make_shared<planning_factor_interface::PlanningFactorInterface>(&node, module_name);
+      std::make_shared<planning_factor_interface::PlanningFactorInterface>(
+        &node, module_name, enable_console_output, throttle_duration_ms);
 
     processing_time_publisher_ = std::make_shared<DebugPublisher>(&node, "~/debug");