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 8150b7b0212dd..6a5ea9d17b4c9 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 @@ -108,9 +108,9 @@ class PurePursuitLateralController : public LateralControllerBase tier4_autoware_utils::SelfPoseListener self_pose_listener_; boost::optional> 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 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 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_{}; 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 7067e89f7e748..fdd5577b686d6 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 @@ -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,7 +171,7 @@ void PurePursuitLateralController::setResampledTrajectory() { // Interpolate with constant interval distance. std::vector 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); @@ -204,8 +179,8 @@ void PurePursuitLateralController::setResampledTrajectory() trajectory_resampled_ = std::make_shared(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>( motion_utils::convertToTrajectoryPointArray(*trajectory_resampled_)); } @@ -320,7 +295,7 @@ boost::optional 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,15 +340,19 @@ boost::optional PurePursuitLateralController::generatePredictedTraje return predicted_trajectory; } -boost::optional 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_); @@ -381,8 +360,26 @@ boost::optional PurePursuitLateralController::run() 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 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(param_.converged_steer_rad_); } @@ -491,7 +488,7 @@ boost::optional 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 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; } diff --git a/control/trajectory_follower_nodes/package.xml b/control/trajectory_follower_nodes/package.xml index f35a2685419a2..de184228e5f87 100644 --- a/control/trajectory_follower_nodes/package.xml +++ b/control/trajectory_follower_nodes/package.xml @@ -25,11 +25,11 @@ autoware_auto_system_msgs autoware_auto_vehicle_msgs motion_utils + pure_pursuit rclcpp rclcpp_components trajectory_follower vehicle_info_util - ros2launch diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index ca9fe54dfbff5..30b8602eb8757 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/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(*this); break; } - // case LateralControllerMode::PURE_PURSUIT: { - // lateral_controller_ = std::make_shared(*this); - // break; - // } + case LateralControllerMode::PURE_PURSUIT: { + lateral_controller_ = std::make_shared(*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; }