diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp index b342aace69bde..e9c69a2b2896d 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/controller_node.hpp @@ -21,6 +21,7 @@ #include "tf2_ros/buffer.h" #include "tf2_ros/transform_listener.h" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include "tier4_autoware_utils/system/stop_watch.hpp" #include "trajectory_follower_base/lateral_controller_base.hpp" #include "trajectory_follower_base/longitudinal_controller_base.hpp" @@ -75,21 +76,34 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node std::shared_ptr longitudinal_controller_; std::shared_ptr lateral_controller_; - rclcpp::Subscription::SharedPtr sub_ref_path_; - rclcpp::Subscription::SharedPtr sub_odometry_; - rclcpp::Subscription::SharedPtr sub_steering_; - rclcpp::Subscription::SharedPtr sub_accel_; - rclcpp::Subscription::SharedPtr sub_operation_mode_; + // Subscribers + tier4_autoware_utils::InterProcessPollingSubscriber + sub_ref_path_{this, "~/input/reference_trajectory"}; + + tier4_autoware_utils::InterProcessPollingSubscriber sub_odometry_{ + this, "~/input/current_odometry"}; + + tier4_autoware_utils::InterProcessPollingSubscriber + sub_steering_{this, "~/input/current_steering"}; + + tier4_autoware_utils::InterProcessPollingSubscriber< + geometry_msgs::msg::AccelWithCovarianceStamped> + sub_accel_{this, "~/input/current_accel"}; + + tier4_autoware_utils::InterProcessPollingSubscriber sub_operation_mode_{ + this, "~/input/current_operation_mode"}; + + // Publishers rclcpp::Publisher::SharedPtr control_cmd_pub_; rclcpp::Publisher::SharedPtr pub_processing_time_lat_ms_; rclcpp::Publisher::SharedPtr pub_processing_time_lon_ms_; rclcpp::Publisher::SharedPtr debug_marker_pub_; - autoware_planning_msgs::msg::Trajectory::SharedPtr current_trajectory_ptr_; - nav_msgs::msg::Odometry::SharedPtr current_odometry_ptr_; - autoware_vehicle_msgs::msg::SteeringReport::SharedPtr current_steering_ptr_; - geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr current_accel_ptr_; - OperationModeState::SharedPtr current_operation_mode_ptr_; + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr current_trajectory_ptr_; + nav_msgs::msg::Odometry::ConstSharedPtr current_odometry_ptr_; + autoware_vehicle_msgs::msg::SteeringReport::ConstSharedPtr current_steering_ptr_; + geometry_msgs::msg::AccelWithCovarianceStamped::ConstSharedPtr current_accel_ptr_; + OperationModeState::ConstSharedPtr current_operation_mode_ptr_; enum class LateralControllerMode { INVALID = 0, @@ -104,12 +118,9 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node /** * @brief compute control command, and publish periodically */ - boost::optional createInputData(rclcpp::Clock & clock) const; + boost::optional createInputData(rclcpp::Clock & clock); void callbackTimerControl(); - void onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr); - void onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg); - void onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg); - void onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg); + bool processData(rclcpp::Clock & clock); bool isTimeOut(const LongitudinalOutput & lon_out, const LateralOutput & lat_out); LateralControllerMode getLateralControllerMode(const std::string & algorithm_name) const; LongitudinalControllerMode getLongitudinalControllerMode( @@ -125,6 +136,8 @@ class TRAJECTORY_FOLLOWER_PUBLIC Controller : public rclcpp::Node void publishProcessingTime( const double t_ms, const rclcpp::Publisher::SharedPtr pub); StopWatch stop_watch_; + + static constexpr double logger_throttle_interval = 5000; }; } // namespace trajectory_follower_node } // namespace autoware::motion::control diff --git a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp index e744243969cab..9360f900bca5f 100644 --- a/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp +++ b/control/trajectory_follower_node/include/trajectory_follower_node/simple_trajectory_follower.hpp @@ -15,6 +15,8 @@ #ifndef TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ #define TRAJECTORY_FOLLOWER_NODE__SIMPLE_TRAJECTORY_FOLLOWER_HPP_ +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" + #include #include @@ -42,20 +44,22 @@ class SimpleTrajectoryFollower : public rclcpp::Node ~SimpleTrajectoryFollower() = default; private: - rclcpp::Subscription::SharedPtr sub_kinematics_; - rclcpp::Subscription::SharedPtr sub_trajectory_; + tier4_autoware_utils::InterProcessPollingSubscriber sub_kinematics_{ + this, "~/input/kinematics"}; + tier4_autoware_utils::InterProcessPollingSubscriber sub_trajectory_{ + this, "~/input/trajectory"}; rclcpp::Publisher::SharedPtr pub_cmd_; rclcpp::TimerBase::SharedPtr timer_; - Trajectory::SharedPtr trajectory_; - Odometry::SharedPtr odometry_; + Trajectory::ConstSharedPtr trajectory_; + Odometry::ConstSharedPtr odometry_; TrajectoryPoint closest_traj_point_; bool use_external_target_vel_; double external_target_vel_; double lateral_deviation_; void onTimer(); - bool checkData(); + bool processData(); void updateClosest(); double calcSteerCmd(); double calcAccCmd(); diff --git a/control/trajectory_follower_node/src/controller_node.cpp b/control/trajectory_follower_node/src/controller_node.cpp index 4be04af5fd184..566a5ee7ed0df 100644 --- a/control/trajectory_follower_node/src/controller_node.cpp +++ b/control/trajectory_follower_node/src/controller_node.cpp @@ -62,17 +62,6 @@ Controller::Controller(const rclcpp::NodeOptions & node_options) : Node("control throw std::domain_error("[LongitudinalController] invalid algorithm"); } - sub_ref_path_ = create_subscription( - "~/input/reference_trajectory", rclcpp::QoS{1}, std::bind(&Controller::onTrajectory, this, _1)); - sub_steering_ = create_subscription( - "~/input/current_steering", rclcpp::QoS{1}, std::bind(&Controller::onSteering, this, _1)); - sub_odometry_ = create_subscription( - "~/input/current_odometry", rclcpp::QoS{1}, std::bind(&Controller::onOdometry, this, _1)); - sub_accel_ = create_subscription( - "~/input/current_accel", rclcpp::QoS{1}, std::bind(&Controller::onAccel, this, _1)); - sub_operation_mode_ = create_subscription( - "~/input/current_operation_mode", rclcpp::QoS{1}, - [this](const OperationModeState::SharedPtr msg) { current_operation_mode_ptr_ = msg; }); control_cmd_pub_ = create_publisher( "~/output/control_cmd", rclcpp::QoS{1}.transient_local()); pub_processing_time_lat_ms_ = @@ -112,24 +101,32 @@ Controller::LongitudinalControllerMode Controller::getLongitudinalControllerMode return LongitudinalControllerMode::INVALID; } -void Controller::onTrajectory(const autoware_planning_msgs::msg::Trajectory::SharedPtr msg) +bool Controller::processData(rclcpp::Clock & clock) { - current_trajectory_ptr_ = msg; -} - -void Controller::onOdometry(const nav_msgs::msg::Odometry::SharedPtr msg) -{ - current_odometry_ptr_ = msg; -} + bool is_ready = true; + + const auto & logData = [&clock, this](const std::string & data_type) { + std::string msg = "Waiting for " + data_type + " data"; + RCLCPP_INFO_THROTTLE(get_logger(), clock, logger_throttle_interval, msg.c_str()); + }; + + const auto & getData = [&logData](auto & dest, auto & sub, const std::string & data_type = "") { + const auto temp = sub.takeData(); + if (temp) { + dest = temp; + return true; + } + if (!data_type.empty()) logData(data_type); + return false; + }; -void Controller::onSteering(const autoware_vehicle_msgs::msg::SteeringReport::SharedPtr msg) -{ - current_steering_ptr_ = msg; -} + is_ready &= getData(current_accel_ptr_, sub_accel_, "acceleration"); + is_ready &= getData(current_steering_ptr_, sub_steering_, "steering"); + is_ready &= getData(current_trajectory_ptr_, sub_ref_path_, "trajectory"); + is_ready &= getData(current_odometry_ptr_, sub_odometry_, "odometry"); + is_ready &= getData(current_operation_mode_ptr_, sub_operation_mode_, "operation mode"); -void Controller::onAccel(const geometry_msgs::msg::AccelWithCovarianceStamped::SharedPtr msg) -{ - current_accel_ptr_ = msg; + return is_ready; } bool Controller::isTimeOut( @@ -152,31 +149,9 @@ bool Controller::isTimeOut( return false; } -boost::optional Controller::createInputData( - rclcpp::Clock & clock) const +boost::optional Controller::createInputData(rclcpp::Clock & clock) { - 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 {}; - } - - if (!current_operation_mode_ptr_) { - RCLCPP_INFO_THROTTLE(get_logger(), clock, 5000, "Waiting for current operation mode."); + if (!processData(clock)) { return {}; } diff --git a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp index 7c27eaed41380..bd24516ed033b 100644 --- a/control/trajectory_follower_node/src/simple_trajectory_follower.cpp +++ b/control/trajectory_follower_node/src/simple_trajectory_follower.cpp @@ -31,11 +31,6 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o { pub_cmd_ = create_publisher("output/control_cmd", 1); - sub_kinematics_ = create_subscription( - "input/kinematics", 1, [this](const Odometry::SharedPtr msg) { odometry_ = msg; }); - sub_trajectory_ = create_subscription( - "input/trajectory", 1, [this](const Trajectory::SharedPtr msg) { trajectory_ = msg; }); - use_external_target_vel_ = declare_parameter("use_external_target_vel"); external_target_vel_ = declare_parameter("external_target_vel"); lateral_deviation_ = declare_parameter("lateral_deviation"); @@ -47,7 +42,7 @@ SimpleTrajectoryFollower::SimpleTrajectoryFollower(const rclcpp::NodeOptions & o void SimpleTrajectoryFollower::onTimer() { - if (!checkData()) { + if (!processData()) { RCLCPP_INFO(get_logger(), "data not ready"); return; } @@ -110,9 +105,18 @@ double SimpleTrajectoryFollower::calcAccCmd() return acc; } -bool SimpleTrajectoryFollower::checkData() +bool SimpleTrajectoryFollower::processData() { - return (trajectory_ && odometry_); + bool is_ready = true; + const auto & getData = [](auto & dest, auto & sub) { + const auto temp = sub.takeData(); + if (!temp) return false; + dest = temp; + return true; + }; + is_ready &= getData(odometry_, sub_kinematics_); + is_ready &= getData(trajectory_, sub_trajectory_); + return is_ready; } } // namespace simple_trajectory_follower