Skip to content

Commit b6cd8f4

Browse files
authored
feat(pid_longitudinal_controller): add maker for stop reason (#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>
1 parent 3ba1ecd commit b6cd8f4

File tree

3 files changed

+25
-3
lines changed

3 files changed

+25
-3
lines changed

control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "tf2/utils.h"
2626
#include "tf2_ros/buffer.h"
2727
#include "tf2_ros/transform_listener.h"
28+
#include "tier4_autoware_utils/ros/marker_helper.hpp"
2829
#include "trajectory_follower_base/longitudinal_controller_base.hpp"
2930
#include "vehicle_info_util/vehicle_info_util.hpp"
3031

@@ -39,6 +40,7 @@
3940
#include "nav_msgs/msg/odometry.hpp"
4041
#include "tf2_msgs/msg/tf_message.hpp"
4142
#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
43+
#include "visualization_msgs/msg/marker.hpp"
4244

4345
#include <deque>
4446
#include <memory>
@@ -50,6 +52,11 @@
5052
namespace autoware::motion::control::pid_longitudinal_controller
5153
{
5254
using autoware_adapi_v1_msgs::msg::OperationModeState;
55+
using tier4_autoware_utils::createDefaultMarker;
56+
using tier4_autoware_utils::createMarkerColor;
57+
using tier4_autoware_utils::createMarkerScale;
58+
using visualization_msgs::msg::Marker;
59+
5360
namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
5461

5562
/// \class PidLongitudinalController
@@ -97,6 +104,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
97104
// ros variables
98105
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_slope;
99106
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug;
107+
rclcpp::Publisher<Marker>::SharedPtr m_pub_stop_reason_marker;
100108

101109
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
102110
rcl_interfaces::msg::SetParametersResult paramCallback(
@@ -109,7 +117,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
109117
OperationModeState m_current_operation_mode;
110118

111119
// vehicle info
112-
double m_wheel_base;
120+
double m_wheel_base{0.0};
121+
double m_vehicle_width{0.0};
122+
double m_front_overhang{0.0};
113123
bool m_prev_vehicle_is_under_control{false};
114124
std::shared_ptr<rclcpp::Time> m_under_control_starting_time{nullptr};
115125

control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

+13-2
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
3939
m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double();
4040

4141
m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m;
42+
m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m;
43+
m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m;
4244

4345
// parameters for delay compensation
4446
m_delay_compensation_time = node.declare_parameter<double>("delay_compensation_time"); // [s]
@@ -204,6 +206,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
204206
"~/output/slope_angle", rclcpp::QoS{1});
205207
m_pub_debug = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
206208
"~/output/longitudinal_diagnostic", rclcpp::QoS{1});
209+
m_pub_stop_reason_marker = node.create_publisher<Marker>("~/output/stop_reason", rclcpp::QoS{1});
207210

208211
// set parameter callback
209212
m_set_param_res = node.add_on_set_parameters_callback(
@@ -597,6 +600,16 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
597600
const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
598601
m_enable_keep_stopped_until_steer_convergence &&
599602
!lateral_sync_data_.is_steer_converged;
603+
if (keep_stopped_condition) {
604+
auto marker = createDefaultMarker(
605+
"map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
606+
createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
607+
marker.pose = tier4_autoware_utils::calcOffsetPose(
608+
m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
609+
m_vehicle_width / 2 + 2.0, 1.5);
610+
marker.text = "steering not\nconverged";
611+
m_pub_stop_reason_marker->publish(marker);
612+
}
600613

601614
const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
602615

@@ -662,7 +675,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
662675
if (emergency_condition) {
663676
return changeState(ControlState::EMERGENCY);
664677
}
665-
666678
if (!is_under_control && stopped_condition && keep_stopped_condition) {
667679
// NOTE: When the ego is stopped on manual driving, since the driving state may transit to
668680
// autonomous, keep_stopped_condition should be checked.
@@ -692,7 +704,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
692704
if (emergency_condition) {
693705
return changeState(ControlState::EMERGENCY);
694706
}
695-
696707
if (stopped_condition) {
697708
return changeState(ControlState::STOPPED);
698709
}

launch/tier4_control_launch/launch/control.launch.py

+1
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,7 @@ def launch_setup(context, *args, **kwargs):
8181
("~/output/lateral_diagnostic", "lateral/diagnostic"),
8282
("~/output/slope_angle", "longitudinal/slope_angle"),
8383
("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"),
84+
("~/output/stop_reason", "longitudinal/stop_reason"),
8485
("~/output/control_cmd", "control_cmd"),
8586
],
8687
parameters=[

0 commit comments

Comments
 (0)