From 7c5cf1c702926ba3f47f9084f913e6038b375d82 Mon Sep 17 00:00:00 2001
From: kosuke55 <kosuke.tnp@gmail.com>
Date: Thu, 8 Dec 2022 21:12:03 +0900
Subject: [PATCH] add steer converged debug marker in contoller_node

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
---
 .../src/mpc_lateral_controller.cpp            |  1 +
 .../controller_node.hpp                       |  2 ++
 .../src/controller_node.cpp                   | 30 +++++++++++++++++++
 3 files changed, 33 insertions(+)

diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp
index a90fa0fd61da6..aaefa696d6494 100644
--- a/control/trajectory_follower/src/mpc_lateral_controller.cpp
+++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp
@@ -234,6 +234,7 @@ bool MpcLateralController::isSteerConverged(
   // wait for a while to propagate the trajectory shape to the output command when the trajectory
   // shape is changed.
   if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) {
+    RCLCPP_DEBUG(node_->get_logger(), "trajectory shaped is changed");
     return false;
   }
 
diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp
index 346fe9a92d56a..f8493c7a1cb28 100644
--- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp
+++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp
@@ -83,6 +83,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::Subscription<geometry_msgs::msg::AccelWithCovarianceStamped>::SharedPtr sub_accel_;
   rclcpp::Publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>::SharedPtr
     control_cmd_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
 
   enum class LateralControllerMode {
     INVALID = 0,
@@ -106,6 +107,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
   LongitudinalControllerMode getLongitudinalControllerMode(
     const std::string & algorithm_name) const;
+  void publishDebugMarker() const;
 };
 }  // namespace trajectory_follower_nodes
 }  // namespace control
diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp
index 0bd1a694f1bd7..8b168afc08416 100644
--- a/control/trajectory_follower_nodes/src/controller_node.cpp
+++ b/control/trajectory_follower_nodes/src/controller_node.cpp
@@ -77,6 +77,8 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
     "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1));
   control_cmd_pub_ = create_publisher<autoware_auto_control_msgs::msg::AckermannControlCommand>(
     "~/output/control_cmd", rclcpp::QoS{1}.transient_local());
+  debug_marker_pub_ =
+    create_publisher<visualization_msgs::msg::MarkerArray>("~/output/debug_marker", rclcpp::QoS{1});
 
   // Timer
   {
@@ -168,6 +170,34 @@ void Controller::callbackTimerControl()
   out.lateral = lateral_output_->control_cmd;
   out.longitudinal = longitudinal_output_->control_cmd;
   control_cmd_pub_->publish(out);
+
+  publishDebugMarker();
+}
+
+void Controller::publishDebugMarker() const
+{
+  visualization_msgs::msg::MarkerArray debug_marker_array{};
+
+  // steer converged marker
+  {
+    auto marker = tier4_autoware_utils::createDefaultMarker(
+      "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0),
+      tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99));
+    marker.pose = input_data_.current_odometry_ptr->pose.pose;
+
+    std::stringstream ss;
+    const double current = input_data_.current_steering_ptr->steering_tire_angle;
+    const double cmd = lateral_output_->control_cmd.steering_tire_angle;
+    const double diff = current - cmd;
+    ss << "current:" << current << " cmd:" << cmd << " diff:" << diff
+       << (lateral_output_->sync_data.is_steer_converged ? " (converged)" : " (not converged)");
+    marker.text = ss.str();
+
+    debug_marker_array.markers.push_back(marker);
+  }
+
+  debug_marker_pub_->publish(debug_marker_array);
 }
 
 }  // namespace trajectory_follower_nodes