feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature (#9675)

* feat(pid_longitudinal_controller): remove trans/rot deviation validation since the control_validator has the same feature

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

* fix test

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

---------

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>

Author: takayuki5168

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

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<diagnostic_updater::Updater>
     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);
 

control/autoware_pid_longitudinal_controller/media/LongitudinalControllerStateTransition.drawio.svg

@@ -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",

control/autoware_pid_longitudinal_controller/schema/autoware_pid_longitudinal_controller.schema.json

@@ -79,10 +79,6 @@ PidLongitudinalController::PidLongitudinalController(
     // 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
@@ -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<int32_t>("control_state", static_cast<int32_t>(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);
 }
 

control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.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();