add minor fix and modify unit test for dry steering

Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com>
autowarefoundation · Jul 9, 2024 · d1cdedf · d1cdedf
1 parent bbd6042
commit d1cdedf
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 9 deletions.
diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -718,17 +718,16 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
     if (departure_condition_from_stopped) {
       // Let vehicle start after the steering is converged for dry steering
-      const bool current_keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
-                                                  m_enable_keep_stopped_until_steer_convergence &&
-                                                  !lateral_sync_data_.is_steer_converged;
+      const bool current_keep_stopped_condition =
+        std::fabs(current_vel) < vel_epsilon && !lateral_sync_data_.is_steer_converged;
       // NOTE: Dry steering is considered unnecessary when the steering is converged twice in a
       //       row. This is because lateral_sync_data_.is_steer_converged is not the current but
       //       the previous value due to the order controllers' run and sync functions.
       const bool keep_stopped_condition =
         !m_prev_keep_stopped_condition ||
         (current_keep_stopped_condition || *m_prev_keep_stopped_condition);
       m_prev_keep_stopped_condition = current_keep_stopped_condition;
-      if (keep_stopped_condition) {
+      if (m_enable_keep_stopped_until_steer_convergence && keep_stopped_condition) {
         // debug print
         if (has_nonzero_target_vel) {
           debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED.");

diff --git a/control/autoware_trajectory_follower_node/test/test_controller_node.cpp b/control/autoware_trajectory_follower_node/test/test_controller_node.cpp
@@ -589,11 +589,28 @@ TEST_F(FakeNodeFixture, longitudinal_check_steer_converged)
   traj.points.push_back(make_traj_point(0.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);
+  {  // Check if the ego can keep stopped when the steering is not converged.
+    tester.traj_pub->publish(traj);
+    test_utils::waitForMessage(tester.node, this, tester.received_control_command);
 
-  ASSERT_TRUE(tester.received_control_command);
-  // Keep stopped state when the lateral control is not converged.
-  EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f);
+    ASSERT_TRUE(tester.received_control_command);
+    // Keep stopped state when the lateral control is not converged.
+    EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f);
+  }
+
+  {  // Check if the ego can keep stopped after the following sequence
+    // 1. not converged -> 2. converged -> 3. not converged
+    tester.publish_steer_angle(0.0);
+    tester.traj_pub->publish(traj);
+    test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+    tester.publish_steer_angle(steering_tire_angle);
+    tester.traj_pub->publish(traj);
+    test_utils::waitForMessage(tester.node, this, tester.received_control_command);
+
+    ASSERT_TRUE(tester.received_control_command);
+    // Keep stopped state when the lateral control is not converged.
+    EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.velocity, 0.0f);
+  }
 }