chore: add debug publisher of pass judge

Signed-off-by: Tomohito Ando <[email protected]>

planning/behavior_velocity_planner_common/include/behavior_velocity_planner_common/scene_module_interface.hpp

@@ -410,6 +410,29 @@ class SceneModuleManagerInterfaceWithRTC : public SceneModuleManagerInterface
   }
 };
 
+class DebugDataPublisher
+{
+public:
+  using Float32Stamped = tier4_debug_msgs::msg::Float32Stamped;
+
+  explicit DebugDataPublisher(rclcpp::Node & node) : node_(node)
+  {
+    pub_dist_to_stop_line_ =
+      node.create_publisher<Float32Stamped>("~/debug/traffic_light/dist_to_stop_line", 1);
+    pub_reachable_dist_ =
+      node.create_publisher<Float32Stamped>("~/debug/traffic_light/reachable_dist", 1);
+    pub_pass_judge_dist_ =
+      node.create_publisher<Float32Stamped>("~/debug/traffic_light/pass_judge_dist", 1);
+  }
+
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_dist_to_stop_line_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_reachable_dist_;
+  rclcpp::Publisher<Float32Stamped>::SharedPtr pub_pass_judge_dist_;
+
+private:
+  rclcpp::Node & node_;
+};
+
 }  // namespace behavior_velocity_planner
 
 #endif  // BEHAVIOR_VELOCITY_PLANNER_COMMON__SCENE_MODULE_INTERFACE_HPP_

planning/behavior_velocity_traffic_light_module/src/manager.cpp

@@ -37,6 +37,7 @@ TrafficLightModuleManager::TrafficLightModuleManager(rclcpp::Node & node)
   planner_param_.yellow_lamp_period = node.declare_parameter<double>(ns + ".yellow_lamp_period");
   pub_tl_state_ = node.create_publisher<autoware_perception_msgs::msg::TrafficSignal>(
     "~/output/traffic_signal", 1);
+  debug_data_publisher_ = std::make_shared<DebugDataPublisher>(node);
 }
 
 void TrafficLightModuleManager::modifyPathVelocity(
@@ -124,7 +125,8 @@ void TrafficLightModuleManager::launchNewModules(
     if (!isModuleRegisteredFromExistingAssociatedModule(module_id)) {
       registerModule(std::make_shared<TrafficLightModule>(
         module_id, lane_id, *(traffic_light_reg_elem.first), traffic_light_reg_elem.second,
-        planner_param_, logger_.get_child("traffic_light_module"), clock_));
+        planner_param_, logger_.get_child("traffic_light_module"), clock_,
+        debug_data_publisher_));
       generateUUID(module_id);
       updateRTCStatus(
         getUUID(module_id), true, std::numeric_limits<double>::lowest(), path.header.stamp);

planning/behavior_velocity_traffic_light_module/src/manager.hpp

@@ -58,6 +58,8 @@ class TrafficLightModuleManager : public SceneModuleManagerInterfaceWithRTC
   rclcpp::Publisher<autoware_perception_msgs::msg::TrafficSignal>::SharedPtr pub_tl_state_;
 
   boost::optional<int> first_ref_stop_path_point_index_;
+
+  std::shared_ptr<DebugDataPublisher> debug_data_publisher_;
 };
 
 class TrafficLightModulePlugin : public PluginWrapper<TrafficLightModuleManager>

planning/behavior_velocity_traffic_light_module/src/scene.cpp

@@ -165,13 +165,14 @@ TrafficLightModule::TrafficLightModule(
   const int64_t module_id, const int64_t lane_id,
   const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
   const PlannerParam & planner_param, const rclcpp::Logger logger,
-  const rclcpp::Clock::SharedPtr clock)
+  const rclcpp::Clock::SharedPtr clock, std::shared_ptr<DebugDataPublisher> debug_data_publisher)
 : SceneModuleInterface(module_id, logger, clock),
   lane_id_(lane_id),
   traffic_light_reg_elem_(traffic_light_reg_elem),
   lane_(lane),
   state_(State::APPROACH),
-  is_prev_state_stop_(false)
+  is_prev_state_stop_(false),
+  debug_data_publisher_(debug_data_publisher)
 {
   velocity_factor_.init(VelocityFactor::TRAFFIC_SIGNAL);
   planner_param_ = planner_param;
@@ -302,6 +303,23 @@ bool TrafficLightModule::isPassthrough(const double & signed_arc_length) const
 
   const auto & enable_pass_judge = planner_param_.enable_pass_judge;
 
+  RCLCPP_WARN_STREAM(rclcpp::get_logger("debug"), "signed_arc_length: " << signed_arc_length);
+  RCLCPP_WARN_STREAM(rclcpp::get_logger("debug"), "reachable_distance: " << reachable_distance);
+  RCLCPP_WARN_STREAM(
+    rclcpp::get_logger("debug"), "pass_judge_line_distance: " << pass_judge_line_distance);
+
+  {
+    auto get_debug_value = [&](const float value) {
+      tier4_debug_msgs::msg::Float32Stamped debug_value;
+      debug_value.stamp = clock_->now();
+      debug_value.data = value;
+      return debug_value;
+    };
+    debug_data_publisher_->pub_dist_to_stop_line_->publish(get_debug_value(signed_arc_length));
+    debug_data_publisher_->pub_reachable_dist_->publish(get_debug_value(reachable_distance));
+    debug_data_publisher_->pub_pass_judge_dist_->publish(get_debug_value(pass_judge_line_distance));
+  }
+
   if (enable_pass_judge && !stoppable && !is_prev_state_stop_) {
     // Cannot stop under acceleration and jerk limits.
     // However, ego vehicle can't enter the intersection while the light is yellow.

planning/behavior_velocity_traffic_light_module/src/scene.hpp

@@ -68,7 +68,7 @@ class TrafficLightModule : public SceneModuleInterface
     const int64_t module_id, const int64_t lane_id,
     const lanelet::TrafficLight & traffic_light_reg_elem, lanelet::ConstLanelet lane,
     const PlannerParam & planner_param, const rclcpp::Logger logger,
-    const rclcpp::Clock::SharedPtr clock);
+    const rclcpp::Clock::SharedPtr clock, std::shared_ptr<DebugDataPublisher> debug_data_publisher);
 
   bool modifyPathVelocity(PathWithLaneId * path, StopReason * stop_reason) override;
 
@@ -127,6 +127,8 @@ class TrafficLightModule : public SceneModuleInterface
 
   // Traffic Light State
   TrafficSignal looking_tl_state_;
+
+  std::shared_ptr<DebugDataPublisher> debug_data_publisher_;
 };
 }  // namespace behavior_velocity_planner