feat(trajectory_follower): pub steer converged marker

Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -80,6 +80,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr m_pub_predicted_traj;
   //!< @brief topic publisher for control debug values
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug_values;
+  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr m_pub_debug_marker;
   //!< @brief subscription for transform messages
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_sub;
   rclcpp::Subscription<tf2_msgs::msg::TFMessage>::SharedPtr m_tf_static_sub;
@@ -133,6 +134,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   double m_ego_nearest_dist_threshold;
   double m_ego_nearest_yaw_threshold;
 
+  // debug marker
+  visualization_msgs::msg::MarkerArray m_debug_marker_array;
+
   //!< initialize timer to work in real, simulation, and replay
   void initTimer(double period_s);
   /**
@@ -187,7 +191,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   /**
    * @brief check ego car is in stopped state
    */
-  bool isStoppedState() const;
+  bool isStoppedState();
 
   /**
    * @brief check if the trajectory has valid value
@@ -196,7 +200,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
 
   bool isTrajectoryShapeChanged() const;
 
-  bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const;
+  bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd);
 
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
 

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -154,6 +154,8 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
     "~/output/predicted_trajectory", 1);
   m_pub_debug_values = node_->create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
     "~/output/lateral_diagnostic", 1);
+  m_pub_debug_marker =
+    node_->create_publisher<visualization_msgs::msg::MarkerArray>("~/output/debug_marker", 1);
 
   // TODO(Frederik.Beaujean) ctor is too long, should factor out parameter declarations
   declareMPCparameters();
@@ -180,6 +182,7 @@ boost::optional<LateralOutput> MpcLateralController::run()
   autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd;
   autoware_auto_planning_msgs::msg::Trajectory predicted_traj;
   tier4_debug_msgs::msg::Float32MultiArrayStamped debug_values;
+  m_debug_marker_array.markers.clear();
 
   if (!m_is_ctrl_cmd_prev_initialized) {
     m_ctrl_cmd_prev = getInitialControlCommand();
@@ -207,7 +210,9 @@ boost::optional<LateralOutput> MpcLateralController::run()
     }
     // Use previous command value as previous raw steer command
     m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
-    return createLateralOutput(m_ctrl_cmd_prev);
+    const auto output = createLateralOutput(m_ctrl_cmd_prev);
+    m_pub_debug_marker->publish(m_debug_marker_array);
+    return output;
   }
 
   if (!is_mpc_solved) {
@@ -218,7 +223,9 @@ boost::optional<LateralOutput> MpcLateralController::run()
   }
 
   m_ctrl_cmd_prev = ctrl_cmd;
-  return createLateralOutput(ctrl_cmd);
+  const auto output = createLateralOutput(ctrl_cmd);
+  m_pub_debug_marker->publish(m_debug_marker_array);
+  return output;
 }
 
 void MpcLateralController::setInputData(InputData const & input_data)
@@ -229,17 +236,30 @@ void MpcLateralController::setInputData(InputData const & input_data)
 }
 
 bool MpcLateralController::isSteerConverged(
-  const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const
+  const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd)
 {
+  bool is_converged = false;
+  std::stringstream ss;
   // 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()) {
-    return false;
+    ss << "is trajectory shape changed (not converged)";
+  } else {
+    is_converged = std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
+                   static_cast<float>(m_converged_steer_rad);
+    ss << "cmd: " << cmd.steering_tire_angle
+       << " current: " << m_current_steering_ptr->steering_tire_angle
+       << (is_converged ? " (converged)" : " (not converged)");
   }
 
-  const bool is_converged =
-    std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
-    static_cast<float>(m_converged_steer_rad);
+  // create debug marker for converged
+  auto marker = tier4_autoware_utils::createDefaultMarker(
+    "map", node_->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 = m_current_kinematic_state_ptr->pose.pose;
+  marker.text = ss.str();
+  m_debug_marker_array.markers.push_back(marker);
 
   return is_converged;
 }
@@ -338,7 +358,7 @@ MpcLateralController::getInitialControlCommand() const
   return cmd;
 }
 
-bool MpcLateralController::isStoppedState() const
+bool MpcLateralController::isStoppedState()
 {
   // If the nearest index is not found, return false
   if (m_current_trajectory_ptr->points.empty()) {