fix the bug that initialization of mpc does not finish

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

control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp

@@ -40,7 +40,7 @@ struct LateralOutput
 class LateralControllerBase
 {
 public:
-  virtual bool isReady() = 0;
+  virtual bool isReady(const InputData & input_data) = 0;
   virtual LateralOutput run(InputData const & input_data) = 0;
   void sync(LongitudinalSyncData const & longitudinal_sync_data)
   {

control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp

@@ -39,7 +39,7 @@ struct LongitudinalOutput
 class LongitudinalControllerBase
 {
 public:
-  virtual bool isReady() = 0;
+  virtual bool isReady(const InputData & input_data) = 0;
   virtual LongitudinalOutput run(InputData const & input_data) = 0;
   void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; }
 

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -136,7 +136,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   //!< initialize timer to work in real, simulation, and replay
   void initTimer(double period_s);
 
-  bool isReady() override;
+  bool isReady(const InputData & input_data) override;
 
   /**
    * @brief compute control command for path follow with a constant control period
@@ -146,7 +146,7 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   /**
    * @brief set m_current_trajectory with received message
    */
-  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory);
+  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg);
 
   /**
    * @brief check if the received data is valid.

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -223,21 +223,21 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
    * @brief set current and previous velocity with received message
    * @param [in] msg current state message
    */
-  void setKinematicState(const nav_msgs::msg::Odometry msg);
+  void setKinematicState(const nav_msgs::msg::Odometry & msg);
 
   /**
    * @brief set current acceleration with received message
    * @param [in] msg trajectory message
    */
-  void setCurrentAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped msg);
+  void setCurrentAcceleration(const geometry_msgs::msg::AccelWithCovarianceStamped & msg);
 
   /**
    * @brief set reference trajectory with received message
    * @param [in] msg trajectory message
    */
-  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory msg);
+  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg);
 
-  bool isReady() override;
+  bool isReady(const InputData & input_data) override;
 
   /**
    * @brief compute control command, and publish periodically

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -238,26 +238,32 @@ bool MpcLateralController::isSteerConverged(
   return is_converged;
 }
 
-bool MpcLateralController::isReady()
+bool MpcLateralController::isReady(const InputData & input_data)
 {
+  setTrajectory(input_data.current_trajectory);
+  m_current_kinematic_state = input_data.current_odometry;
+  m_current_steering = input_data.current_steering;
+
   if (!m_mpc.hasVehicleModel()) {
-    RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a vehicle model");
+    RCLCPP_INFO_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a vehicle model");
     return false;
   }
   if (!m_mpc.hasQPSolver()) {
-    RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a QP solver");
+    RCLCPP_INFO_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a QP solver");
     return false;
   }
-
   if (m_mpc.m_ref_traj.size() == 0) {
-    RCLCPP_DEBUG(node_->get_logger(), "trajectory size is zero.");
+    RCLCPP_INFO_THROTTLE(
+      node_->get_logger(), *node_->get_clock(), 5000, "trajectory size is zero.");
     return false;
   }
 
   return true;
 }
 
-void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory msg)
+void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg)
 {
   m_current_trajectory = msg;
 

control/trajectory_follower/src/pid_longitudinal_controller.cpp

@@ -199,19 +199,19 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
   setupDiagnosticUpdater();
 }
 
-void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry msg)
+void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry & msg)
 {
   m_current_kinematic_state = msg;
 }
 
 void PidLongitudinalController::setCurrentAcceleration(
-  const geometry_msgs::msg::AccelWithCovarianceStamped msg)
+  const geometry_msgs::msg::AccelWithCovarianceStamped & msg)
 {
   m_current_accel = msg;
 }
 
 void PidLongitudinalController::setTrajectory(
-  const autoware_auto_planning_msgs::msg::Trajectory msg)
+  const autoware_auto_planning_msgs::msg::Trajectory & msg)
 {
   if (!trajectory_follower::longitudinal_utils::isValidTrajectory(msg)) {
     RCLCPP_ERROR_THROTTLE(
@@ -351,7 +351,10 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac
   return result;
 }
 
-bool PidLongitudinalController::isReady() { return true; }
+bool PidLongitudinalController::isReady([[maybe_unused]] const InputData & input_data)
+{
+  return true;
+}
 
 LongitudinalOutput PidLongitudinalController::run(InputData const & input_data)
 {

control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp

@@ -71,7 +71,6 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   rclcpp::TimerBase::SharedPtr timer_control_;
   double timeout_thr_sec_;
   boost::optional<LongitudinalOutput> longitudinal_output_{boost::none};
-  boost::optional<LateralOutput> lateral_output_{boost::none};
 
   bool is_initialized_{false};
 
@@ -103,14 +102,14 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node
   /**
    * @brief compute control command, and publish periodically
    */
-  boost::optional<trajectory_follower::InputData> createInputData() const;
+  boost::optional<trajectory_follower::InputData> createInputData(rclcpp::Clock & clock) const;
   void initialize(const trajectory_follower::InputData & input_data);
   void callbackTimerControl();
   void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
   void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg);
   void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg);
   void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg);
-  bool isTimeOut();
+  bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out);
   LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const;
   LongitudinalControllerMode getLongitudinalControllerMode(
     const std::string & algorithm_name) const;

control/trajectory_follower_nodes/src/controller_node.cpp

@@ -125,16 +125,18 @@ void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::S
   current_accel_ptr_ = msg;
 }
 
-bool Controller::isTimeOut()
+bool Controller::isTimeOut(
+  const trajectory_follower::LongitudinalOutput & lon_out,
+  const trajectory_follower::LateralOutput & lat_out)
 {
   const auto now = this->now();
-  if ((now - lateral_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) {
+  if ((now - lat_out.control_cmd.stamp).seconds() > timeout_thr_sec_) {
     RCLCPP_ERROR_THROTTLE(
       get_logger(), *get_clock(), 5000 /*ms*/,
       "Lateral control command too old, control_cmd will not be published.");
     return true;
   }
-  if ((now - longitudinal_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) {
+  if ((now - lon_out.control_cmd.stamp).seconds() > timeout_thr_sec_) {
     RCLCPP_ERROR_THROTTLE(
       get_logger(), *get_clock(), 5000 /*ms*/,
       "Longitudinal control command too old, control_cmd will not be published.");
@@ -143,12 +145,26 @@ bool Controller::isTimeOut()
   return false;
 }
 
-boost::optional<trajectory_follower::InputData> Controller::createInputData() const
+boost::optional<trajectory_follower::InputData> Controller::createInputData(
+  rclcpp::Clock & clock) const
 {
-  if (
-    !current_trajectory_ptr_ || !current_odometry_ptr_ || !current_steering_ptr_ ||
-    !current_accel_ptr_) {
-    // RCL
+  if (!current_trajectory_ptr_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory.");
+    return {};
+  }
+
+  if (!current_odometry_ptr_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry.");
+    return {};
+  }
+
+  if (!current_steering_ptr_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering.");
+    return {};
+  }
+
+  if (!current_accel_ptr_) {
+    RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel.");
     return {};
   }
 
@@ -179,19 +195,28 @@ void Controller::initialize(const trajectory_follower::InputData & input_data)
 void Controller::callbackTimerControl()
 {
   // 1. create input data
-  const auto input_data = createInputData();
+  const auto input_data = createInputData(*get_clock());
   if (!input_data) {
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), *get_clock(), 5000, "Control is skipped since input data is not ready.");
     return;
   }
 
   // 2. check if controllers are ready
-  if (!lateral_controller_->isReady() || !longitudinal_controller_->isReady()) {
+  if (
+    !lateral_controller_->isReady(*input_data) || !longitudinal_controller_->isReady(*input_data)) {
+    RCLCPP_INFO_THROTTLE(
+      get_logger(), *get_clock(), 5000,
+      "Control is skipped since lateral and/or longitudinal controllers are not ready to run.");
     return;
   }
 
   // 3. check if controllers are initialized
   if (!is_initialized_) {
     initialize(*input_data);
+    RCLCPP_INFO(
+      get_logger(),
+      "Control is skipped since lateral and longitudinal controllers are being initialized.");
     return;
   }
 
@@ -204,7 +229,7 @@ void Controller::callbackTimerControl()
   lateral_controller_->sync(lon_out.sync_data);
 
   // TODO(Horibe): Think specification. This comes from the old implementation.
-  if (isTimeOut()) return;
+  if (isTimeOut(lon_out, lat_out)) return;
 
   // 6. publish control command
   autoware_auto_control_msgs::msg::AckermannControlCommand out;