fix(trajectory_follower_nodes): mpc_follower does not send proper con…

…verged data under low steering rate limit

Signed-off-by: Berkay Karaman <[email protected]>

dasds


asd

control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp

@@ -31,6 +31,7 @@ namespace control
 {
 namespace trajectory_follower
 {
+
 struct LongitudinalOutput
 {
   autoware_auto_control_msgs::msg::LongitudinalCommand control_cmd;
@@ -43,6 +44,14 @@ class LongitudinalControllerBase
   virtual void setInputData(InputData const & input_data) = 0;
   void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; }
 
+  std::string toStr(const ControlState s)
+  {
+    if (s == ControlState::DRIVE) return "DRIVE";
+    if (s == ControlState::STOPPING) return "STOPPING";
+    if (s == ControlState::STOPPED) return "STOPPED";
+    if (s == ControlState::EMERGENCY) return "EMERGENCY";
+    return "UNDEFINED";
+  };
 protected:
   LateralSyncData lateral_sync_data_;
 };

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -187,7 +187,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   /**
    * @brief check ego car is in stopped state
    */
-  bool isStoppedState() const;
+  bool isStoppedState();
 
   /**
    * @brief check if the trajectory has valid value
@@ -196,7 +196,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
 
   bool isTrajectoryShapeChanged() const;
 
-  bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const;
+  bool isSteerConverged(
+    const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd, const bool is_mpc_solved);
 
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
 

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -98,17 +98,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
   // vehicle info
   double m_wheel_base;
 
-  // control state
-  enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY };
   ControlState m_control_state{ControlState::STOPPED};
-  std::string toStr(const ControlState s)
-  {
-    if (s == ControlState::DRIVE) return "DRIVE";
-    if (s == ControlState::STOPPING) return "STOPPING";
-    if (s == ControlState::STOPPED) return "STOPPED";
-    if (s == ControlState::EMERGENCY) return "EMERGENCY";
-    return "UNDEFINED";
-  };
 
   // control period
   double m_longitudinal_ctrl_period;

control/trajectory_follower/include/trajectory_follower/sync_data.hpp

@@ -23,6 +23,9 @@ namespace control
 {
 namespace trajectory_follower
 {
+// control state
+enum class ControlState { DRIVE = 0, STOPPING, STOPPED, EMERGENCY };
+
 struct LateralSyncData
 {
   bool is_steer_converged{false};
@@ -31,6 +34,7 @@ struct LateralSyncData
 struct LongitudinalSyncData
 {
   bool is_velocity_converged{false};
+  ControlState state_of_longitudinal_controller{ControlState::STOPPED};
 };
 
 }  // namespace trajectory_follower

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -186,39 +186,47 @@ boost::optional<LateralOutput> MpcLateralController::run()
     m_is_ctrl_cmd_prev_initialized = true;
   }
 
+  if (isStoppedState()) {
+    // If the difference between current steering and desired steering is large while vehicle in
+    // stopped state, the mpc may not solve the problem under low steering angle rate limits. And it
+    // sends the previous control command. We want to make sure that vehicle sends the desired
+    // steering angle before it starts to moving.
+    m_mpc.m_steer_rate_lim = std::numeric_limits<double>::max();
+    // Reset input buffer
+    for (auto & value : m_mpc.m_input_buffer) {
+      value = m_ctrl_cmd_prev.steering_tire_angle;
+    }
+    // Use previous command value as previous raw steer command
+    m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
+  } else {
+    m_mpc.m_steer_rate_lim =
+      tier4_autoware_utils::deg2rad(node_->get_parameter("steer_rate_lim_dps").as_double());
+  }
+
   const bool is_mpc_solved = m_mpc.calculateMPC(
     *m_current_steering_ptr, m_current_kinematic_state_ptr->twist.twist.linear.x,
     m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, debug_values);
 
   publishPredictedTraj(predicted_traj);
   publishDebugValues(debug_values);
 
-  const auto createLateralOutput = [this](const auto & cmd) {
+  const auto createLateralOutput = [this](const auto & cmd, const bool is_mpc_solved) {
     LateralOutput output;
     output.control_cmd = createCtrlCmdMsg(cmd);
-    output.sync_data.is_steer_converged = isSteerConverged(cmd);
+    output.sync_data.is_steer_converged = isSteerConverged(cmd, is_mpc_solved);
     return boost::optional<LateralOutput>(output);
   };
 
-  if (isStoppedState()) {
-    // Reset input buffer
-    for (auto & value : m_mpc.m_input_buffer) {
-      value = m_ctrl_cmd_prev.steering_tire_angle;
-    }
-    // Use previous command value as previous raw steer command
-    m_mpc.m_raw_steer_cmd_prev = m_ctrl_cmd_prev.steering_tire_angle;
-    return createLateralOutput(m_ctrl_cmd_prev);
-  }
-
   if (!is_mpc_solved) {
     RCLCPP_WARN_SKIPFIRST_THROTTLE(
       node_->get_logger(), *node_->get_clock(), 5000 /*ms*/,
       "MPC is not solved. publish 0 velocity.");
     ctrl_cmd = getStopControlCommand();
   }
 
+  const auto output = createLateralOutput(ctrl_cmd, is_mpc_solved);
   m_ctrl_cmd_prev = ctrl_cmd;
-  return createLateralOutput(ctrl_cmd);
+  return output;
 }
 
 void MpcLateralController::setInputData(InputData const & input_data)
@@ -229,17 +237,24 @@ void MpcLateralController::setInputData(InputData const & input_data)
 }
 
 bool MpcLateralController::isSteerConverged(
-  const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const
+  const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd, const bool is_mpc_solved)
 {
   // wait for a while to propagate the trajectory shape to the output command when the trajectory
   // shape is changed.
   if (!m_has_received_first_trajectory || isTrajectoryShapeChanged()) {
     return false;
   }
 
+  const auto isCmdChanging = [this](const auto & cmd) {
+    constexpr double min_steer_rate_treshold = 0.001;  // [rad/s]
+    const double steer_cmd_diff = std::abs(cmd.steering_tire_angle - m_ctrl_cmd_prev.steering_tire_angle);
+    const double steer_cmd_rate = steer_cmd_diff / m_mpc.m_ctrl_period;
+    return steer_cmd_rate > min_steer_rate_treshold;  // need to consider which thresh is reasonable
+  };
+
   const bool is_converged =
-    std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
-    static_cast<float>(m_converged_steer_rad);
+    (std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
+     static_cast<float>(m_converged_steer_rad)) && !isCmdChanging(cmd) && is_mpc_solved;
 
   return is_converged;
 }
@@ -338,33 +353,15 @@ MpcLateralController::getInitialControlCommand() const
   return cmd;
 }
 
-bool MpcLateralController::isStoppedState() const
+bool MpcLateralController::isStoppedState()
 {
-  // If the nearest index is not found, return false
-  if (m_current_trajectory_ptr->points.empty()) {
-    return false;
-  }
-
-  // Note: This function used to take into account the distance to the stop line
-  // for the stop state judgement. However, it has been removed since the steering
-  // control was turned off when approaching/exceeding the stop line on a curve or
-  // emergency stop situation and it caused large tracking error.
-  const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints(
-    m_current_trajectory_ptr->points, m_current_kinematic_state_ptr->pose.pose,
-    m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold);
-
-  const double current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x;
-  const double target_vel =
-    m_current_trajectory_ptr->points.at(static_cast<size_t>(nearest)).longitudinal_velocity_mps;
-
   const auto latest_published_cmd = m_ctrl_cmd_prev;  // use prev_cmd as a latest published command
-  if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) {
-    return false;  // not stopState: keep control
+  if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd, true)) {
+    return false;
   }
-
   if (
-    std::fabs(current_vel) < m_stop_state_entry_ego_speed &&
-    std::fabs(target_vel) < m_stop_state_entry_target_speed) {
+    longitudinal_sync_data_.state_of_longitudinal_controller == ControlState::EMERGENCY ||
+    longitudinal_sync_data_.state_of_longitudinal_controller == ControlState::STOPPED) {
     return true;
   } else {
     return false;

control/trajectory_follower/src/pid_longitudinal_controller.cpp

@@ -385,6 +385,7 @@ boost::optional<LongitudinalOutput> PidLongitudinalController::run()
     publishDebugData(raw_ctrl_cmd, control_data);                       // publish debug data
     LongitudinalOutput output;
     output.control_cmd = cmd_msg;
+    output.sync_data.state_of_longitudinal_controller = m_control_state;
     return output;
   }
 
@@ -398,6 +399,7 @@ boost::optional<LongitudinalOutput> PidLongitudinalController::run()
   const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd, control_data.current_motion.vel);
   LongitudinalOutput output;
   output.control_cmd = cmd_msg;
+  output.sync_data.state_of_longitudinal_controller = m_control_state;
 
   // publish debug data
   publishDebugData(ctrl_cmd, control_data);