refactor(trajectory_follower_nodes): refactor the controller framework (

autowarefoundation#2432)

* refactor(trajectory_follower_nodes): refactor the controller framework

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

* fix

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

* add test for keeping stopped state when steer is not converged

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

* fix the bug that initialization of mpc does not finish

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

* apply the change to pure_pursuit

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

* remove initialize function and implement reset function

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

* minor change

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

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

control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp

@@ -108,9 +108,9 @@ class PurePursuitLateralController : public LateralControllerBase
   tier4_autoware_utils::SelfPoseListener self_pose_listener_;
   boost::optional<std::vector<TrajectoryPoint>> output_tp_array_;
   autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_;
-  autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr trajectory_;
-  nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_;
-  autoware_auto_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_;
+  autoware_auto_planning_msgs::msg::Trajectory trajectory_;
+  nav_msgs::msg::Odometry current_odometry_;
+  autoware_auto_vehicle_msgs::msg::SteeringReport current_steering_;
   boost::optional<AckermannLateralCommand> prev_cmd_;
 
   // Debug Publisher
@@ -138,15 +138,11 @@ class PurePursuitLateralController : public LateralControllerBase
   /**
    * @brief compute control command for path follow with a constant control period
    */
-  boost::optional<LateralOutput> run() override;
+  bool isReady([[maybe_unused]] const InputData & input_data) override;
+  LateralOutput run(const InputData & input_data) override;
 
   AckermannLateralCommand generateCtrlCmdMsg(const double target_curvature);
 
-  /**
-   * @brief set input data
-   */
-  void setInputData(InputData const & input_data) override;
-
   // Parameter
   Param param_{};
 

control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp

@@ -103,30 +103,12 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
 
 bool PurePursuitLateralController::isDataReady()
 {
-  if (!current_odometry_) {
-    RCLCPP_WARN_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_odometry...");
-    return false;
-  }
-
-  if (!trajectory_) {
-    RCLCPP_WARN_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), 5000, "waiting for trajectory...");
-    return false;
-  }
-
   if (!current_pose_) {
     RCLCPP_WARN_THROTTLE(
       node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_pose...");
     return false;
   }
 
-  if (!current_steering_) {
-    RCLCPP_WARN_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_steering...");
-    return false;
-  }
-
   return true;
 }
 
@@ -168,13 +150,6 @@ double PurePursuitLateralController::calcLookaheadDistance(
   return total_ld;
 }
 
-void PurePursuitLateralController::setInputData(InputData const & input_data)
-{
-  trajectory_ = input_data.current_trajectory_ptr;
-  current_odometry_ = input_data.current_odometry_ptr;
-  current_steering_ = input_data.current_steering_ptr;
-}
-
 TrajectoryPoint PurePursuitLateralController::calcNextPose(
   const double ds, TrajectoryPoint & point, AckermannLateralCommand cmd) const
 {
@@ -196,16 +171,16 @@ void PurePursuitLateralController::setResampledTrajectory()
 {
   // Interpolate with constant interval distance.
   std::vector<double> out_arclength;
-  const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(*trajectory_);
+  const auto input_tp_array = motion_utils::convertToTrajectoryPointArray(trajectory_);
   const auto traj_length = motion_utils::calcArcLength(input_tp_array);
   for (double s = 0; s < traj_length; s += param_.resampling_ds) {
     out_arclength.push_back(s);
   }
   trajectory_resampled_ =
     std::make_shared<autoware_auto_planning_msgs::msg::Trajectory>(motion_utils::resampleTrajectory(
       motion_utils::convertToTrajectory(input_tp_array), out_arclength));
-  trajectory_resampled_->points.back() = trajectory_->points.back();
-  trajectory_resampled_->header = trajectory_->header;
+  trajectory_resampled_->points.back() = trajectory_.points.back();
+  trajectory_resampled_->header = trajectory_.header;
   output_tp_array_ = boost::optional<std::vector<TrajectoryPoint>>(
     motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_));
 }
@@ -320,7 +295,7 @@ boost::optional<Trajectory> PurePursuitLateralController::generatePredictedTraje
 
       TrajectoryPoint p;
       p.pose = current_pose_->pose;
-      p.longitudinal_velocity_mps = current_odometry_->twist.twist.linear.x;
+      p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x;
       predicted_trajectory.points.push_back(p);
 
       const auto pp_output = calcTargetCurvature(true, predicted_trajectory.points.at(i).pose);
@@ -365,24 +340,46 @@ boost::optional<Trajectory> PurePursuitLateralController::generatePredictedTraje
   return predicted_trajectory;
 }
 
-boost::optional<LateralOutput> PurePursuitLateralController::run()
+bool PurePursuitLateralController::isReady(const InputData & input_data)
 {
   current_pose_ = self_pose_listener_.getCurrentPose();
+  trajectory_ = input_data.current_trajectory;
+  current_odometry_ = input_data.current_odometry;
+  current_steering_ = input_data.current_steering;
+
   if (!isDataReady()) {
-    return boost::none;
+    return false;
   }
   setResampledTrajectory();
   if (!output_tp_array_ || !trajectory_resampled_) {
-    return boost::none;
+    return false;
   }
   if (param_.enable_path_smoothing) {
     averageFilterTrajectory(*trajectory_resampled_);
   }
   const auto cmd_msg = generateOutputControlCmd();
   if (!cmd_msg) {
     RCLCPP_ERROR(node_->get_logger(), "Failed to generate control command.");
-    return boost::none;
+    return false;
   }
+
+  return true;
+}
+
+LateralOutput PurePursuitLateralController::run(const InputData & input_data)
+{
+  // TODO(murooka) needs to be refactored to seperate isReady and run clearly
+  current_pose_ = self_pose_listener_.getCurrentPose();
+  trajectory_ = input_data.current_trajectory;
+  current_odometry_ = input_data.current_odometry;
+  current_steering_ = input_data.current_steering;
+
+  setResampledTrajectory();
+  if (param_.enable_path_smoothing) {
+    averageFilterTrajectory(*trajectory_resampled_);
+  }
+  const auto cmd_msg = generateOutputControlCmd();
+
   LateralOutput output;
   output.control_cmd = *cmd_msg;
   output.sync_data.is_steer_converged = calcIsSteerConverged(*cmd_msg);
@@ -400,7 +397,7 @@ boost::optional<LateralOutput> PurePursuitLateralController::run()
 
 bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCommand & cmd)
 {
-  return std::abs(cmd.steering_tire_angle - current_steering_->steering_tire_angle) <
+  return std::abs(cmd.steering_tire_angle - current_steering_.steering_tire_angle) <
          static_cast<float>(param_.converged_steer_rad_);
 }
 
@@ -491,7 +488,7 @@ boost::optional<PpOutput> PurePursuitLateralController::calcTargetCurvature(
   double lookahead_distance = min_lookahead_distance;
   if (is_control_output) {
     lookahead_distance = calcLookaheadDistance(
-      lateral_error, current_curvature, current_odometry_->twist.twist.linear.x,
+      lateral_error, current_curvature, current_odometry_.twist.twist.linear.x,
       min_lookahead_distance, is_control_output);
   } else {
     lookahead_distance = calcLookaheadDistance(
@@ -518,7 +515,7 @@ boost::optional<PpOutput> PurePursuitLateralController::calcTargetCurvature(
   PpOutput output{};
   output.curvature = kappa;
   if (!is_control_output) {
-    output.velocity = current_odometry_->twist.twist.linear.x;
+    output.velocity = current_odometry_.twist.twist.linear.x;
   } else {
     output.velocity = target_vel;
   }

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,8 @@ struct LateralOutput
 class LateralControllerBase
 {
 public:
-  virtual boost::optional<LateralOutput> run() = 0;
-  virtual void setInputData(InputData const & input_data) = 0;
+  virtual bool isReady(const InputData & input_data) = 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,9 +39,12 @@ struct LongitudinalOutput
 class LongitudinalControllerBase
 {
 public:
-  virtual boost::optional<LongitudinalOutput> run() = 0;
-  virtual void setInputData(InputData const & input_data) = 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; }
+  // NOTE: This reset function should be called when the trajectory is replaned by changing ego pose
+  // or goal pose.
+  void reset() { lateral_sync_data_.is_steer_converged = false; }
 
 protected:
   LateralSyncData lateral_sync_data_;

control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp

@@ -115,11 +115,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;
@@ -138,20 +138,18 @@ 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 isReady(const InputData & input_data) 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 & msg);
 
   /**
    * @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,26 @@ 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 isReady(const InputData & input_data) 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