diff --git a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp b/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp index c0ca24bfcfb28..e9e57cf08bcc4 100644 --- a/control/pure_pursuit/include/pure_pursuit/pure_pursuit_lateral_controller.hpp +++ b/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 @@ -105,8 +104,7 @@ class PurePursuitLateralController : public LateralControllerBase private: rclcpp::Node::SharedPtr node_; - tier4_autoware_utils::SelfPoseListener self_pose_listener_; - boost::optional> output_tp_array_; + std::vector 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::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 generatePredictedTrajectory(); - boost::optional generateOutputControlCmd(); + AckermannLateralCommand generateOutputControlCmd(); bool calcIsSteerConverged(const AckermannLateralCommand & cmd); diff --git a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp b/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp index fdd5577b686d6..5cf4ee07990b2 100644 --- a/control/pure_pursuit/src/pure_pursuit/pure_pursuit_lateral_controller.cpp +++ b/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(); @@ -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>( - 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 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 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 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(param_.converged_steer_rad_); } -boost::optional 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 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 {};