diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp index d370af383b39a..540fab950cebe 100644 --- a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp +++ b/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) { diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp index d743dc29e6294..a163c98fa56db 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/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; } diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index fcb1f850337c4..56544243abb82 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/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. diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index ae0f4c6b0688d..d37aed07c377b 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/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 diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index 986f50f6c16e1..abe57e1bf2110 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/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; diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index 706d9a47ba188..77af30e33bfbb 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/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) { diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index 8634bede4a552..2bd40f404d907 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/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 longitudinal_output_{boost::none}; - boost::optional 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 createInputData() const; + boost::optional 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; diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index a0cca2249870a..ca9fe54dfbff5 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/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 Controller::createInputData() const +boost::optional 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;