feat(trajectory_follower): pub steer converged marker (autowarefounda…

…tion#2441) * feat(trajectory_follower): pub steer converged marker Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * Revert "feat(trajectory_follower): pub steer converged marker" This reverts commit a6f6917. Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> * add steer converged debug marker in contoller_node Signed-off-by: kosuke55 <kosuke.tnp@gmail.com> Signed-off-by: kosuke55 <kosuke.tnp@gmail.com>
HansRobo · Dec 12, 2022 · 4a6990c · 4a6990c
1 parent 3c01f15
commit 4a6990c
Show file tree

Hide file tree

Showing 3 changed files with 33 additions and 0 deletions.
diff --git 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
@@ -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
@@ -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