refactor(pure_pursuit): minor refactoring for trajectory_follower_node (

autowarefoundation#2583)

* refactor(pure_pursuit): minor refactoring for trajectory_follower_node

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

* removed self_pose_listener

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

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

control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp

@@ -35,7 +35,6 @@
 #include "rclcpp/rclcpp.hpp"
 #include "tf2_ros/buffer.h"
 #include "tf2_ros/transform_listener.h"
-#include "tier4_autoware_utils/ros/self_pose_listener.hpp"
 #include "trajectory_follower_base/lateral_controller_base.hpp"
 
 #include <motion_utils/resample/resample.hpp>
@@ -105,8 +104,7 @@ class PurePursuitLateralController : public LateralControllerBase
 
 private:
   rclcpp::Node::SharedPtr node_;
-  tier4_autoware_utils::SelfPoseListener self_pose_listener_;
-  boost::optional<std::vector<TrajectoryPoint>> output_tp_array_;
+  std::vector<TrajectoryPoint> output_tp_array_;
   autoware_auto_planning_msgs::msg::Trajectory::SharedPtr trajectory_resampled_;
   autoware_auto_planning_msgs::msg::Trajectory trajectory_;
   nav_msgs::msg::Odometry current_odometry_;
@@ -120,8 +118,6 @@ class PurePursuitLateralController : public LateralControllerBase
   rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
     pub_predicted_trajectory_;
 
-  bool isDataReady();
-
   void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
 
   void onCurrentOdometry(const nav_msgs::msg::Odometry::ConstSharedPtr msg);
@@ -131,7 +127,7 @@ class PurePursuitLateralController : public LateralControllerBase
   // TF
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf_listener_;
-  geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
+  geometry_msgs::msg::Pose current_pose_;
 
   void publishDebugMarker() const;
 
@@ -162,7 +158,7 @@ class PurePursuitLateralController : public LateralControllerBase
 
   boost::optional<Trajectory> generatePredictedTrajectory();
 
-  boost::optional<AckermannLateralCommand> generateOutputControlCmd();
+  AckermannLateralCommand generateOutputControlCmd();
 
   bool calcIsSteerConverged(const AckermannLateralCommand & cmd);
 

control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp

@@ -57,7 +57,7 @@ enum TYPE {
 namespace pure_pursuit
 {
 PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
-: node_{&node}, self_pose_listener_(&node), tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
+: node_{&node}, tf_buffer_(node_->get_clock()), tf_listener_(tf_buffer_)
 {
   pure_pursuit_ = std::make_unique<PurePursuit>();
 
@@ -101,17 +101,6 @@ PurePursuitLateralController::PurePursuitLateralController(rclcpp::Node & node)
   tf_utils::waitForTransform(tf_buffer_, "map", "base_link");
 }
 
-bool PurePursuitLateralController::isDataReady()
-{
-  if (!current_pose_) {
-    RCLCPP_WARN_THROTTLE(
-      node_->get_logger(), *node_->get_clock(), 5000, "waiting for current_pose...");
-    return false;
-  }
-
-  return true;
-}
-
 double PurePursuitLateralController::calcLookaheadDistance(
   const double lateral_error, const double curvature, const double velocity, const double min_ld,
   const bool is_control_cmd)
@@ -181,8 +170,7 @@ void PurePursuitLateralController::setResampledTrajectory()
       motion_utils::convertToTrajectory(input_tp_array), out_arclength));
   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_));
+  output_tp_array_ = motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_);
 }
 
 double PurePursuitLateralController::calcCurvature(const size_t closest_idx)
@@ -273,7 +261,7 @@ void PurePursuitLateralController::averageFilterTrajectory(
 boost::optional<Trajectory> PurePursuitLateralController::generatePredictedTrajectory()
 {
   const auto closest_idx_result =
-    motion_utils::findNearestIndex(*output_tp_array_, current_pose_->pose, 3.0, M_PI_4);
+    motion_utils::findNearestIndex(output_tp_array_, current_pose_, 3.0, M_PI_4);
 
   if (!closest_idx_result) {
     return boost::none;
@@ -294,7 +282,7 @@ boost::optional<Trajectory> PurePursuitLateralController::generatePredictedTraje
       // For first point, use the odometry for velocity, and use the current_pose for prediction.
 
       TrajectoryPoint p;
-      p.pose = current_pose_->pose;
+      p.pose = current_pose_;
       p.longitudinal_velocity_mps = current_odometry_.twist.twist.linear.x;
       predicted_trajectory.points.push_back(p);
 
@@ -340,36 +328,14 @@ boost::optional<Trajectory> PurePursuitLateralController::generatePredictedTraje
   return predicted_trajectory;
 }
 
-bool PurePursuitLateralController::isReady(const InputData & input_data)
+bool PurePursuitLateralController::isReady([[maybe_unused]] 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 false;
-  }
-  setResampledTrajectory();
-  if (!output_tp_array_ || !trajectory_resampled_) {
-    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 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();
+  current_pose_ = input_data.current_odometry.pose.pose;
   trajectory_ = input_data.current_trajectory;
   current_odometry_ = input_data.current_odometry;
   current_steering_ = input_data.current_steering;
@@ -381,8 +347,8 @@ LateralOutput PurePursuitLateralController::run(const InputData & input_data)
   const auto cmd_msg = generateOutputControlCmd();
 
   LateralOutput output;
-  output.control_cmd = *cmd_msg;
-  output.sync_data.is_steer_converged = calcIsSteerConverged(*cmd_msg);
+  output.control_cmd = cmd_msg;
+  output.sync_data.is_steer_converged = calcIsSteerConverged(cmd_msg);
 
   // calculate predicted trajectory with iterative calculation
   const auto predicted_trajectory = generatePredictedTrajectory();
@@ -401,10 +367,10 @@ bool PurePursuitLateralController::calcIsSteerConverged(const AckermannLateralCo
          static_cast<float>(param_.converged_steer_rad_);
 }
 
-boost::optional<AckermannLateralCommand> PurePursuitLateralController::generateOutputControlCmd()
+AckermannLateralCommand PurePursuitLateralController::generateOutputControlCmd()
 {
   // Generate the control command
-  const auto pp_output = calcTargetCurvature(true, current_pose_->pose);
+  const auto pp_output = calcTargetCurvature(true, current_pose_);
   AckermannLateralCommand output_cmd;
 
   if (pp_output) {
@@ -444,7 +410,7 @@ void PurePursuitLateralController::publishDebugMarker() const
 
   marker_array.markers.push_back(createNextTargetMarker(debug_data_.next_target));
   marker_array.markers.push_back(
-    createTrajectoryCircleMarker(debug_data_.next_target, current_pose_->pose));
+    createTrajectoryCircleMarker(debug_data_.next_target, current_pose_));
 
   pub_debug_marker_->publish(marker_array);
 }
@@ -462,7 +428,7 @@ boost::optional<PpOutput> PurePursuitLateralController::calcTargetCurvature(
   // Calculate target point for velocity/acceleration
 
   const auto closest_idx_result =
-    motion_utils::findNearestIndex(*output_tp_array_, pose, 3.0, M_PI_4);
+    motion_utils::findNearestIndex(output_tp_array_, pose, 3.0, M_PI_4);
   if (!closest_idx_result) {
     RCLCPP_ERROR(node_->get_logger(), "cannot find closest waypoint");
     return {};