feat(pid_longitudinal_controller): add maker for stop reason (autowarefoundation#6579)

* feat(pid_longitudinal_controller): add maker for stop reason

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* minor fix

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

@@ -25,6 +25,7 @@
 #include "tf2/utils.h"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
+#include "tier4_autoware_utils/ros/marker_helper.hpp"
 #include "trajectory_follower_base/longitudinal_controller_base.hpp"
 #include "vehicle_info_util/vehicle_info_util.hpp"
 
@@ -39,6 +40,7 @@
 #include "nav_msgs/msg/odometry.hpp"
 #include "tf2_msgs/msg/tf_message.hpp"
 #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
+#include "visualization_msgs/msg/marker.hpp"
 
 #include <deque>
 #include <memory>
@@ -50,6 +52,11 @@
 namespace autoware::motion::control::pid_longitudinal_controller
 {
 using autoware_adapi_v1_msgs::msg::OperationModeState;
+using tier4_autoware_utils::createDefaultMarker;
+using tier4_autoware_utils::createMarkerColor;
+using tier4_autoware_utils::createMarkerScale;
+using visualization_msgs::msg::Marker;
+
 namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
 
 /// \class PidLongitudinalController
@@ -85,6 +92,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   // ros variables
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_slope;
   rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug;
+  rclcpp::Publisher<Marker>::SharedPtr m_pub_stop_reason_marker;
 
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
   rcl_interfaces::msg::SetParametersResult paramCallback(
@@ -97,7 +105,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   OperationModeState m_current_operation_mode;
 
   // vehicle info
-  double m_wheel_base;
+  double m_wheel_base{0.0};
+  double m_vehicle_width{0.0};
+  double m_front_overhang{0.0};
   bool m_prev_vehicle_is_under_control{false};
   std::shared_ptr<rclcpp::Time> m_under_control_starting_time{nullptr};
 

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

@@ -39,6 +39,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
   m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double();
 
   m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m;
+  m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m;
+  m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m;
 
   // parameters for delay compensation
   m_delay_compensation_time = node.declare_parameter<double>("delay_compensation_time");  // [s]
@@ -189,6 +191,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
     "~/output/slope_angle", rclcpp::QoS{1});
   m_pub_debug = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
     "~/output/longitudinal_diagnostic", rclcpp::QoS{1});
+  m_pub_stop_reason_marker = node.create_publisher<Marker>("~/output/stop_reason", rclcpp::QoS{1});
 
   // set parameter callback
   m_set_param_res = node.add_on_set_parameters_callback(
@@ -510,6 +513,16 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
   const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
                                       m_enable_keep_stopped_until_steer_convergence &&
                                       !lateral_sync_data_.is_steer_converged;
+  if (keep_stopped_condition) {
+    auto marker = createDefaultMarker(
+      "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
+      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
+    marker.pose = tier4_autoware_utils::calcOffsetPose(
+      m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
+      m_vehicle_width / 2 + 2.0, 1.5);
+    marker.text = "steering not\nconverged";
+    m_pub_stop_reason_marker->publish(marker);
+  }
 
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
 
@@ -571,7 +584,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     if (emergency_condition) {
       return changeState(ControlState::EMERGENCY);
     }
-
     if (!is_under_control && stopped_condition && keep_stopped_condition) {
       // NOTE: When the ego is stopped on manual driving, since the driving state may transit to
       //       autonomous, keep_stopped_condition should be checked.
@@ -602,7 +614,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     if (emergency_condition) {
       return changeState(ControlState::EMERGENCY);
     }
-
     if (stopped_condition) {
       return changeState(ControlState::STOPPED);
     }

launch/tier4_control_launch/launch/control.launch.py

@@ -81,6 +81,7 @@ def launch_setup(context, *args, **kwargs):
             ("~/output/lateral_diagnostic", "lateral/diagnostic"),
             ("~/output/slope_angle", "longitudinal/slope_angle"),
             ("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"),
+            ("~/output/stop_reason", "longitudinal/stop_reason"),
             ("~/output/control_cmd", "control_cmd"),
         ],
         parameters=[