apply the change to pure_pursuit

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_nodes/package.xml

@@ -25,11 +25,11 @@
   <depend>autoware_auto_system_msgs</depend>
   <depend>autoware_auto_vehicle_msgs</depend>
   <depend>motion_utils</depend>
+  <depend>pure_pursuit</depend>
   <depend>rclcpp</depend>
   <depend>rclcpp_components</depend>
   <depend>trajectory_follower</depend>
   <depend>vehicle_info_util</depend>
-  <!-- <depend>pure_pursuit</depend> TODO(murooka) revert this later-->
 
   <exec_depend>ros2launch</exec_depend>
 

control/trajectory_follower_nodes/src/controller_node.cpp

@@ -14,8 +14,7 @@
 
 #include "trajectory_follower_nodes/controller_node.hpp"
 
-// TODO(murooka) revert pure_pursuit later
-// #include "pure_pursuit/pure_pursuit_lateral_controller.hpp"
+#include "pure_pursuit/pure_pursuit_lateral_controller.hpp"
 #include "trajectory_follower/mpc_lateral_controller.hpp"
 #include "trajectory_follower/pid_longitudinal_controller.hpp"
 
@@ -48,10 +47,10 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control
       lateral_controller_ = std::make_shared<trajectory_follower::MpcLateralController>(*this);
       break;
     }
-    // case LateralControllerMode::PURE_PURSUIT: {
-    //   lateral_controller_ = std::make_shared<pure_pursuit::PurePursuitLateralController>(*this);
-    //   break;
-    // }
+    case LateralControllerMode::PURE_PURSUIT: {
+      lateral_controller_ = std::make_shared<pure_pursuit::PurePursuitLateralController>(*this);
+      break;
+    }
     default:
       throw std::domain_error("[LateralController] invalid algorithm");
   }
@@ -92,7 +91,7 @@ Controller::LateralControllerMode Controller::getLateralControllerMode(
   const std::string & controller_mode) const
 {
   if (controller_mode == "mpc_follower") return LateralControllerMode::MPC;
-  // if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT;
+  if (controller_mode == "pure_pursuit") return LateralControllerMode::PURE_PURSUIT;
 
   return LateralControllerMode::INVALID;
 }