feat(pid_longitudinal_controller): add maker for stop reason

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
@@ -97,6 +104,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(
@@ -109,7 +117,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

@@ -1,4 +1,4 @@
 // Copyright 2021 Tier IV, Inc. All rights reserved.
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -39,171 +39,174 @@
   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]
 
   // parameters to enable functions
   m_enable_smooth_stop = node.declare_parameter<bool>("enable_smooth_stop");
   m_enable_overshoot_emergency = node.declare_parameter<bool>("enable_overshoot_emergency");
   m_enable_large_tracking_error_emergency =
     node.declare_parameter<bool>("enable_large_tracking_error_emergency");
   m_enable_slope_compensation = node.declare_parameter<bool>("enable_slope_compensation");
   m_enable_keep_stopped_until_steer_convergence =
     node.declare_parameter<bool>("enable_keep_stopped_until_steer_convergence");
 
   // parameters for state transition
   {
     auto & p = m_state_transition_params;
     // drive
     p.drive_state_stop_dist = node.declare_parameter<double>("drive_state_stop_dist");  // [m]
     p.drive_state_offset_stop_dist =
       node.declare_parameter<double>("drive_state_offset_stop_dist");  // [m]
     // stopping
     p.stopping_state_stop_dist = node.declare_parameter<double>("stopping_state_stop_dist");  // [m]
     p.stopped_state_entry_duration_time =
       node.declare_parameter<double>("stopped_state_entry_duration_time");  // [s]
     // stop
     p.stopped_state_entry_vel = node.declare_parameter<double>("stopped_state_entry_vel");  // [m/s]
     p.stopped_state_entry_acc =
       node.declare_parameter<double>("stopped_state_entry_acc");  // [m/s²]
 
     // emergency
     p.emergency_state_overshoot_stop_dist =
       node.declare_parameter<double>("emergency_state_overshoot_stop_dist");  // [m]
     p.emergency_state_traj_trans_dev =
       node.declare_parameter<double>("emergency_state_traj_trans_dev");  // [m]
     p.emergency_state_traj_rot_dev =
       node.declare_parameter<double>("emergency_state_traj_rot_dev");  // [m]
   }
 
   // parameters for drive state
   {
     // initialize PID gain
     const double kp{node.declare_parameter<double>("kp")};
     const double ki{node.declare_parameter<double>("ki")};
     const double kd{node.declare_parameter<double>("kd")};
     m_pid_vel.setGains(kp, ki, kd);
 
     // initialize PID limits
     const double max_pid{node.declare_parameter<double>("max_out")};     // [m/s^2]
     const double min_pid{node.declare_parameter<double>("min_out")};     // [m/s^2]
     const double max_p{node.declare_parameter<double>("max_p_effort")};  // [m/s^2]
     const double min_p{node.declare_parameter<double>("min_p_effort")};  // [m/s^2]
     const double max_i{node.declare_parameter<double>("max_i_effort")};  // [m/s^2]
     const double min_i{node.declare_parameter<double>("min_i_effort")};  // [m/s^2]
     const double max_d{node.declare_parameter<double>("max_d_effort")};  // [m/s^2]
     const double min_d{node.declare_parameter<double>("min_d_effort")};  // [m/s^2]
     m_pid_vel.setLimits(max_pid, min_pid, max_p, min_p, max_i, min_i, max_d, min_d);
 
     // set lowpass filter for vel error and pitch
     const double lpf_vel_error_gain{node.declare_parameter<double>("lpf_vel_error_gain")};
     m_lpf_vel_error = std::make_shared<LowpassFilter1d>(0.0, lpf_vel_error_gain);
 
     m_enable_integration_at_low_speed =
       node.declare_parameter<bool>("enable_integration_at_low_speed");
     m_current_vel_threshold_pid_integrate =
       node.declare_parameter<double>("current_vel_threshold_pid_integration");  // [m/s]
 
     m_time_threshold_before_pid_integrate =
       node.declare_parameter<double>("time_threshold_before_pid_integration");  // [s]
 
     m_enable_brake_keeping_before_stop =
       node.declare_parameter<bool>("enable_brake_keeping_before_stop");         // [-]
     m_brake_keeping_acc = node.declare_parameter<double>("brake_keeping_acc");  // [m/s^2]
   }
 
   // parameters for smooth stop state
   {
     const double max_strong_acc{
       node.declare_parameter<double>("smooth_stop_max_strong_acc")};  // [m/s^2]
     const double min_strong_acc{
       node.declare_parameter<double>("smooth_stop_min_strong_acc")};                // [m/s^2]
     const double weak_acc{node.declare_parameter<double>("smooth_stop_weak_acc")};  // [m/s^2]
     const double weak_stop_acc{
       node.declare_parameter<double>("smooth_stop_weak_stop_acc")};  // [m/s^2]
     const double strong_stop_acc{
       node.declare_parameter<double>("smooth_stop_strong_stop_acc")};  // [m/s^2]
 
     const double max_fast_vel{node.declare_parameter<double>("smooth_stop_max_fast_vel")};  // [m/s]
     const double min_running_vel{
       node.declare_parameter<double>("smooth_stop_min_running_vel")};  // [m/s]
     const double min_running_acc{
       node.declare_parameter<double>("smooth_stop_min_running_acc")};  // [m/s^2]
     const double weak_stop_time{
       node.declare_parameter<double>("smooth_stop_weak_stop_time")};  // [s]
 
     const double weak_stop_dist{
       node.declare_parameter<double>("smooth_stop_weak_stop_dist")};  // [m]
     const double strong_stop_dist{
       node.declare_parameter<double>("smooth_stop_strong_stop_dist")};  // [m]
 
     m_smooth_stop.setParams(
       max_strong_acc, min_strong_acc, weak_acc, weak_stop_acc, strong_stop_acc, max_fast_vel,
       min_running_vel, min_running_acc, weak_stop_time, weak_stop_dist, strong_stop_dist);
   }
 
   // parameters for stop state
   {
     auto & p = m_stopped_state_params;
     p.vel = node.declare_parameter<double>("stopped_vel");    // [m/s]
     p.acc = node.declare_parameter<double>("stopped_acc");    // [m/s^2]
     p.jerk = node.declare_parameter<double>("stopped_jerk");  // [m/s^3]
   }
 
   // parameters for emergency state
   {
     auto & p = m_emergency_state_params;
     p.vel = node.declare_parameter<double>("emergency_vel");    // [m/s]
     p.acc = node.declare_parameter<double>("emergency_acc");    // [m/s^2]
     p.jerk = node.declare_parameter<double>("emergency_jerk");  // [m/s^3]
   }
 
   // parameters for acceleration limit
   m_max_acc = node.declare_parameter<double>("max_acc");  // [m/s^2]
   m_min_acc = node.declare_parameter<double>("min_acc");  // [m/s^2]
 
   // parameters for jerk limit
   m_max_jerk = node.declare_parameter<double>("max_jerk");  // [m/s^3]
   m_min_jerk = node.declare_parameter<double>("min_jerk");  // [m/s^3]
 
   // parameters for slope compensation
   m_adaptive_trajectory_velocity_th =
     node.declare_parameter<double>("adaptive_trajectory_velocity_th");  // [m/s^2]
   const double lpf_pitch_gain{node.declare_parameter<double>("lpf_pitch_gain")};
   m_lpf_pitch = std::make_shared<LowpassFilter1d>(0.0, lpf_pitch_gain);
   m_max_pitch_rad = node.declare_parameter<double>("max_pitch_rad");  // [rad]
   m_min_pitch_rad = node.declare_parameter<double>("min_pitch_rad");  // [rad]
 
   // check slope source is proper
   const std::string slope_source = node.declare_parameter<std::string>(
     "slope_source");  // raw_pitch, trajectory_pitch or trajectory_adaptive
   if (slope_source == "raw_pitch") {
     m_slope_source = SlopeSource::RAW_PITCH;
   } else if (slope_source == "trajectory_pitch") {
     m_slope_source = SlopeSource::TRAJECTORY_PITCH;
   } else if (slope_source == "trajectory_adaptive") {
     m_slope_source = SlopeSource::TRAJECTORY_ADAPTIVE;
   } else {
     RCLCPP_ERROR(logger_, "Slope source is not valid. Using raw_pitch option as default");
     m_slope_source = SlopeSource::RAW_PITCH;
   }
 
   // ego nearest index search
   m_ego_nearest_dist_threshold =
     node.has_parameter("ego_nearest_dist_threshold")
       ? node.get_parameter("ego_nearest_dist_threshold").as_double()
       : node.declare_parameter<double>("ego_nearest_dist_threshold");  // [m]
   m_ego_nearest_yaw_threshold =
     node.has_parameter("ego_nearest_yaw_threshold")
       ? node.get_parameter("ego_nearest_yaw_threshold").as_double()
       : node.declare_parameter<double>("ego_nearest_yaw_threshold");  // [rad]
 
   // subscriber, publisher
   m_pub_slope = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
     "~/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(
@@ -597,102 +600,110 @@
   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;
 
   const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel &&
                           std::abs(current_acc) < p.stopped_state_entry_acc;
   // Case where the ego slips in the opposite direction of the gear due to e.g. a slope is also
   // considered as a stop
   const bool is_not_running = [&]() {
     if (control_data.shift == Shift::Forward) {
       if (is_stopped || current_vel < 0.0) {
         // NOTE: Stopped or moving backward
         return true;
       }
     } else {
       if (is_stopped || 0.0 < current_vel) {
         // NOTE: Stopped or moving forward
         return true;
       }
     }
     return false;
   }();
   if (!is_not_running) {
     m_last_running_time = std::make_shared<rclcpp::Time>(clock_->now());
   }
   const bool stopped_condition =
     m_last_running_time
       ? (clock_->now() - *m_last_running_time).seconds() > p.stopped_state_entry_duration_time
       : false;
 
   // ==========================================================================================
   // NOTE: due to removeOverlapPoints() in getControlData() m_trajectory and
   // control_data.interpolated_traj have different size.
   // ==========================================================================================
   const double current_vel_cmd = std::fabs(
     control_data.interpolated_traj.points.at(control_data.nearest_idx).longitudinal_velocity_mps);
   const bool emergency_condition = m_enable_overshoot_emergency &&
                                    stop_dist < -p.emergency_state_overshoot_stop_dist &&
                                    current_vel_cmd < vel_epsilon;
   const bool has_nonzero_target_vel = std::abs(current_vel_cmd) > 1.0e-5;
 
   const auto changeState = [this](const auto s) {
     if (s != m_control_state) {
       RCLCPP_DEBUG_STREAM(
         logger_, "controller state changed: " << toStr(m_control_state) << " -> " << toStr(s));
     }
     m_control_state = s;
     return;
   };
 
   const auto debug_msg_once = [this](const auto & s) { RCLCPP_DEBUG_ONCE(logger_, "%s", s); };
 
   const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
                                 m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
 
   if (is_under_control != m_prev_vehicle_is_under_control) {
     m_prev_vehicle_is_under_control = is_under_control;
     m_under_control_starting_time =
       is_under_control ? std::make_shared<rclcpp::Time>(clock_->now()) : nullptr;
   }
   // transit state
   // in DRIVE state
   if (m_control_state == ControlState::DRIVE) {
     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.
       return changeState(ControlState::STOPPED);
     }
 
     if (m_enable_smooth_stop) {
       if (stopping_condition) {
         // predictions after input time delay
         const double pred_vel_in_target = control_data.state_after_delay.vel;
         const double pred_stop_dist =
           control_data.stop_dist -
           0.5 * (pred_vel_in_target + current_vel) * m_delay_compensation_time;
         m_smooth_stop.init(pred_vel_in_target, pred_stop_dist);
         return changeState(ControlState::STOPPING);
       }
     } else {
       if (stopped_condition && !departure_condition_from_stopped) {
         return changeState(ControlState::STOPPED);
       }
     }
     return;
   }
 
   // in STOPPING state
   if (m_control_state == ControlState::STOPPING) {
     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 @@
             ("~/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=[