fix(controller): revival of dry steering (autowarefoundation#7903) (#…

…1405) * Revert "fix(autoware_mpc_lateral_controller): delete the zero speed constraint (autowarefoundation#7673)" This reverts commit 69258bd. * dry steering * add comments * add minor fix and modify unit test for dry steering --------- Signed-off-by: Takayuki Murooka <[email protected]>
tier4 · Jul 10, 2024 · e7b2795 · e7b2795
1 parent 49d0c35
commit e7b2795
Show file tree

Hide file tree

Showing 4 changed files with 64 additions and 33 deletions.
diff --git a/control/autoware_mpc_lateral_controller/src/mpc.cpp b/control/autoware_mpc_lateral_controller/src/mpc.cpp
@@ -765,10 +765,11 @@ VectorXd MPC::calcSteerRateLimitOnTrajectory(
     return reference.back();
   };
 
-  // when the vehicle is stopped, no steering rate limit.
+  // When the vehicle is stopped, a large steer rate limit is used for the dry steering.
+  constexpr double steer_rate_lim = 100.0;
   const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
   if (is_vehicle_stopped) {
-    return VectorXd::Zero(m_param.prediction_horizon);
+    return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon);
   }
 
   // calculate steering rate limit

diff --git a/...l_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp b/...l_controller/include/autoware/pid_longitudinal_controller/pid_longitudinal_controller.hpp
@@ -233,6 +233,8 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
   // debug values
   DebugValues m_debug_values;
 
+  std::optional<bool> m_prev_keep_stopped_condition{std::nullopt};
+
   std::shared_ptr<rclcpp::Time> m_last_running_time{std::make_shared<rclcpp::Time>(clock_->now())};
 
   // Diagnostic

diff --git a/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp b/control/autoware_pid_longitudinal_controller/src/pid_longitudinal_controller.cpp
@@ -597,21 +597,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
   // NOTE: the same velocity threshold as autoware::motion_utils::searchZeroVelocity
   static constexpr double vel_epsilon = 1e-3;
 
-  // Let vehicle start after the steering is converged for control accuracy
-  const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
-                                      m_enable_keep_stopped_until_steer_convergence &&
-                                      !lateral_sync_data_.is_steer_converged;
-  if (keep_stopped_condition) {
-    auto marker = createDefaultMarker(
-      "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
-      createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
-    marker.pose = autoware::universe_utils::calcOffsetPose(
-      m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
-      m_vehicle_width / 2 + 2.0, 1.5);
-    marker.text = "steering not\nconverged";
-    m_pub_stop_reason_marker->publish(marker);
-  }
-
   const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
 
   const bool is_stopped = std::abs(current_vel) < p.stopped_state_entry_vel &&
@@ -670,15 +655,18 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
     m_under_control_starting_time =
       is_under_control ? std::make_shared<rclcpp::Time>(clock_->now()) : nullptr;
   }
+
+  if (m_control_state != ControlState::STOPPED) {
+    m_prev_keep_stopped_condition = std::nullopt;
+  }
+
   // transit state
   // in DRIVE state
   if (m_control_state == ControlState::DRIVE) {
     if (emergency_condition) {
       return changeState(ControlState::EMERGENCY);
     }
-    if (!is_under_control && stopped_condition && keep_stopped_condition) {
-      // NOTE: When the ego is stopped on manual driving, since the driving state may transit to
-      //       autonomous, keep_stopped_condition should be checked.
+    if (!is_under_control && stopped_condition) {
       return changeState(ControlState::STOPPED);
     }
 
@@ -721,19 +709,42 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
 
   // in STOPPED state
   if (m_control_state == ControlState::STOPPED) {
-    // -- debug print --
+    // debug print
     if (has_nonzero_target_vel && !departure_condition_from_stopped) {
       debug_msg_once("target speed > 0, but departure condition is not met. Keep STOPPED.");
     }
-    if (has_nonzero_target_vel && keep_stopped_condition) {
-      debug_msg_once("target speed > 0, but keep stop condition is met. Keep STOPPED.");
-    }
-    // ---------------
 
-    if (keep_stopped_condition) {
-      return changeState(ControlState::STOPPED);
-    }
     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 && !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 (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.");
+        }
+
+        // publish debug marker
+        auto marker = createDefaultMarker(
+          "map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
+          createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
+        marker.pose = autoware::universe_utils::calcOffsetPose(
+          m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
+          m_vehicle_width / 2 + 2.0, 1.5);
+        marker.text = "steering not\nconverged";
+        m_pub_stop_reason_marker->publish(marker);
+
+        // keep STOPPED
+        return;
+      }
+
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
       // prevent the car from taking a long time to start to move

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);
+  }
 }