From 50701301325402063dfff065de2170a256fdaf42 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Sat, 24 Dec 2022 23:40:42 +0900 Subject: [PATCH] refactor(trajectory_follower_nodes): refactor the controller framework (#2432) * refactor(trajectory_follower_nodes): refactor the controller framework Signed-off-by: Takayuki Murooka * fix Signed-off-by: Takayuki Murooka * add test for keeping stopped state when steer is not converged Signed-off-by: Takayuki Murooka * fix the bug that initialization of mpc does not finish Signed-off-by: Takayuki Murooka * apply the change to pure_pursuit Signed-off-by: Takayuki Murooka * remove initialize function and implement reset function Signed-off-by: Takayuki Murooka * minor change Signed-off-by: Takayuki Murooka Signed-off-by: Takayuki Murooka --- .../pure_pursuit_lateral_controller.hpp | 14 +-- .../pure_pursuit_lateral_controller.cpp | 69 +++++------ .../trajectory_follower/input_data.hpp | 8 +- .../lateral_controller_base.hpp | 4 +- .../longitudinal_controller_base.hpp | 7 +- .../mpc_lateral_controller.hpp | 18 ++- .../pid_longitudinal_controller.hpp | 22 ++-- .../include/trajectory_follower/sync_data.hpp | 3 +- .../src/mpc_lateral_controller.cpp | 90 ++++++-------- .../src/pid_longitudinal_controller.cpp | 67 +++++----- .../controller_node.hpp | 14 ++- .../src/controller_node.cpp | 110 ++++++++++++----- .../test/test_controller_node.cpp | 115 +++++++++++++++--- 13 files changed, 319 insertions(+), 222 deletions(-) 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/include/trajectory_follower/input_data.hpp b/control/trajectory_follower/include/trajectory_follower/input_data.hpp index b356095e2f78e..6eab1823a5915 100644 --- a/control/trajectory_follower/include/trajectory_follower/input_data.hpp +++ b/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 diff --git a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp index 0a234fa0b42b1..540fab950cebe 100644 --- a/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp +++ b/control/trajectory_follower/include/trajectory_follower/lateral_controller_base.hpp @@ -40,8 +40,8 @@ struct LateralOutput class LateralControllerBase { public: - virtual boost::optional 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; diff --git a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp index 6eb4d13119275..c6f578ebdaef7 100644 --- a/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp +++ b/control/trajectory_follower/include/trajectory_follower/longitudinal_controller_base.hpp @@ -39,9 +39,12 @@ struct LongitudinalOutput class LongitudinalControllerBase { public: - virtual boost::optional 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_; diff --git a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp b/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp index 8ed6d44b85258..8b764d3fb7b32 100644 --- a/control/trajectory_follower/include/trajectory_follower/mpc_lateral_controller.hpp +++ b/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 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. diff --git a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp index f91c09ecf3527..d37aed07c377b 100644 --- a/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp +++ b/control/trajectory_follower/include/trajectory_follower/pid_longitudinal_controller.hpp @@ -91,9 +91,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC PidLongitudinalController : public Longitudinal const std::vector & 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 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 diff --git a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp b/control/trajectory_follower/include/trajectory_follower/sync_data.hpp index 48224a5498a9c..7acb37aa69808 100644 --- a/control/trajectory_follower/include/trajectory_follower/sync_data.hpp +++ b/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 diff --git a/control/trajectory_follower/src/mpc_lateral_controller.cpp b/control/trajectory_follower/src/mpc_lateral_controller.cpp index b2e0ac18753a5..b9c5e9c77af41 100644 --- a/control/trajectory_follower/src/mpc_lateral_controller.cpp +++ b/control/trajectory_follower/src/mpc_lateral_controller.cpp @@ -173,11 +173,12 @@ MpcLateralController::MpcLateralController(rclcpp::Node & node) : node_{&node} MpcLateralController::~MpcLateralController() {} -boost::optional MpcLateralController::run() +LateralOutput MpcLateralController::run(InputData const & input_data) { - if (!checkData()) { - return boost::none; - } + // set input data + setTrajectory(input_data.current_trajectory); + m_current_kinematic_state = input_data.current_odometry; + m_current_steering = input_data.current_steering; autoware_auto_control_msgs::msg::AckermannLateralCommand ctrl_cmd; autoware_auto_planning_msgs::msg::Trajectory predicted_traj; @@ -189,8 +190,8 @@ boost::optional MpcLateralController::run() } const bool is_mpc_solved = m_mpc.calculateMPC( - *m_current_steering_ptr, m_current_kinematic_state_ptr->twist.twist.linear.x, - m_current_kinematic_state_ptr->pose.pose, ctrl_cmd, predicted_traj, debug_values); + m_current_steering, m_current_kinematic_state.twist.twist.linear.x, + m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values); publishPredictedTraj(predicted_traj); publishDebugValues(debug_values); @@ -199,7 +200,7 @@ boost::optional MpcLateralController::run() LateralOutput output; output.control_cmd = createCtrlCmdMsg(cmd); output.sync_data.is_steer_converged = isSteerConverged(cmd); - return boost::optional(output); + return output; }; if (isStoppedState()) { @@ -223,13 +224,6 @@ boost::optional MpcLateralController::run() return createLateralOutput(ctrl_cmd); } -void MpcLateralController::setInputData(InputData const & input_data) -{ - setTrajectory(input_data.current_trajectory_ptr); - m_current_kinematic_state_ptr = input_data.current_odometry_ptr; - m_current_steering_ptr = input_data.current_steering_ptr; -} - bool MpcLateralController::isSteerConverged( const autoware_auto_control_msgs::msg::AckermannLateralCommand & cmd) const { @@ -241,74 +235,58 @@ bool MpcLateralController::isSteerConverged( } const bool is_converged = - std::abs(cmd.steering_tire_angle - m_current_steering_ptr->steering_tire_angle) < + std::abs(cmd.steering_tire_angle - m_current_steering.steering_tire_angle) < static_cast(m_converged_steer_rad); return is_converged; } -bool MpcLateralController::checkData() const +bool MpcLateralController::isReady(const InputData & input_data) { + setTrajectory(input_data.current_trajectory); + m_current_kinematic_state = input_data.current_odometry; + m_current_steering = input_data.current_steering; + if (!m_mpc.hasVehicleModel()) { - RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a vehicle model"); + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a vehicle model"); return false; } if (!m_mpc.hasQPSolver()) { - RCLCPP_DEBUG(node_->get_logger(), "MPC does not have a QP solver"); - return false; - } - - if (!m_current_kinematic_state_ptr) { - RCLCPP_DEBUG( - node_->get_logger(), "waiting data. kinematic_state = %d", - m_current_kinematic_state_ptr != nullptr); + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "MPC does not have a QP solver"); return false; } - - if (!m_current_steering_ptr) { - RCLCPP_DEBUG( - node_->get_logger(), "waiting data. current_steering = %d", - m_current_steering_ptr != nullptr); - return false; - } - if (m_mpc.m_ref_traj.size() == 0) { - RCLCPP_DEBUG(node_->get_logger(), "trajectory size is zero."); + RCLCPP_INFO_THROTTLE( + node_->get_logger(), *node_->get_clock(), 5000, "trajectory size is zero."); return false; } return true; } -void MpcLateralController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) +void MpcLateralController::setTrajectory(const autoware_auto_planning_msgs::msg::Trajectory & msg) { - if (!msg) return; - - m_current_trajectory_ptr = msg; - - if (!m_current_kinematic_state_ptr) { - RCLCPP_DEBUG(node_->get_logger(), "Current kinematic state is not received yet."); - return; - } + m_current_trajectory = msg; - if (msg->points.size() < 3) { + if (msg.points.size() < 3) { RCLCPP_DEBUG(node_->get_logger(), "received path size is < 3, not enough."); return; } - if (!isValidTrajectory(*msg)) { + if (!isValidTrajectory(msg)) { RCLCPP_ERROR(node_->get_logger(), "Trajectory is invalid!! stop computing."); return; } m_mpc.setReferenceTrajectory( - *msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, + msg, m_traj_resample_dist, m_enable_path_smoothing, m_path_filter_moving_ave_num, m_curvature_smoothing_num_traj, m_curvature_smoothing_num_ref_steer, m_extend_trajectory_for_end_yaw_control); // update trajectory buffer to check the trajectory shape change. - m_trajectory_buffer.push_back(*m_current_trajectory_ptr); + m_trajectory_buffer.push_back(m_current_trajectory); while (rclcpp::ok()) { const auto time_diff = rclcpp::Time(m_trajectory_buffer.back().header.stamp) - rclcpp::Time(m_trajectory_buffer.front().header.stamp); @@ -337,7 +315,7 @@ autoware_auto_control_msgs::msg::AckermannLateralCommand MpcLateralController::getInitialControlCommand() const { autoware_auto_control_msgs::msg::AckermannLateralCommand cmd; - cmd.steering_tire_angle = m_current_steering_ptr->steering_tire_angle; + cmd.steering_tire_angle = m_current_steering.steering_tire_angle; cmd.steering_tire_rotation_rate = 0.0; return cmd; } @@ -345,7 +323,7 @@ MpcLateralController::getInitialControlCommand() const bool MpcLateralController::isStoppedState() const { // If the nearest index is not found, return false - if (m_current_trajectory_ptr->points.empty()) { + if (m_current_trajectory.points.empty()) { return false; } @@ -354,12 +332,12 @@ bool MpcLateralController::isStoppedState() const // control was turned off when approaching/exceeding the stop line on a curve or // emergency stop situation and it caused large tracking error. const size_t nearest = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_current_trajectory_ptr->points, m_current_kinematic_state_ptr->pose.pose, - m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + m_current_trajectory.points, m_current_kinematic_state.pose.pose, m_ego_nearest_dist_threshold, + m_ego_nearest_yaw_threshold); - const double current_vel = m_current_kinematic_state_ptr->twist.twist.linear.x; + const double current_vel = m_current_kinematic_state.twist.twist.linear.x; const double target_vel = - m_current_trajectory_ptr->points.at(static_cast(nearest)).longitudinal_velocity_mps; + m_current_trajectory.points.at(static_cast(nearest)).longitudinal_velocity_mps; const auto latest_published_cmd = m_ctrl_cmd_prev; // use prev_cmd as a latest published command if (m_keep_steer_control_until_converged && !isSteerConverged(latest_published_cmd)) { @@ -387,7 +365,7 @@ void MpcLateralController::publishPredictedTraj( autoware_auto_planning_msgs::msg::Trajectory & predicted_traj) const { predicted_traj.header.stamp = node_->now(); - predicted_traj.header.frame_id = m_current_trajectory_ptr->header.frame_id; + predicted_traj.header.frame_id = m_current_trajectory.header.frame_id; m_pub_predicted_traj->publish(predicted_traj); } @@ -521,7 +499,7 @@ bool MpcLateralController::isTrajectoryShapeChanged() const for (const auto & trajectory : m_trajectory_buffer) { if ( tier4_autoware_utils::calcDistance2d( - trajectory.points.back().pose, m_current_trajectory_ptr->points.back().pose) > + trajectory.points.back().pose, m_current_trajectory.points.back().pose) > m_new_traj_end_dist) { return true; } diff --git a/control/trajectory_follower/src/pid_longitudinal_controller.cpp b/control/trajectory_follower/src/pid_longitudinal_controller.cpp index bde2139bcaa78..77af30e33bfbb 100644 --- a/control/trajectory_follower/src/pid_longitudinal_controller.cpp +++ b/control/trajectory_follower/src/pid_longitudinal_controller.cpp @@ -198,44 +198,34 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node) // diagnostic setupDiagnosticUpdater(); } -void PidLongitudinalController::setInputData(InputData const & input_data) -{ - setTrajectory(input_data.current_trajectory_ptr); - setKinematicState(input_data.current_odometry_ptr); - setCurrentAcceleration(input_data.current_accel_ptr); -} -void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry::ConstSharedPtr msg) +void PidLongitudinalController::setKinematicState(const nav_msgs::msg::Odometry & msg) { - if (!msg) return; - m_current_kinematic_state_ptr = msg; + m_current_kinematic_state = msg; } void PidLongitudinalController::setCurrentAcceleration( - const geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr msg) + const geometry_msgs::msg::AccelWithCovarianceStamped & msg) { - if (!msg) return; - m_current_accel_ptr = msg; + m_current_accel = msg; } void PidLongitudinalController::setTrajectory( - const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg) + const autoware_auto_planning_msgs::msg::Trajectory & msg) { - if (!msg) return; - - if (!trajectory_follower::longitudinal_utils::isValidTrajectory(*msg)) { + if (!trajectory_follower::longitudinal_utils::isValidTrajectory(msg)) { RCLCPP_ERROR_THROTTLE( node_->get_logger(), *node_->get_clock(), 3000, "received invalid trajectory. ignore."); return; } - if (msg->points.size() < 2) { + if (msg.points.size() < 2) { RCLCPP_WARN_THROTTLE( node_->get_logger(), *node_->get_clock(), 3000, "Unexpected trajectory size < 2. Ignored."); return; } - m_trajectory_ptr = msg; + m_trajectory = msg; } rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallback( @@ -361,15 +351,20 @@ rcl_interfaces::msg::SetParametersResult PidLongitudinalController::paramCallbac return result; } -boost::optional PidLongitudinalController::run() +bool PidLongitudinalController::isReady([[maybe_unused]] const InputData & input_data) { - // wait for initial pointers - if (!m_current_kinematic_state_ptr || !m_trajectory_ptr || !m_current_accel_ptr) { - return boost::none; - } + return true; +} + +LongitudinalOutput PidLongitudinalController::run(InputData const & input_data) +{ + // set input data + setTrajectory(input_data.current_trajectory); + setKinematicState(input_data.current_odometry); + setCurrentAcceleration(input_data.current_accel); // calculate current pose and control data - geometry_msgs::msg::Pose current_pose = m_current_kinematic_state_ptr->pose.pose; + geometry_msgs::msg::Pose current_pose = m_current_kinematic_state.pose.pose; const auto control_data = getControlData(current_pose); @@ -417,14 +412,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData control_data.dt = getDt(); // current velocity and acceleration - control_data.current_motion.vel = m_current_kinematic_state_ptr->twist.twist.linear.x; - control_data.current_motion.acc = m_current_accel_ptr->accel.accel.linear.x; + control_data.current_motion.vel = m_current_kinematic_state.twist.twist.linear.x; + control_data.current_motion.acc = m_current_accel.accel.accel.linear.x; // nearest idx const size_t nearest_idx = motion_utils::findFirstNearestIndexWithSoftConstraints( - m_trajectory_ptr->points, current_pose, m_ego_nearest_dist_threshold, - m_ego_nearest_yaw_threshold); - const auto & nearest_point = m_trajectory_ptr->points.at(nearest_idx).pose; + m_trajectory.points, current_pose, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + const auto & nearest_point = m_trajectory.points.at(nearest_idx).pose; // check if the deviation is worth emergency m_diagnostic_data.trans_deviation = @@ -452,13 +446,13 @@ PidLongitudinalController::ControlData PidLongitudinalController::getControlData // distance to stopline control_data.stop_dist = trajectory_follower::longitudinal_utils::calcStopDistance( - current_pose, *m_trajectory_ptr, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); + current_pose, m_trajectory, m_ego_nearest_dist_threshold, m_ego_nearest_yaw_threshold); // pitch const double raw_pitch = trajectory_follower::longitudinal_utils::getPitchByPose(current_pose.orientation); const double traj_pitch = trajectory_follower::longitudinal_utils::getPitchByTraj( - *m_trajectory_ptr, control_data.nearest_idx, m_wheel_base); + m_trajectory, control_data.nearest_idx, m_wheel_base); control_data.slope_angle = m_use_traj_for_pitch ? traj_pitch : m_lpf_pitch->filter(raw_pitch); updatePitchDebugValues(control_data.slope_angle, traj_pitch, raw_pitch); @@ -512,7 +506,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d static constexpr double vel_epsilon = 1e-3; // NOTE: the same velocity threshold as motion_utils::searchZeroVelocity const double current_vel_cmd = - std::fabs(m_trajectory_ptr->points.at(control_data.nearest_idx).longitudinal_velocity_mps); + std::fabs(m_trajectory.points.at(control_data.nearest_idx).longitudinal_velocity_mps); const bool emergency_condition = m_enable_overshoot_emergency && stop_dist < -p.emergency_state_overshoot_stop_dist && current_vel_cmd < vel_epsilon; @@ -628,13 +622,12 @@ PidLongitudinalController::Motion PidLongitudinalController::calcCtrlCmd( if (m_control_state == ControlState::DRIVE) { const auto target_pose = trajectory_follower::longitudinal_utils::calcPoseAfterTimeDelay( current_pose, m_delay_compensation_time, current_vel); - const auto target_interpolated_point = - calcInterpolatedTargetValue(*m_trajectory_ptr, target_pose); + const auto target_interpolated_point = calcInterpolatedTargetValue(m_trajectory, target_pose); target_motion = Motion{ target_interpolated_point.longitudinal_velocity_mps, target_interpolated_point.acceleration_mps2}; - target_motion = keepBrakeBeforeStop(*m_trajectory_ptr, target_motion, nearest_idx); + target_motion = keepBrakeBeforeStop(m_trajectory, target_motion, nearest_idx); const double pred_vel_in_target = predictedVelocityInTargetPoint(control_data.current_motion, m_delay_compensation_time); @@ -752,7 +745,7 @@ enum PidLongitudinalController::Shift PidLongitudinalController::getCurrentShift { constexpr double epsilon = 1e-5; - const double target_vel = m_trajectory_ptr->points.at(nearest_idx).longitudinal_velocity_mps; + const double target_vel = m_trajectory.points.at(nearest_idx).longitudinal_velocity_mps; if (target_vel > epsilon) { return Shift::Forward; @@ -965,7 +958,7 @@ void PidLongitudinalController::updateDebugVelAcc( using trajectory_follower::DebugValues; const double current_vel = control_data.current_motion.vel; - const auto interpolated_point = calcInterpolatedTargetValue(*m_trajectory_ptr, current_pose); + const auto interpolated_point = calcInterpolatedTargetValue(m_trajectory, current_pose); m_debug_values.setValues(DebugValues::TYPE::CURRENT_VEL, current_vel); m_debug_values.setValues(DebugValues::TYPE::TARGET_VEL, target_motion.vel); diff --git a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp index 67f901ec1f8f5..62bf01b75eb44 100644 --- a/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp +++ b/control/trajectory_follower_nodes/include/trajectory_follower_nodes/controller_node.hpp @@ -68,10 +68,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node private: rclcpp::TimerBase::SharedPtr timer_control_; - trajectory_follower::InputData input_data_; double timeout_thr_sec_; boost::optional longitudinal_output_{boost::none}; - boost::optional lateral_output_{boost::none}; std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; @@ -84,6 +82,11 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node control_cmd_pub_; rclcpp::Publisher::SharedPtr debug_marker_pub_; + 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_; + enum class LateralControllerMode { INVALID = 0, MPC = 1, @@ -97,16 +100,19 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node /** * @brief compute control command, and publish periodically */ + boost::optional createInputData(rclcpp::Clock & clock) const; void callbackTimerControl(); void onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr); void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); void onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg); void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); - bool isTimeOut(); + bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; LongitudinalControllerMode getLongitudinalControllerMode( const std::string & algorithm_name) const; - void publishDebugMarker() const; + void publishDebugMarker( + const trajectory_follower::InputData & input_data, + const trajectory_follower::LateralOutput & lat_out) const; }; } // namespace trajectory_follower_nodes } // namespace control diff --git a/control/trajectory_follower_nodes/src/controller_node.cpp b/control/trajectory_follower_nodes/src/controller_node.cpp index 8b168afc08416..0436d0feca0d7 100644 --- a/control/trajectory_follower_nodes/src/controller_node.cpp +++ b/control/trajectory_follower_nodes/src/controller_node.cpp @@ -108,34 +108,36 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode void Controller::onTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::SharedPtr msg) { - input_data_.current_trajectory_ptr = msg; + current_trajectory_ptr_ = msg; } void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) { - input_data_.current_odometry_ptr = msg; + current_odometry_ptr_ = msg; } void Controller::onSteering(const autoware_auto_vehicle_msgs::msg::SteeringReport::SharedPtr msg) { - input_data_.current_steering_ptr = msg; + current_steering_ptr_ = msg; } void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) { - input_data_.current_accel_ptr = msg; + current_accel_ptr_ = msg; } -bool Controller::isTimeOut() +bool Controller::isTimeOut( + const trajectory_follower::LongitudinalOutput & lon_out, + const trajectory_follower::LateralOutput & lat_out) { const auto now = this->now(); - if ((now - lateral_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) { + if ((now - lat_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000 /*ms*/, "Lateral control command too old, control_cmd will not be published."); return true; } - if ((now - longitudinal_output_->control_cmd.stamp).seconds() > timeout_thr_sec_) { + if ((now - lon_out.control_cmd.stamp).seconds() > timeout_thr_sec_) { RCLCPP_ERROR_THROTTLE( get_logger(), *get_clock(), 5000 /*ms*/, "Longitudinal control command too old, control_cmd will not be published."); @@ -144,37 +146,83 @@ bool Controller::isTimeOut() return false; } +boost::optional Controller::createInputData( + rclcpp::Clock & clock) const +{ + if (!current_trajectory_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for trajectory."); + return {}; + } + + if (!current_odometry_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current odometry."); + return {}; + } + + if (!current_steering_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current steering."); + return {}; + } + + if (!current_accel_ptr_) { + RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current accel."); + return {}; + } + + trajectory_follower::InputData input_data; + input_data.current_trajectory = *current_trajectory_ptr_; + input_data.current_odometry = *current_odometry_ptr_; + input_data.current_steering = *current_steering_ptr_; + input_data.current_accel = *current_accel_ptr_; + + return input_data; +} + void Controller::callbackTimerControl() { - // Since the longitudinal uses the convergence information of the steer - // with the current trajectory, it is necessary to run the lateral first. - // TODO(kosuke55): Do not depend on the order of execution. - lateral_controller_->setInputData(input_data_); // trajectory, odometry, steering - const auto lat_out = lateral_controller_->run(); - lateral_output_ = lat_out ? lat_out : lateral_output_; // use previous value if none. - if (!lateral_output_) return; - - longitudinal_controller_->sync(lateral_output_->sync_data); - longitudinal_controller_->setInputData(input_data_); // trajectory, odometry - const auto lon_out = longitudinal_controller_->run(); - longitudinal_output_ = lon_out ? lon_out : longitudinal_output_; // use previous value if none. - if (!longitudinal_output_) return; - - lateral_controller_->sync(longitudinal_output_->sync_data); + // 1. create input data + const auto input_data = createInputData(*get_clock()); + if (!input_data) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, "Control is skipped since input data is not ready."); + return; + } + + // 2. check if controllers are ready + const bool is_lat_ready = lateral_controller_->isReady(*input_data); + const bool is_lon_ready = longitudinal_controller_->isReady(*input_data); + if (!is_lat_ready || !is_lon_ready) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 5000, + "Control is skipped since lateral and/or longitudinal controllers are not ready to run."); + return; + } + + // 3. run controllers + const auto lat_out = lateral_controller_->run(*input_data); + const auto lon_out = longitudinal_controller_->run(*input_data); + + // 4. sync with each other controllers + longitudinal_controller_->sync(lat_out.sync_data); + lateral_controller_->sync(lon_out.sync_data); // TODO(Horibe): Think specification. This comes from the old implementation. - if (isTimeOut()) return; + if (isTimeOut(lon_out, lat_out)) return; + // 5. publish control command autoware_auto_control_msgs::msg::AckermannControlCommand out; out.stamp = this->now(); - out.lateral = lateral_output_->control_cmd; - out.longitudinal = longitudinal_output_->control_cmd; + out.lateral = lat_out.control_cmd; + out.longitudinal = lon_out.control_cmd; control_cmd_pub_->publish(out); - publishDebugMarker(); + // 6. publish debug marker + publishDebugMarker(*input_data, lat_out); } -void Controller::publishDebugMarker() const +void Controller::publishDebugMarker( + const trajectory_follower::InputData & input_data, + const trajectory_follower::LateralOutput & lat_out) const { visualization_msgs::msg::MarkerArray debug_marker_array{}; @@ -184,14 +232,14 @@ void Controller::publishDebugMarker() const "map", this->now(), "steer_converged", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, tier4_autoware_utils::createMarkerScale(0.0, 0.0, 1.0), tier4_autoware_utils::createMarkerColor(1.0, 1.0, 1.0, 0.99)); - marker.pose = input_data_.current_odometry_ptr->pose.pose; + marker.pose = input_data.current_odometry.pose.pose; std::stringstream ss; - const double current = input_data_.current_steering_ptr->steering_tire_angle; - const double cmd = lateral_output_->control_cmd.steering_tire_angle; + const double current = input_data.current_steering.steering_tire_angle; + const double cmd = lat_out.control_cmd.steering_tire_angle; const double diff = current - cmd; ss << "current:" << current << " cmd:" << cmd << " diff:" << diff - << (lateral_output_->sync_data.is_steer_converged ? " (converged)" : " (not converged)"); + << (lat_out.sync_data.is_steer_converged ? " (converged)" : " (not converged)"); marker.text = ss.str(); debug_marker_array.markers.push_back(marker); diff --git a/control/trajectory_follower_nodes/test/test_controller_node.cpp b/control/trajectory_follower_nodes/test/test_controller_node.cpp index 8ea882d64aad1..205c4ea9e8c2d 100644 --- a/control/trajectory_follower_nodes/test/test_controller_node.cpp +++ b/control/trajectory_follower_nodes/test/test_controller_node.cpp @@ -45,7 +45,7 @@ using FakeNodeFixture = autoware::tools::testing::FakeTestNode; const rclcpp::Duration one_second(1, 0); -std::shared_ptr makeNode() +rclcpp::NodeOptions makeNodeOptions(const bool enable_keep_stopped_until_steer_convergence = false) { // Pass default parameter file to the node const auto share_dir = ament_index_cpp::get_package_share_directory("trajectory_follower_nodes"); @@ -53,12 +53,19 @@ std::shared_ptr makeNode() node_options.append_parameter_override("ctrl_period", 0.03); node_options.append_parameter_override("timeout_thr_sec", 0.5); node_options.append_parameter_override( - "enable_keep_stopped_until_steer_convergence", false); // longitudinal + "enable_keep_stopped_until_steer_convergence", + enable_keep_stopped_until_steer_convergence); // longitudinal node_options.arguments( {"--ros-args", "--params-file", share_dir + "/param/lateral_controller_defaults.param.yaml", "--params-file", share_dir + "/param/longitudinal_controller_defaults.param.yaml", "--params-file", share_dir + "/param/test_vehicle_info.param.yaml", "--params-file", share_dir + "/param/test_nearest_search.param.yaml"}); + + return node_options; +} + +std::shared_ptr makeNode(const rclcpp::NodeOptions & node_options) +{ std::shared_ptr node = std::make_shared(node_options); // Enable all logging in the node @@ -73,11 +80,14 @@ std::shared_ptr makeNode() class ControllerTester { public: - explicit ControllerTester(FakeNodeFixture * _fnf) : fnf(_fnf) {} - - std::shared_ptr node = makeNode(); + explicit ControllerTester(FakeNodeFixture * _fnf, const rclcpp::NodeOptions & node_options) + : fnf(_fnf), node(makeNode(node_options)) + { + } FakeNodeFixture * fnf; + std::shared_ptr node; + AckermannControlCommand::SharedPtr cmd_msg; bool received_control_command = false; @@ -172,7 +182,8 @@ TrajectoryPoint make_traj_point(const double px, const double py, const float vx TEST_F(FakeNodeFixture, no_input) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); // No published data: expect a stopped command test_utils::waitForMessage( @@ -182,7 +193,8 @@ TEST_F(FakeNodeFixture, no_input) TEST_F(FakeNodeFixture, empty_trajectory) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); @@ -200,7 +212,8 @@ TEST_F(FakeNodeFixture, empty_trajectory) // lateral TEST_F(FakeNodeFixture, straight_trajectory) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -225,7 +238,8 @@ TEST_F(FakeNodeFixture, straight_trajectory) TEST_F(FakeNodeFixture, right_turn) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -251,7 +265,8 @@ TEST_F(FakeNodeFixture, right_turn) TEST_F(FakeNodeFixture, left_turn) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -277,7 +292,8 @@ TEST_F(FakeNodeFixture, left_turn) TEST_F(FakeNodeFixture, stopped) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_odom(); @@ -306,7 +322,8 @@ TEST_F(FakeNodeFixture, stopped) // longitudinal TEST_F(FakeNodeFixture, longitudinal_keep_velocity) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_odom_vx(1.0); @@ -338,7 +355,8 @@ TEST_F(FakeNodeFixture, longitudinal_keep_velocity) TEST_F(FakeNodeFixture, longitudinal_slow_down) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_acc(); @@ -372,7 +390,8 @@ TEST_F(FakeNodeFixture, longitudinal_slow_down) TEST_F(FakeNodeFixture, longitudinal_accelerate) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_steer(); @@ -406,7 +425,8 @@ TEST_F(FakeNodeFixture, longitudinal_accelerate) TEST_F(FakeNodeFixture, longitudinal_stopped) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_odom(); @@ -433,7 +453,8 @@ TEST_F(FakeNodeFixture, longitudinal_stopped) TEST_F(FakeNodeFixture, longitudinal_reverse) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); @@ -459,7 +480,8 @@ TEST_F(FakeNodeFixture, longitudinal_reverse) TEST_F(FakeNodeFixture, longitudinal_emergency) { - ControllerTester tester(this); + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); tester.send_default_transform(); tester.publish_default_odom(); @@ -482,3 +504,62 @@ TEST_F(FakeNodeFixture, longitudinal_emergency) EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); EXPECT_LT(tester.cmd_msg->longitudinal.acceleration, 0.0f); } + +TEST_F(FakeNodeFixture, longitudinal_not_check_steer_converged) +{ + const auto node_options = makeNodeOptions(); + ControllerTester tester(this, node_options); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_acc(); + + // steering_tire_angle has to be larger than the threshold to check convergence. + const double steering_tire_angle = -0.5; + tester.publish_steer_angle(steering_tire_angle); + + // Publish trajectory starting away from the current ego pose + Trajectory traj; + traj.header.stamp = tester.node->now(); + traj.header.frame_id = "map"; + traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + // Not keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 1.0f); +} + +TEST_F(FakeNodeFixture, longitudinal_check_steer_converged) +{ + // set enable_keep_stopped_until_steer_convergence true + const auto node_options = makeNodeOptions(true); + ControllerTester tester(this, node_options); + + tester.send_default_transform(); + tester.publish_default_odom(); + tester.publish_default_acc(); + + // steering_tire_angle has to be larger than the threshold to check convergence. + const double steering_tire_angle = -0.5; + tester.publish_steer_angle(steering_tire_angle); + + // Publish trajectory starting away from the current ego pose + Trajectory traj; + traj.header.stamp = tester.node->now(); + traj.header.frame_id = "map"; + traj.points.push_back(make_traj_point(0.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(50.0, 0.0, 1.0f)); + traj.points.push_back(make_traj_point(100.0, 0.0, 1.0f)); + tester.traj_pub->publish(traj); + + test_utils::waitForMessage(tester.node, this, tester.received_control_command); + + ASSERT_TRUE(tester.received_control_command); + // Keep stopped state when the lateral control is not converged. + EXPECT_DOUBLE_EQ(tester.cmd_msg->longitudinal.speed, 0.0f); +}