refactor(vehicle_cmd_gate): move isSteerConverged debug marker from t…

…rajectory_follower

Signed-off-by: Berkay Karaman <[email protected]>

control/vehicle_cmd_gate/src/marker_helper.hpp

@@ -106,6 +106,30 @@ inline visualization_msgs::msg::Marker createStringMarker(
   return marker;
 }
 
+inline visualization_msgs::msg::Marker createDefaultMarker(
+  const std::string & frame_id, const rclcpp::Time & now, const std::string & ns, const int32_t id,
+  const int32_t type, const geometry_msgs::msg::Vector3 & scale,
+  const std_msgs::msg::ColorRGBA & color)
+{
+  visualization_msgs::msg::Marker marker;
+
+  marker.header.frame_id = frame_id;
+  marker.header.stamp = now;
+  marker.ns = ns;
+  marker.id = id;
+  marker.type = type;
+  marker.action = visualization_msgs::msg::Marker::ADD;
+  marker.lifetime = rclcpp::Duration::from_seconds(0.5);
+
+  marker.pose.position = createMarkerPosition(0.0, 0.0, 0.0);
+  marker.pose.orientation = createMarkerOrientation(0.0, 0.0, 0.0, 1.0);
+  marker.scale = scale;
+  marker.color = color;
+  marker.frame_locked = true;
+
+  return marker;
+}
+
 inline void appendMarkerArray(
   const visualization_msgs::msg::MarkerArray & additional_marker_array,
   visualization_msgs::msg::MarkerArray * marker_array)

control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp

@@ -82,6 +82,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options)
   filter_activated_flag_pub_ =
     create_publisher<BoolStamped>("~/is_filter_activated/flag", durable_qos);
 
+  debug_marker_pub_ = create_publisher<visualization_msgs::msg::MarkerArray>(
+    "~/is_steering_converged/debug_marker", rclcpp::QoS{1});
+
   // Subscriber
   external_emergency_stop_heartbeat_sub_ = create_subscription<Heartbeat>(
     "input/external_emergency_stop_heartbeat", 1,
@@ -844,8 +847,36 @@ void VehicleCmdGate::publishMarkers(const IsFilterActivated & filter_activated)
 }
 bool VehicleCmdGate::isSteeringConverged(AckermannLateralCommand & lateral_cmd)
 {
-  return std::fabs(lateral_cmd.steering_tire_angle - current_steer_) <
-         steering_convergence_threshold_;
+  const bool is_converged =
+    std::fabs(lateral_cmd.steering_tire_angle - current_steer_) < steering_convergence_threshold_;
+  publishConvergenceDebugMarker(lateral_cmd, is_converged);
+  return is_converged;
+}
+
+void VehicleCmdGate::publishConvergenceDebugMarker(
+  const AckermannLateralCommand & lat_cmd, const bool is_converged) const
+{
+  visualization_msgs::msg::MarkerArray debug_marker_array{};
+
+  // steer converged marker
+  {
+    auto marker = createDefaultMarker(
+      "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.99));
+    marker.pose = current_kinematics_.pose.pose;
+
+    std::stringstream ss;
+    const double current = current_steer_;
+    const double cmd = lat_cmd.steering_tire_angle;
+    const double diff = current - cmd;
+    ss << "current:" << current << " cmd:" << cmd << " diff:" << diff
+       << (is_converged ? " (converged)" : " (not converged)");
+    marker.text = ss.str();
+
+    debug_marker_array.markers.push_back(marker);
+  }
+
+  debug_marker_pub_->publish(debug_marker_array);
 }
 
 }  // namespace vehicle_cmd_gate

control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp

@@ -111,6 +111,7 @@ class VehicleCmdGate : public rclcpp::Node
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_pub_;
   rclcpp::Publisher<MarkerArray>::SharedPtr filter_activated_marker_raw_pub_;
   rclcpp::Publisher<BoolStamped>::SharedPtr filter_activated_flag_pub_;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr debug_marker_pub_;
 
   // Subscription
   rclcpp::Subscription<Heartbeat>::SharedPtr external_emergency_stop_heartbeat_sub_;
@@ -249,7 +250,8 @@ class VehicleCmdGate : public rclcpp::Node
   // debug
   MarkerArray createMarkerArray(const IsFilterActivated & filter_activated);
   void publishMarkers(const IsFilterActivated & filter_activated);
-
+  void publishConvergenceDebugMarker(
+    const AckermannLateralCommand & lat_cmd, const bool is_converged) const;
   std::unique_ptr<tier4_autoware_utils::LoggerLevelConfigure> logger_configure_;
 };