feat(start_planner): visualize planner evaluation table in rviz (auto…

…warefoundation#10029)

visualize planner evaluation table in rviz

Signed-off-by: Kyoichi Sugahara <kyoichi.sugahara@tier4.jp>

planning/behavior_path_planner/autoware_behavior_path_planner/include/autoware/behavior_path_planner/behavior_path_planner_node.hpp

@@ -17,6 +17,7 @@
 
 #include "autoware/behavior_path_planner_common/data_manager.hpp"
 #include "autoware/behavior_path_planner_common/interface/scene_module_interface.hpp"
+#include "autoware/universe_utils/ros/debug_publisher.hpp"
 #include "autoware/universe_utils/ros/logger_level_configure.hpp"
 #include "planner_manager.hpp"
 
@@ -72,6 +73,7 @@ using tier4_planning_msgs::msg::RerouteAvailability;
 using tier4_planning_msgs::msg::Scenario;
 using visualization_msgs::msg::Marker;
 using visualization_msgs::msg::MarkerArray;
+using DebugPublisher = autoware::universe_utils::DebugPublisher;
 
 class BehaviorPathPlannerNode : public rclcpp::Node
 {
@@ -186,6 +188,7 @@ class BehaviorPathPlannerNode : public rclcpp::Node
   // debug
   rclcpp::Publisher<AvoidanceDebugMsgArray>::SharedPtr debug_avoidance_msg_array_publisher_;
   rclcpp::Publisher<MarkerArray>::SharedPtr debug_turn_signal_info_publisher_;
+  std::unique_ptr<DebugPublisher> debug_start_planner_evaluation_table_publisher_ptr_;
 
   /**
    * @brief publish reroute availability

planning/behavior_path_planner/autoware_behavior_path_planner/src/behavior_path_planner_node.cpp

@@ -20,6 +20,7 @@
 #include <autoware/universe_utils/ros/update_param.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
+#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
 #include <tier4_planning_msgs/msg/path_change_module_id.hpp>
 
 #include <memory>
@@ -30,6 +31,7 @@ namespace autoware::behavior_path_planner
 {
 using autoware::vehicle_info_utils::VehicleInfoUtils;
 using tier4_planning_msgs::msg::PathChangeModuleId;
+using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped;
 
 BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & node_options)
 : Node("behavior_path_planner", node_options),
@@ -55,9 +57,13 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod
     create_publisher<PoseWithUuidStamped>("~/output/modified_goal", durable_qos);
   reroute_availability_publisher_ =
     create_publisher<RerouteAvailability>("~/output/is_reroute_available", 1);
+
   debug_avoidance_msg_array_publisher_ =
     create_publisher<AvoidanceDebugMsgArray>("~/debug/avoidance_debug_message_array", 1);
 
+  debug_start_planner_evaluation_table_publisher_ptr_ =
+    std::make_unique<DebugPublisher>(this, "~/debug/start_planner_evaluation_table");
+
   debug_turn_signal_info_publisher_ = create_publisher<MarkerArray>("~/debug/turn_signal_info", 1);
 
   bound_publisher_ = create_publisher<MarkerArray>("~/debug/bound", 1);
@@ -608,6 +614,11 @@ void BehaviorPathPlannerNode::publishSceneModuleDebugMsg(
   if (avoidance_debug_message) {
     debug_avoidance_msg_array_publisher_->publish(*avoidance_debug_message);
   }
+  const auto start_planner_debug_message = debug_messages_data_ptr->getStartPlannerModuleDebugMsg();
+  if (start_planner_debug_message) {
+    debug_start_planner_evaluation_table_publisher_ptr_->publish<DebugStringMsg>(
+      "start_planner_evaluation_table", *(start_planner_debug_message));
+  }
 }
 
 void BehaviorPathPlannerNode::publishPathCandidate(

planning/behavior_path_planner/autoware_behavior_path_planner_common/include/autoware/behavior_path_planner_common/interface/scene_module_visitor.hpp

@@ -16,6 +16,7 @@
 #define AUTOWARE__BEHAVIOR_PATH_PLANNER_COMMON__INTERFACE__SCENE_MODULE_VISITOR_HPP_
 
 #include "tier4_planning_msgs/msg/detail/avoidance_debug_msg_array__struct.hpp"
+#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
 
 #include <memory>
 namespace autoware::behavior_path_planner
@@ -31,16 +32,20 @@ class SideShiftModule;
 
 using tier4_planning_msgs::msg::AvoidanceDebugMsg;
 using tier4_planning_msgs::msg::AvoidanceDebugMsgArray;
+using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped;
 
 class SceneModuleVisitor
 {
 public:
   void visitAvoidanceModule(const StaticObstacleAvoidanceModule * module) const;
+  void visitStartPlannerModule(const StartPlannerModule * module) const;
 
   std::shared_ptr<AvoidanceDebugMsgArray> getAvoidanceModuleDebugMsg() const;
+  std::shared_ptr<DebugStringMsg> getStartPlannerModuleDebugMsg() const;
 
 protected:
   mutable std::shared_ptr<AvoidanceDebugMsgArray> avoidance_visitor_;
+  mutable std::shared_ptr<DebugStringMsg> start_planner_visitor_;
 };
 }  // namespace autoware::behavior_path_planner
 

planning/behavior_path_planner/autoware_behavior_path_planner_common/src/interface/scene_module_visitor.cpp

@@ -24,4 +24,10 @@ std::shared_ptr<AvoidanceDebugMsgArray> SceneModuleVisitor::getAvoidanceModuleDe
 {
   return avoidance_visitor_;
 }
+
+std::shared_ptr<DebugStringMsg> SceneModuleVisitor::getStartPlannerModuleDebugMsg() const
+{
+  return start_planner_visitor_;
+}
+
 }  // namespace autoware::behavior_path_planner

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/data_structs.hpp

@@ -26,7 +26,6 @@
 
 #include <string>
 #include <vector>
-
 namespace autoware::behavior_path_planner
 {
 
@@ -51,29 +50,34 @@ struct PlannerDebugData
 {
 public:
   PlannerType planner_type;
-  std::vector<std::string> conditions_evaluation;
-  double required_margin{0.0};
   double backward_distance{0.0};
+  double required_margin{0.0};
+  std::vector<std::string> conditions_evaluation;
 
-  auto header_str() const
+  static std::string double_to_str(double value, int precision = 1)
   {
-    std::stringstream ss;
-    ss << std::left << std::setw(20) << "| Planner type " << std::setw(20) << "| Required margin "
-       << std::setw(20) << "| Backward distance " << std::setw(25) << "| Condition evaluation |"
-       << "\n";
-    return ss.str();
+    std::ostringstream oss;
+    oss << std::fixed << std::setprecision(precision) << value;
+    return oss.str();
   }
 
-  auto str() const
+  static std::string to_planner_type_name(PlannerType pt)
   {
-    std::stringstream ss;
-    for (const auto & result : conditions_evaluation) {
-      ss << std::left << std::setw(23) << magic_enum::enum_name(planner_type) << std::setw(23)
-         << (std::to_string(required_margin) + "[m]") << std::setw(23)
-         << (std::to_string(backward_distance) + "[m]") << std::setw(25) << result << "\n";
+    // Adding whitespace for column width alignment in RViz display
+    switch (pt) {
+      case PlannerType::NONE:
+        return "NONE                  ";
+      case PlannerType::SHIFT:
+        return "SHIFT               ";
+      case PlannerType::GEOMETRIC:
+        return "GEOMETRIC   ";
+      case PlannerType::STOP:
+        return "STOP                  ";
+      case PlannerType::FREESPACE:
+        return "FREESPACE   ";
+      default:
+        return "UNKNOWN";
     }
-    ss << std::setw(40);
-    return ss.str();
   }
 };
 struct StartPlannerDebugData

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/include/autoware/behavior_path_start_planner_module/start_planner_module.hpp

@@ -26,10 +26,12 @@
 #include "autoware/behavior_path_start_planner_module/geometric_pull_out.hpp"
 #include "autoware/behavior_path_start_planner_module/pull_out_path.hpp"
 #include "autoware/behavior_path_start_planner_module/shift_pull_out.hpp"
+#include "data_structs.hpp"
 
 #include <autoware_vehicle_info_utils/vehicle_info.hpp>
 #include <autoware_vehicle_info_utils/vehicle_info_utils.hpp>
 
+#include <autoware_internal_debug_msgs/msg/string_stamped.hpp>
 #include <tier4_planning_msgs/msg/path_with_lane_id.hpp>
 
 #include <lanelet2_core/Forward.h>
@@ -52,6 +54,7 @@ using autoware::behavior_path_planner::utils::path_safety_checker::SafetyCheckPa
 using autoware::behavior_path_planner::utils::path_safety_checker::TargetObjectsOnLane;
 using geometry_msgs::msg::PoseArray;
 using PriorityOrder = std::vector<std::pair<size_t, std::shared_ptr<PullOutPlannerBase>>>;
+using DebugStringMsg = autoware_internal_debug_msgs::msg::StringStamped;
 
 struct PullOutStatus
 {
@@ -146,16 +149,16 @@ class StartPlannerModule : public SceneModuleInterface
   }
   void resetStatus();
 
-  void acceptVisitor(
-    [[maybe_unused]] const std::shared_ptr<SceneModuleVisitor> & visitor) const override
-  {
-  }
+  void acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const override;
 
   // Condition to disable simultaneous execution
   bool isDrivingForward() const { return status_.driving_forward; }
   bool isFreespacePlanning() const { return status_.planner_type == PlannerType::FREESPACE; }
 
+  std::string get_planner_evaluation_table() const { return planner_evaluation_table_; }
+
 private:
+  friend class SceneModuleVisitor;
   struct StartPlannerData
   {
     StartPlannerParameters parameters;
@@ -283,6 +286,7 @@ ego pose.
   std::vector<std::shared_ptr<PullOutPlannerBase>> start_planners_;
   PullOutStatus status_;
   mutable StartPlannerDebugData debug_data_;
+  std::string planner_evaluation_table_;
 
   // Keeps track of lanelets that should be ignored when calculating the turnSignalInfo for this
   // module's output. If the ego vehicle is in this lanelet, the calculation is skipped.
@@ -352,6 +356,10 @@ ego pose.
     const StartPlannerParameters & parameters,
     const std::shared_ptr<const PlannerData> & planner_data, const PullOutStatus & pull_out_status);
 
+  std::string create_planner_evaluation_table(
+    const std::vector<PlannerDebugData> & planner_debug_data_vector) const;
+  void set_planner_evaluation_table(const std::vector<PlannerDebugData> & debug_data_vector);
+
   void setDebugData();
   void logPullOutStatus(rclcpp::Logger::Level log_level = rclcpp::Logger::Level::Info) const;
 };

planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/start_planner_module.cpp

@@ -894,16 +894,6 @@ void StartPlannerModule::planWithPriority(
 
   if (start_pose_candidates.empty()) return;
 
-  auto get_accumulated_debug_stream = [](const std::vector<PlannerDebugData> & debug_data_vector) {
-    std::stringstream ss;
-    if (debug_data_vector.empty()) return ss;
-    ss << debug_data_vector.front().header_str();
-    for (const auto & debug_data : debug_data_vector) {
-      ss << debug_data.str();
-    }
-    return ss;
-  };
-
   const PriorityOrder order_priority =
     determinePriorityOrder(search_priority, start_pose_candidates.size());
 
@@ -918,20 +908,13 @@ void StartPlannerModule::planWithPriority(
               collision_check_margin, debug_data_vector)) {
           debug_data_.selected_start_pose_candidate_index = index;
           debug_data_.margin_for_start_pose_candidate = collision_check_margin;
-          if (parameters_->print_debug_info) {
-            const auto ss = get_accumulated_debug_stream(debug_data_vector);
-            DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str());
-          }
+          set_planner_evaluation_table(debug_data_vector);
           return;
         }
       }
     }
   }
-
-  if (parameters_->print_debug_info) {
-    const auto ss = get_accumulated_debug_stream(debug_data_vector);
-    DEBUG_PRINT("\nPull out path search results:\n%s", ss.str().c_str());
-  }
+  set_planner_evaluation_table(debug_data_vector);
   updateStatusIfNoSafePathFound();
 }
 
@@ -973,7 +956,7 @@ bool StartPlannerModule::findPullOutPath(
 
   planner->setCollisionCheckMargin(collision_check_margin);
   PlannerDebugData debug_data{
-    planner->getPlannerType(), {}, collision_check_margin, backwards_distance};
+    planner->getPlannerType(), backwards_distance, collision_check_margin, {}};
 
   const auto pull_out_path =
     planner->plan(start_pose_candidate, goal_pose, planner_data_, debug_data);
@@ -1580,12 +1563,9 @@ std::optional<PullOutStatus> StartPlannerModule::planFreespacePath(
 
   for (const auto & p : center_line_path.points) {
     const Pose end_pose = p.point.pose;
-    PlannerDebugData debug_data{freespace_planner_->getPlannerType(), {}, 0.0, 0.0};
+    PlannerDebugData debug_data{freespace_planner_->getPlannerType(), 0.0, 0.0, {}};
     auto freespace_path =
       freespace_planner_->plan(current_pose, end_pose, planner_data, debug_data);
-    DEBUG_PRINT(
-      "\nFreespace Pull out path search results\n%s%s", debug_data.header_str().c_str(),
-      debug_data.str().c_str());
     if (!freespace_path) {
       continue;
     }
@@ -1628,6 +1608,83 @@ void StartPlannerModule::setDrivableAreaInfo(BehaviorModuleOutput & output) cons
   }
 }
 
+std::string StartPlannerModule::create_planner_evaluation_table(
+  const std::vector<PlannerDebugData> & planner_debug_data_vector) const
+{
+  if (planner_debug_data_vector.empty()) {
+    return "";
+  }
+
+  const std::string header_planner_type = "Planner type ";
+  const std::string header_required_margin = "Required margin [m]";
+  const std::string header_backward_distance = "Backward distance [m]";
+  const std::string header_condition_eval = "Condition";
+
+  std::ostringstream oss;
+  oss << "-----------------------------------------------------------------------------------------"
+         "----------------"
+      << "\n";
+  oss << "| " << std::left << header_planner_type << " | " << header_required_margin << " | "
+      << header_backward_distance << " | " << header_condition_eval << " \n";
+  oss << "-----------------------------------------------------------------------------------------"
+         "----------------"
+      << "\n";
+
+  for (const auto & d : planner_debug_data_vector) {
+    const std::string pt_str = PlannerDebugData::to_planner_type_name(d.planner_type);
+    const std::string rm_str =
+      PlannerDebugData::double_to_str(d.required_margin, 1) + "                                   ";
+    const std::string bd_str = PlannerDebugData::double_to_str(d.backward_distance, 1) +
+                               "                                      ";
+
+    if (d.conditions_evaluation.empty()) {
+      oss << "| " << std::left << pt_str << " | " << rm_str << " | " << bd_str << " | "
+          << "Unexpected empty condition evaluation"
+          << " \n";
+    } else {
+      for (size_t i = 0; i < d.conditions_evaluation.size(); ++i) {
+        const std::string cond_with_index =
+          "#" + std::to_string(i + 1) + ": " + d.conditions_evaluation[i];
+
+        oss << "| " << std::left << pt_str << " | " << rm_str << " | " << bd_str << " | "
+            << cond_with_index << " \n";
+      }
+    }
+  }
+
+  return oss.str();
+}
+
+void StartPlannerModule::set_planner_evaluation_table(
+  const std::vector<PlannerDebugData> & debug_data_vector)
+{
+  planner_evaluation_table_.clear();
+  if (debug_data_vector.empty()) {
+    return;
+  }
+
+  planner_evaluation_table_ = create_planner_evaluation_table(debug_data_vector);
+}
+
+void StartPlannerModule::acceptVisitor(const std::shared_ptr<SceneModuleVisitor> & visitor) const
+{
+  if (visitor) {
+    visitor->visitStartPlannerModule(this);
+  }
+}
+
+void SceneModuleVisitor::visitStartPlannerModule(const StartPlannerModule * module) const
+{
+  auto debug_msg = std::make_shared<autoware_internal_debug_msgs::msg::StringStamped>();
+  auto debug_info = module->get_planner_evaluation_table();
+  if (debug_info.empty()) return;
+
+  debug_msg->stamp = rclcpp::Clock{RCL_ROS_TIME}.now();
+  debug_msg->data = debug_info;
+
+  start_planner_visitor_ = debug_msg;
+}
+
 void StartPlannerModule::setDebugData()
 {
   using autoware::universe_utils::createDefaultMarker;