update from review comment

Signed-off-by: Takamasa Horibe <horibe.takamasa@gmail.com>

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -104,7 +104,12 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   float64_t m_stop_state_entry_ego_speed;
   float64_t m_stop_state_entry_target_speed;
   float64_t m_converged_steer_rad;
-  bool m_check_steer_converged_for_stopped_state;
+  float64_t m_new_traj_duration_time;  // check trajectory shape change
+  float64_t m_new_traj_end_dist;       // check trajectory shape change
+  bool m_keep_steer_control_until_converged;
+
+  // trajectory buffer for detecting new trajectory
+  std::deque<autoware_auto_planning_msgs::msg::Trajectory> m_trajectory_buffer;
 
   // MPC object
   trajectory_follower::MPC m_mpc;
@@ -198,6 +203,10 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
    */
   bool8_t isValidTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & traj) const;
 
+  bool8_t isTrajectoryShapeChanged() const;
+
+  bool isSteerConverged(const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const;
+
   rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
 
   /**

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -124,9 +124,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
   bool8_t m_enable_slope_compensation;
   bool8_t m_enable_keep_stopped_until_steer_convergence;
 
-  // trajectory beffer for detecting new trajectory
-  std::deque<autoware_auto_planning_msgs::msg::Trajectory> m_trajectory_buffer;
-
   // smooth stop transition
   struct StateTransitionParams
   {
@@ -139,8 +136,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
     float64_t stopped_state_entry_duration_time;
     float64_t stopped_state_entry_vel;
     float64_t stopped_state_entry_acc;
-    float64_t stopped_state_new_traj_duration_time;
-    float64_t stopped_state_new_traj_end_dist;
     // emergency
     float64_t emergency_state_overshoot_stop_dist;
     float64_t emergency_state_traj_trans_dev;
@@ -246,8 +241,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
    */
   Motion calcEmergencyCtrlCmd(const float64_t dt) const;
 
-  bool isNewTrajectory();
-
   /**
    * @brief update control state according to the current situation
    * @param [in] current_control_state current control state

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -73,8 +73,10 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
   m_stop_state_entry_target_speed =
     node_->declare_parameter<float64_t>("stop_state_entry_target_speed");
   m_converged_steer_rad = node_->declare_parameter<float64_t>("converged_steer_rad");
-  m_check_steer_converged_for_stopped_state =
-    node_->declare_parameter<bool8_t>("check_steer_converged_for_stopped_state");
+  m_keep_steer_control_until_converged =
+    node_->declare_parameter<bool8_t>("keep_steer_control_until_converged");
+  m_new_traj_duration_time = node_->declare_parameter<float64_t>("new_traj_duration_time");  // [s]
+  m_new_traj_end_dist = node_->declare_parameter<float64_t>("new_traj_end_dist");            // [m]
 
   /* mpc parameters */
   const float64_t steer_lim_deg = node_->declare_parameter<float64_t>("steer_lim_deg");
@@ -180,23 +182,24 @@ boost::optional<LateralOutput> MpcLateralController::run()
     *m_current_steering_ptr, m_current_odometry_ptr->twist.twist.linear.x, m_current_pose_ptr->pose,
     ctrl_cmd, predicted_traj, diagnostic);
 
+  publishPredictedTraj(predicted_traj);
+  publishDiagnostic(diagnostic);
+
+  const auto createLateralOutput = [this](const auto & cmd) {
+    LateralOutput output;
+    output.control_cmd = createCtrlCmdMsg(cmd);
+    output.sync_data.is_steer_converged = isSteerConverged(cmd);
+    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;
-
-    const auto cmd_msg = createCtrlCmdMsg(m_ctrl_cmd_prev);
-    publishPredictedTraj(predicted_traj);
-    publishDiagnostic(diagnostic);
-    LateralOutput output;
-    output.control_cmd = cmd_msg;
-    output.sync_data.is_steer_converged =
-      std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
-      static_cast<float>(m_converged_steer_rad);
-    return boost::optional<LateralOutput>(output);
+    return createLateralOutput(m_ctrl_cmd_prev);
   }
 
   if (!is_mpc_solved) {
@@ -207,16 +210,7 @@ boost::optional<LateralOutput> MpcLateralController::run()
   }
 
   m_ctrl_cmd_prev = ctrl_cmd;
-  const auto cmd_msg = createCtrlCmdMsg(ctrl_cmd);
-  publishPredictedTraj(predicted_traj);
-  publishDiagnostic(diagnostic);
-  LateralOutput output;
-  output.control_cmd = cmd_msg;
-  output.sync_data.is_steer_converged =
-    std::abs(cmd_msg.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
-    static_cast<float>(m_converged_steer_rad);
-
-  return boost::optional<LateralOutput>(output);
+  return createLateralOutput(ctrl_cmd);
 }
 
 void MpcLateralController::setInputData(InputData const & input_data)
@@ -226,6 +220,22 @@ void MpcLateralController::setInputData(InputData const & input_data)
   m_current_steering_ptr = input_data.current_steering_ptr;
 }
 
+bool MpcLateralController::isSteerConverged(
+  const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const
+{
+  // wait for a while to propagate the trajectory shape to the output command when the trajectory
+  // shape is changed.
+  if (isTrajectoryShapeChanged()) {
+    return false;
+  }
+
+  const bool is_converged =
+    std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
+    static_cast<float>(m_converged_steer_rad);
+
+  return is_converged;
+}
+
 bool8_t MpcLateralController::checkData() const
 {
   if (!m_mpc.hasVehicleModel()) {
@@ -284,6 +294,17 @@ void MpcLateralController::setTrajectory(
   m_mpc.setReferenceTrajectory(
     *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num,
     m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_current_pose_ptr);
+
+  // update trajectory buffer to check the trajectory shape change.
+  m_trajectory_buffer.push_back(*m_current_trajectory_ptr);
+  while (rclcpp::ok()) {
+    const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) -
+                           rclcpp::Time(m_trajectory_buffer.front().header.stamp);
+    if (time_diff.seconds() < m_new_traj_duration_time) {
+      break;
+    }
+    m_trajectory_buffer.pop_front();
+  }
 }
 
 bool8_t MpcLateralController::updateCurrentPose()
@@ -346,12 +367,15 @@ bool8_t MpcLateralController::isStoppedState() const
   const float64_t current_vel = m_current_odometry_ptr->twist.twist.linear.x;
   const float64_t 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 (
     std::fabs(current_vel) < m_stop_state_entry_ego_speed &&
-    std::fabs(target_vel) < m_stop_state_entry_target_speed &&
-    (!m_check_steer_converged_for_stopped_state ||
-     std::abs(m_ctrl_cmd_prev.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
-       static_cast<float>(m_converged_steer_rad))) {
+    std::fabs(target_vel) < m_stop_state_entry_target_speed) {
     return true;
   } else {
     return false;
@@ -496,6 +520,21 @@ rcl_interfaces::msg::SetParametersResult MpcLateralController::paramCallback(
   return result;
 }
 
+bool MpcLateralController::isTrajectoryShapeChanged() const
+{
+  // TODO(Horibe): update implementation to check trajectory shape around ego vehicle.
+  // Now temporally check the goal position.
+  for (const auto & trajectory : m_trajectory_buffer) {
+    if (
+      tier4_autoware_utils::calcDistance2d(
+        trajectory.points.back().pose, m_current_trajectory_ptr->points.back().pose) >
+      m_new_traj_end_dist) {
+      return true;
+    }
+  }
+  return false;
+}
+
 bool8_t MpcLateralController::isValidTrajectory(
   const autoware_auto_planning_msgs::msg::Trajectory & traj) const
 {

control/trajectory_follower/src/pid_longitudinal_controller.cpp

@@ -71,10 +71,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) : node
       node_->declare_parameter<float64_t>("stopped_state_entry_vel");  // [m/s]
     p.stopped_state_entry_acc =
       node_->declare_parameter<float64_t>("stopped_state_entry_acc");  // [m/s²]
-    p.stopped_state_new_traj_duration_time =
-      node_->declare_parameter<float64_t>("stopped_state_new_traj_duration_time");  // [s]
-    p.stopped_state_new_traj_end_dist =
-      node_->declare_parameter<float64_t>("stopped_state_new_traj_end_dist");  // [m]
+
     // emergency
     p.emergency_state_overshoot_stop_dist =
       node_->declare_parameter<float64_t>("emergency_state_overshoot_stop_dist");  // [m]
@@ -256,8 +253,6 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
     update_param("stopped_state_entry_duration_time", p.stopped_state_entry_duration_time);
     update_param("stopped_state_entry_vel", p.stopped_state_entry_vel);
     update_param("stopped_state_entry_acc", p.stopped_state_entry_acc);
-    update_param("stopped_state_new_traj_duration_time", p.stopped_state_new_traj_duration_time);
-    update_param("stopped_state_new_traj_end_dist", p.stopped_state_new_traj_end_dist);
     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);
@@ -471,32 +466,6 @@ PidLongitudinalController::Motion PidLongitudinalController::calcEmergencyCtrlCm
   return Motion{vel, acc};
 }
 
-bool PidLongitudinalController::isNewTrajectory()
-{
-  // flags for state transition
-  const auto & p = m_state_transition_params;
-
-  m_trajectory_buffer.push_back(*m_trajectory_ptr);
-  while (true) {
-    const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) -
-                           rclcpp::Time(m_trajectory_buffer.front().header.stamp);
-    if (time_diff.seconds() < p.stopped_state_new_traj_duration_time) {
-      break;
-    }
-    m_trajectory_buffer.pop_front();
-  }
-
-  for (const auto & trajectory : m_trajectory_buffer) {
-    if (
-      tier4_autoware_utils::calcDistance2d(
-        trajectory.points.back().pose, m_trajectory_ptr->points.back().pose) >
-      p.stopped_state_new_traj_end_dist) {
-      return true;
-    }
-  }
-  return false;
-}
-
 PidLongitudinalController::ControlState PidLongitudinalController::updateControlState(
   const ControlState current_control_state, const ControlData & control_data)
 {
@@ -510,8 +479,9 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl
   const bool8_t departure_condition_from_stopping =
     stop_dist > p.drive_state_stop_dist + p.drive_state_offset_stop_dist;
   const bool8_t departure_condition_from_stopped = stop_dist > p.drive_state_stop_dist;
+
   const bool8_t keep_stopped_condition =
-    !lateral_sync_data_.is_steer_converged || isNewTrajectory();
+    m_enable_keep_stopped_until_steer_convergence && !lateral_sync_data_.is_steer_converged;
 
   const bool8_t stopping_condition = stop_dist < p.stopping_state_stop_dist;
   if (
@@ -566,9 +536,8 @@ PidLongitudinalController::ControlState PidLongitudinalController::updateControl
       return ControlState::DRIVE;
     }
   } else if (current_control_state == ControlState::STOPPED) {
-    if (
-      departure_condition_from_stopped &&
-      (!m_enable_keep_stopped_until_steer_convergence || !keep_stopped_condition)) {
+    if (keep_stopped_condition) return ControlState::STOPPED;
+    if (departure_condition_from_stopped) {
       m_pid_vel.reset();
       m_lpf_vel_error->reset(0.0);
       // prevent the car from taking a long time to start to move

control/trajectory_follower_nodes/param/lateral_controller_defaults.param.yaml

@@ -56,7 +56,9 @@
     stop_state_entry_ego_speed: 0.001
     stop_state_entry_target_speed: 0.001
     converged_steer_rad: 0.1
-    check_steer_converged_for_stopped_state: true
+    keep_steer_control_until_converged: true
+    new_traj_duration_time: 1.0
+    new_traj_end_dist: 0.3
 
     # vehicle parameters
     mass_kg: 2400.0

control/trajectory_follower_nodes/param/longitudinal_controller_defaults.param.yaml

@@ -14,8 +14,6 @@
     stopped_state_entry_duration_time: 0.1
     stopped_state_entry_vel: 0.1
     stopped_state_entry_acc: 0.1
-    stopped_state_new_traj_duration_time: 1.0
-    stopped_state_new_traj_end_dist: 0.3
     emergency_state_overshoot_stop_dist: 1.5
     emergency_state_traj_trans_dev: 3.0
     emergency_state_traj_rot_dev: 0.7

control/trajectory_follower_nodes/src/controller_node.cpp

@@ -122,8 +122,6 @@ void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringRepor
 
 bool Controller::isTimeOut()
 {
-  if (!longitudinal_output_ || !lateral_output_) return true;
-
   const auto now = this->now();
   if ((now - lateral_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) {
     RCLCPP_ERROR_THROTTLE(
@@ -146,12 +144,16 @@ void Controller::callbackTimerControl()
   lateral_controller_->setInputData(input_data_);       // trajectory, odometry, steering
 
   const auto lon_out = longitudinal_controller_->run();
-  longitudinal_output_ = lon_out ? lon_out : longitudinal_output_;
+  longitudinal_output_ = lon_out ? lon_out : longitudinal_output_;  // use previous value if none.
   const auto lat_out = lateral_controller_->run();
-  lateral_output_ = lat_out ? lat_out : lateral_output_;
+  lateral_output_ = lat_out ? lat_out : lateral_output_;  // use previous value if none.
+
+  if (!longitudinal_output_ || !lateral_output_) return;
+
+  longitudinal_controller_->sync(lateral_output_->sync_data);
+  lateral_controller_->sync(longitudinal_output_->sync_data);
 
-  if (lateral_output_) longitudinal_controller_->sync(lateral_output_->sync_data);
-  if (longitudinal_output_) lateral_controller_->sync(longitudinal_output_->sync_data);
+  // TODO(Horibe): Think specification. This comes from the old implementation.
   if (isTimeOut()) return;
 
   autoware_auto_control_msgs::msg::AckermannControlCommand out;

launch/tier4_control_launch/config/trajectory_follower/lateral_controller.param.yaml

@@ -57,7 +57,9 @@
     stop_state_entry_ego_speed: 0.001
     stop_state_entry_target_speed: 0.001
     converged_steer_rad: 0.1
-    check_steer_converged_for_stopped_state: true
+    keep_steer_control_until_converged: true
+    new_traj_duration_time: 1.0
+    new_traj_end_dist: 0.3
 
     # vehicle parameters
     vehicle:

launch/tier4_control_launch/config/trajectory_follower/longitudinal_controller.param.yaml

@@ -14,8 +14,6 @@
     stopped_state_entry_duration_time: 0.1
     stopped_state_entry_vel: 0.1
     stopped_state_entry_acc: 0.1
-    stopped_state_new_traj_duration_time: 1.0
-    stopped_state_new_traj_end_dist: 0.3
     emergency_state_overshoot_stop_dist: 1.5
     emergency_state_traj_trans_dev: 3.0
     emergency_state_traj_rot_dev: 0.7