@@ -39,6 +39,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
39
39
m_longitudinal_ctrl_period = node.get_parameter (" ctrl_period" ).as_double ();
40
40
41
41
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 ;
42
44
43
45
// parameters for delay compensation
44
46
m_delay_compensation_time = node.declare_parameter <double >(" delay_compensation_time" ); // [s]
@@ -204,6 +206,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
204
206
" ~/output/slope_angle" , rclcpp::QoS{1 });
205
207
m_pub_debug = node.create_publisher <tier4_debug_msgs::msg::Float32MultiArrayStamped>(
206
208
" ~/output/longitudinal_diagnostic" , rclcpp::QoS{1 });
209
+ m_pub_stop_reason_marker = node.create_publisher <Marker>(" ~/output/stop_reason" , rclcpp::QoS{1 });
207
210
208
211
// set parameter callback
209
212
m_set_param_res = node.add_on_set_parameters_callback (
@@ -597,6 +600,16 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
597
600
const bool keep_stopped_condition = std::fabs (current_vel) < vel_epsilon &&
598
601
m_enable_keep_stopped_until_steer_convergence &&
599
602
!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\n converged" ;
611
+ m_pub_stop_reason_marker->publish (marker);
612
+ }
600
613
601
614
const bool stopping_condition = stop_dist < p.stopping_state_stop_dist ;
602
615
@@ -662,7 +675,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
662
675
if (emergency_condition) {
663
676
return changeState (ControlState::EMERGENCY);
664
677
}
665
-
666
678
if (!is_under_control && stopped_condition && keep_stopped_condition) {
667
679
// NOTE: When the ego is stopped on manual driving, since the driving state may transit to
668
680
// autonomous, keep_stopped_condition should be checked.
@@ -692,7 +704,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
692
704
if (emergency_condition) {
693
705
return changeState (ControlState::EMERGENCY);
694
706
}
695
-
696
707
if (stopped_condition) {
697
708
return changeState (ControlState::STOPPED);
698
709
}
0 commit comments