diff --git a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml index 6d9b93ab98870..bd905cf9be0f1 100644 --- a/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml +++ b/control/autoware_pid_longitudinal_controller/config/autoware_pid_longitudinal_controller.param.yaml @@ -16,8 +16,6 @@ stopped_state_entry_vel: 0.01 stopped_state_entry_acc: 0.1 emergency_state_overshoot_stop_dist: 1.5 - emergency_state_traj_trans_dev: 3.0 - emergency_state_traj_rot_dev: 0.7854 # drive state kp: 1.0 diff --git a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp index 8c78b2b871fda..1fe78852047c6 100644 --- a/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp +++ b/control/autoware_pid_longitudinal_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp @@ -87,7 +87,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro struct ControlData { - bool is_far_from_trajectory{false}; autoware_planning_msgs::msg::Trajectory interpolated_traj{}; size_t nearest_idx{0}; // nearest_idx = 0 when nearest_idx is not found with findNearestIdx size_t target_idx{0}; @@ -162,8 +161,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro double stopped_state_entry_acc; // emergency double emergency_state_overshoot_stop_dist; - double emergency_state_traj_trans_dev; - double emergency_state_traj_rot_dev; }; StateTransitionParams m_state_transition_params; @@ -246,12 +243,6 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro // Diagnostic std::shared_ptr diag_updater_{}; // Diagnostic updater for publishing diagnostic data. - struct DiagnosticData - { - double trans_deviation{0.0}; // translation deviation between nearest point and current_pose - double rot_deviation{0.0}; // rotation deviation between nearest point and current_pose - }; - DiagnosticData m_diagnostic_data; void setupDiagnosticUpdater(); void checkControlState(diagnostic_updater::DiagnosticStatusWrapper & stat); diff --git a/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg index 88ac881f4f43c..f669b647dbf84 100644 --- a/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg +++ b/control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg @@ -175,7 +175,7 @@ : For 500 [ms], self velocity is smaller than C1, and self acceleration is smaller than C2.
emergency condition - : Speed command is zero velocity, and distance to stop is smaller than D1, or self pose is not within D2, D3 from trajectory. + : Speed command is zero velocity, and distance to stop is smaller than D1.

@@ -315,10 +315,6 @@ C2 : stopped_state_entry_acc
D1 : emergency_state_overshoot_stop_dist -
- D2 : emergency_state_traj_trans_dev -
- D3 : emergency_state_traj_rot_dev

diff --git a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json index ef1272582e52b..14a34f04458eb 100644 --- a/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json +++ b/control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json @@ -71,16 +71,6 @@ "description": "If `enable_overshoot_emergency` is true and the ego is `emergency_state_overshoot_stop_dist`-meter ahead of the stop point, the state will transit to EMERGENCY. [m]", "default": "1.5" }, - "emergency_state_traj_trans_dev": { - "type": "number", - "description": "If the ego's position is `emergency_state_traj_trans_dev` meter away from the nearest trajectory point, the state will transit to EMERGENCY. [m]", - "default": "3.0" - }, - "emergency_state_traj_rot_dev": { - "type": "number", - "description": "If the ego's orientation is `emergency_state_traj_rot_dev` rad away from the nearest trajectory point orientation, the state will transit to EMERGENCY. [rad]", - "default": "0.7854" - }, "kp": { "type": "number", "description": "p gain for longitudinal control", @@ -326,8 +316,6 @@ "stopped_state_entry_vel", "stopped_state_entry_acc", "emergency_state_overshoot_stop_dist", - "emergency_state_traj_trans_dev", - "emergency_state_traj_rot_dev", "kp", "ki", "kd", diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp index 4338b73ccef19..54a5702f333a0 100644 --- a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp +++ b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp @@ -79,10 +79,6 @@ PidLongitudinalController::PidLongitudinalController( // emergency p.emergency_state_overshoot_stop_dist = node.declare_parameter("emergency_state_overshoot_stop_dist"); // [m] - p.emergency_state_traj_trans_dev = - node.declare_parameter("emergency_state_traj_trans_dev"); // [m] - p.emergency_state_traj_rot_dev = - node.declare_parameter("emergency_state_traj_rot_dev"); // [m] } // parameters for drive state @@ -286,8 +282,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac update_param("stopped_state_entry_vel", p.stopped_state_entry_vel); update_param("stopped_state_entry_acc", p.stopped_state_entry_acc); update_param("emergency_state_overshoot_stop_dist", p.emergency_state_overshoot_stop_dist); - update_param("emergency_state_traj_trans_dev", p.emergency_state_traj_trans_dev); - update_param("emergency_state_traj_rot_dev", p.emergency_state_traj_rot_dev); } // drive state @@ -423,24 +417,6 @@ trajectory_follower::LongitudinalOutput PidLongitudinalController::run( const auto control_data = getControlData(current_pose); - // self pose is far from trajectory - if (control_data.is_far_from_trajectory) { - if (m_enable_large_tracking_error_emergency) { - // update control state - changeControlState(ControlState::EMERGENCY, "the tracking error is too large"); - } - const Motion raw_ctrl_cmd = calcEmergencyCtrlCmd(control_data.dt); // calculate control command - m_prev_raw_ctrl_cmd = raw_ctrl_cmd; - const auto cmd_msg = - createCtrlCmdMsg(raw_ctrl_cmd, control_data.current_motion.vel); // create control command - publishDebugData(raw_ctrl_cmd, control_data); // publish debug data - trajectory_follower::LongitudinalOutput output; - output.control_cmd = cmd_msg; - output.control_cmd_horizon.controls.push_back(cmd_msg); - output.control_cmd_horizon.time_step_ms = 0.0; - return output; - } - // update control state updateControlState(control_data); @@ -491,23 +467,6 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData const auto nearest_point = current_interpolated_pose.first; auto target_point = current_interpolated_pose.first; - // check if the deviation is worth emergency - m_diagnostic_data.trans_deviation = - autoware::universe_utils::calcDistance2d(current_interpolated_pose.first, current_pose); - const bool is_dist_deviation_large = - m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation; - m_diagnostic_data.rot_deviation = std::abs(autoware::universe_utils::normalizeRadian( - tf2::getYaw(current_interpolated_pose.first.pose.orientation) - - tf2::getYaw(current_pose.orientation))); - const bool is_yaw_deviation_large = - m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation; - - if (is_dist_deviation_large || is_yaw_deviation_large) { - // return here if nearest index is not found - control_data.is_far_from_trajectory = true; - return control_data; - } - // Delay compensation - Calculate the distance we got, predicted velocity and predicted // acceleration after delay control_data.state_after_delay = @@ -1250,23 +1209,7 @@ void PidLongitudinalController::checkControlState( msg = "emergency occurred due to "; } - if ( - m_state_transition_params.emergency_state_traj_trans_dev < m_diagnostic_data.trans_deviation) { - msg += "translation deviation"; - } - - if (m_state_transition_params.emergency_state_traj_rot_dev < m_diagnostic_data.rot_deviation) { - msg += "rotation deviation"; - } - stat.add("control_state", static_cast(m_control_state)); - stat.addf( - "translation deviation threshold", "%lf", - m_state_transition_params.emergency_state_traj_trans_dev); - stat.addf("translation deviation", "%lf", m_diagnostic_data.trans_deviation); - stat.addf( - "rotation deviation threshold", "%lf", m_state_transition_params.emergency_state_traj_rot_dev); - stat.addf("rotation deviation", "%lf", m_diagnostic_data.rot_deviation); stat.summary(level, msg); } diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp index 0b1dac644a8ab..982af104538b5 100644 --- a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp +++ b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp @@ -510,34 +510,6 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) EXPECT_GT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } -TEST_F(FakeNodeFixture, longitudinal_emergency) -{ - const auto node_options = makeNodeOptions(); - ControllerTester tester(this, node_options); - - tester.send_default_transform(); - tester.publish_default_odom(); - tester.publish_autonomous_operation_mode(); - tester.publish_default_steer(); - tester.publish_default_acc(); - - // Publish trajectory starting away from the current ego pose - Trajectory traj; - traj.header.stamp = tester.node->now(); - traj.header.frame_id = "map"; - traj.points.push_back(make_traj_point(10.0, 0.0, 1.0f)); - traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); - traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); - tester.traj_pub->publish(traj); - - test_utils::waitForMessage(tester.node, this, tester.received_control_command); - - ASSERT_TRUE(tester.received_control_command); - // Emergencies (e.g., far from trajectory) produces braking command (0 vel, negative accel) - EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f); - EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); -} - TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) { const auto node_options = makeNodeOptions();