refactor(trajectory_follower_nodes): refactor the controller framework

Signed-off-by: Takayuki Murooka <[email protected]>

control/trajectory_follower/include/trajectory_follower/input_data.hpp

@@ -30,10 +30,10 @@ namespace trajectory_follower
 {
 struct InputData
 {
-  autoware_auto_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr;
-  nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr;
-  autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr;
-  geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr;
+  autoware_auto_planning_msgs::msg::Trajectory current_trajectory;
+  nav_msgs::msg::Odometry current_odometry;
+  autoware_auto_vehicle_msgs::msg::SteeringReport current_steering;
+  geometry_msgs::msg::AccelWithCovarianceStamped current_accel;
 };
 }  // namespace trajectory_follower
 }  // namespace control

control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp

@@ -40,8 +40,9 @@ struct LateralOutput
 class LateralControllerBase
 {
 public:
-  virtual boost::optional<LateralOutput> run() = 0;
-  virtual void setInputData(InputData const & input_data) = 0;
+  virtual bool initialize(const InputData & input_data) = 0;
+  virtual bool isReady() = 0;
+  virtual LateralOutput run(InputData const & input_data) = 0;
   void sync(LongitudinalSyncData const & longitudinal_sync_data)
   {
     longitudinal_sync_data_ = longitudinal_sync_data;

control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp

@@ -39,8 +39,9 @@ struct LongitudinalOutput
 class LongitudinalControllerBase
 {
 public:
-  virtual boost::optional<LongitudinalOutput> run() = 0;
-  virtual void setInputData(InputData const & input_data) = 0;
+  virtual bool initialize(const InputData & input_data) = 0;
+  virtual bool isReady() = 0;
+  virtual LongitudinalOutput run(InputData const & input_data) = 0;
   void sync(LateralSyncData const & lateral_sync_data) { lateral_sync_data_ = lateral_sync_data; }
 
 protected:

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -112,11 +112,11 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
   trajectory_follower::MPC m_mpc;
 
   //!< @brief measured kinematic state
-  nav_msgs::msg::Odometry::SharedPtr m_current_kinematic_state_ptr;
+  nav_msgs::msg::Odometry m_current_kinematic_state;
   //!< @brief measured steering
-  autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr m_current_steering_ptr;
+  autoware_auto_vehicle_msgs::msg::SteeringReport m_current_steering;
   //!< @brief reference trajectory
-  autoware_auto_planning_msgs::msg::Trajectory::SharedPtr m_current_trajectory_ptr;
+  autoware_auto_planning_msgs::msg::Trajectory m_current_trajectory;
 
   //!< @brief mpc filtered output in previous period
   double m_steer_cmd_prev = 0.0;
@@ -135,20 +135,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC MpcLateralController : public LateralController
 
   //!< initialize timer to work in real, simulation, and replay
   void initTimer(double period_s);
-  /**
-   * @brief compute control command for path follow with a constant control period
-   */
-  boost::optional<LateralOutput> run() override;
+
+  bool initialize(const InputData & input_data) override;
+  bool isReady() override;
 
   /**
-   * @brief set input data like current odometry, trajectory and steering.
+   * @brief compute control command for path follow with a constant control period
    */
-  void setInputData(InputData const & input_data) override;
+  LateralOutput run(InputData const & input_data) override;
 
   /**
    * @brief set m_current_trajectory with received message
    */
-  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr);
+  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory);
 
   /**
    * @brief check if the received data is valid.

control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp

@@ -91,9 +91,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal
     const std::vector<rclcpp::Parameter> & parameters);
 
   // pointers for ros topic
-  nav_msgs::msg::Odometry::ConstSharedPtr m_current_kinematic_state_ptr{nullptr};
-  geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr m_current_accel_ptr{nullptr};
-  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr m_trajectory_ptr{nullptr};
+  nav_msgs::msg::Odometry m_current_kinematic_state;
+  geometry_msgs::msg::AccelWithCovarianceStamped m_current_accel;
+  autoware_auto_planning_msgs::msg::Trajectory m_trajectory;
 
   // vehicle info
   double m_wheel_base;
@@ -223,30 +223,27 @@ 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::ConstSharedPtr 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::ConstSharedPtr 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::ConstSharedPtr msg);
+  void setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory msg);
 
-  /**
-   * @brief compute control command, and publish periodically
-   */
-  boost::optional<LongitudinalOutput> run() override;
+  bool initialize(const InputData & input_data) override;
+  bool isReady() override;
 
   /**
-   * @brief set input data like current odometry and trajectory.
+   * @brief compute control command, and publish periodically
    */
-  void setInputData(InputData const & input_data) override;
+  LongitudinalOutput run(InputData const & input_data) override;
 
   /**
    * @brief calculate data for controllers whose type is ControlData

control/trajectory_follower/include/trajectory_follower/sync_data.hpp

@@ -30,7 +30,8 @@ struct LateralSyncData
 
 struct LongitudinalSyncData
 {
-  bool is_velocity_converged{false};
+  // NOTE: This variable is not used for now
+  // bool is_velocity_converged{false};
 };
 
 }  // namespace trajectory_follower

control/trajectory_follower/src/mpc_lateral_controller.cpp

@@ -171,11 +171,19 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node}
 
 MpcLateralController::~MpcLateralController() {}
 
-boost::optional<LateralOutput> MpcLateralController::run()
+bool MpcLateralController::initialize(const InputData & input_data)
 {
-  if (!checkData()) {
-    return boost::none;
-  }
+  // is_steer_converged in LateralSyncData must be initialized for longitudinal cotnroller
+  run(input_data);
+  return true;
+}
+
+LateralOutput MpcLateralController::run(InputData const & input_data)
+{
+  // set input data
+  setTrajectory(input_data.current_trajectory);
+  m_current_kinematic_state = input_data.current_odometry;
+  m_current_steering = input_data.current_steering;
 
   autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd;
   autoware_auto_planning_msgs::msg::Trajectory predicted_traj;
@@ -187,8 +195,8 @@ boost::optional<LateralOutput> MpcLateralController::run()
   }
 
   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);
+    m_current_steering, m_current_kinematic_state.twist.twist.linear.x,
+    m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values);
 
   publishPredictedTraj(predicted_traj);
   publishDebugValues(debug_values);
@@ -197,7 +205,7 @@ boost::optional<LateralOutput> MpcLateralController::run()
     LateralOutput output;
     output.control_cmd = createCtrlCmdMsg(cmd);
     output.sync_data.is_steer_converged = isSteerConverged(cmd);
-    return boost::optional<LateralOutput>(output);
+    return output;
   };
 
   if (isStoppedState()) {
@@ -221,13 +229,6 @@ boost::optional<LateralOutput> MpcLateralController::run()
   return createLateralOutput(ctrl_cmd);
 }
 
-void MpcLateralController::setInputData(InputData const & input_data)
-{
-  setTrajectory(input_data.current_trajectory_ptr);
-  m_current_kinematic_state_ptr = input_data.current_odometry_ptr;
-  m_current_steering_ptr = input_data.current_steering_ptr;
-}
-
 bool MpcLateralController::isSteerConverged(
   const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const
 {
@@ -238,13 +239,13 @@ bool MpcLateralController::isSteerConverged(
   }
 
   const bool is_converged =
-    std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) <
+    std::abs(cmd.steering_tire_angle - m_current_steering.steering_tire_angle) <
     static_cast<float>(m_converged_steer_rad);
 
   return is_converged;
 }
 
-bool MpcLateralController::checkData() const
+bool MpcLateralController::isReady()
 {
   if (!m_mpc.hasVehicleModel()) {
     RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a vehicle model");
@@ -255,20 +256,6 @@ bool MpcLateralController::checkData() const
     return false;
   }
 
-  if (!m_current_kinematic_state_ptr) {
-    RCLCPP_DEBUG(
-      node_->get_logger(), "waiting data. kinematic_state = %d",
-      m_current_kinematic_state_ptr != nullptr);
-    return false;
-  }
-
-  if (!m_current_steering_ptr) {
-    RCLCPP_DEBUG(
-      node_->get_logger(), "waiting data. current_steering = %d",
-      m_current_steering_ptr != nullptr);
-    return false;
-  }
-
   if (m_mpc.m_ref_traj.size() == 0) {
     RCLCPP_DEBUG(node_->get_logger(), "trajectory size is zero.");
     return false;
@@ -277,34 +264,26 @@ bool MpcLateralController::checkData() const
   return true;
 }
 
-void MpcLateralController::setTrajectory(
-  const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg)
+void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory msg)
 {
-  if (!msg) return;
-
-  m_current_trajectory_ptr = msg;
-
-  if (!m_current_kinematic_state_ptr) {
-    RCLCPP_DEBUG(node_->get_logger(), "Current kinematic state is not received yet.");
-    return;
-  }
+  m_current_trajectory = msg;
 
-  if (msg->points.size() < 3) {
+  if (msg.points.size() < 3) {
     RCLCPP_DEBUG(node_->get_logger(), "received path size is < 3, not enough.");
     return;
   }
 
-  if (!isValidTrajectory(*msg)) {
+  if (!isValidTrajectory(msg)) {
     RCLCPP_ERROR(node_->get_logger(), "Trajectory is invalid!! stop computing.");
     return;
   }
 
   m_mpc.setReferenceTrajectory(
-    *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num,
+    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);
 
   // update trajectory buffer to check the trajectory shape change.
-  m_trajectory_buffer.push_back(*m_current_trajectory_ptr);
+  m_trajectory_buffer.push_back(m_current_trajectory);
   while (rclcpp::ok()) {
     const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) -
                            rclcpp::Time(m_trajectory_buffer.front().header.stamp);
@@ -333,15 +312,15 @@ autoware_auto_control_msgs::msg::AckermannLateralCommand
 MpcLateralController::getInitialControlCommand() const
 {
   autoware_auto_control_msgs::msg::AckermannLateralCommand cmd;
-  cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle;
+  cmd.steering_tire_angle = m_current_steering.steering_tire_angle;
   cmd.steering_tire_rotation_rate = 0.0;
   return cmd;
 }
 
 bool MpcLateralController::isStoppedState() const
 {
   // If the nearest index is not found, return false
-  if (m_current_trajectory_ptr->points.empty()) {
+  if (m_current_trajectory.points.empty()) {
     return false;
   }
 
@@ -350,12 +329,12 @@ bool MpcLateralController::isStoppedState() const
   // 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);
+    m_current_trajectory.points, m_current_kinematic_state.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 current_vel = m_current_kinematic_state.twist.twist.linear.x;
   const double target_vel =
-    m_current_trajectory_ptr->points.at(static_cast<size_t>(nearest)).longitudinal_velocity_mps;
+    m_current_trajectory.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)) {
@@ -383,7 +362,7 @@ void MpcLateralController::publishPredictedTraj(
   autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const
 {
   predicted_traj.header.stamp = node_->now();
-  predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id;
+  predicted_traj.header.frame_id = m_current_trajectory.header.frame_id;
   m_pub_predicted_traj->publish(predicted_traj);
 }
 
@@ -517,7 +496,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const
   for (const auto & trajectory : m_trajectory_buffer) {
     if (
       tier4_autoware_utils::calcDistance2d(
-        trajectory.points.back().pose, m_current_trajectory_ptr->points.back().pose) >
+        trajectory.points.back().pose, m_current_trajectory.points.back().pose) >
       m_new_traj_end_dist) {
       return true;
     }