diff --git a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 8df7cb3b9b3f8..967b7c40fbd10 100644 --- a/control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/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 #include @@ -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::SharedPtr m_pub_slope; rclcpp::Publisher::SharedPtr m_pub_debug; + rclcpp::Publisher::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 m_under_control_starting_time{nullptr}; diff --git a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index aab2ecf8f081e..cc1c0c6383707 100644 --- a/control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/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("delay_compensation_time"); // [s] @@ -204,6 +206,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) "~/output/slope_angle", rclcpp::QoS{1}); m_pub_debug = node.create_publisher( "~/output/longitudinal_diagnostic", rclcpp::QoS{1}); + m_pub_stop_reason_marker = node.create_publisher("~/output/stop_reason", rclcpp::QoS{1}); // set parameter callback m_set_param_res = node.add_on_set_parameters_callback( @@ -597,6 +600,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; @@ -662,7 +675,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. @@ -692,7 +704,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d if (emergency_condition) { return changeState(ControlState::EMERGENCY); } - if (stopped_condition) { return changeState(ControlState::STOPPED); } diff --git a/launch/tier4_control_launch/launch/control.launch.py b/launch/tier4_control_launch/launch/control.launch.py index b49ff7bde2ed3..222ab33894afc 100644 --- a/launch/tier4_control_launch/launch/control.launch.py +++ b/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=[