diff --git a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp index e0bdb326adb33..bc566fdfb96bf 100644 --- a/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp +++ b/planning/path_smoother/include/path_smoother/elastic_band_smoother.hpp @@ -22,6 +22,7 @@ #include "path_smoother/type_alias.hpp" #include "rclcpp/rclcpp.hpp" #include "tier4_autoware_utils/ros/logger_level_configure.hpp" +#include "tier4_autoware_utils/ros/polling_subscriber.hpp" #include @@ -72,9 +73,6 @@ class ElasticBandSmoother : public rclcpp::Node CommonParam common_param_{}; EgoNearestParam ego_nearest_param_{}; - // variables for subscribers - Odometry::ConstSharedPtr ego_state_ptr_; - // variables for previous information std::shared_ptr> prev_optimized_traj_points_ptr_; @@ -84,7 +82,7 @@ class ElasticBandSmoother : public rclcpp::Node // interface subscriber rclcpp::Subscription::SharedPtr path_sub_; - rclcpp::Subscription::SharedPtr odom_sub_; + tier4_autoware_utils::InterProcessPollingSubscriber odom_sub_{this, "~/input/odometry"}; // debug publisher rclcpp::Publisher::SharedPtr debug_extended_traj_pub_; @@ -104,7 +102,8 @@ class ElasticBandSmoother : public rclcpp::Node void resetPreviousData(); // main functions - bool isDataReady(const Path & path, rclcpp::Clock clock) const; + bool isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const; void applyInputVelocity( std::vector & output_traj_points, const std::vector & input_traj_points, diff --git a/planning/path_smoother/src/elastic_band_smoother.cpp b/planning/path_smoother/src/elastic_band_smoother.cpp index 6fb732309da4b..75300286ac9dc 100644 --- a/planning/path_smoother/src/elastic_band_smoother.cpp +++ b/planning/path_smoother/src/elastic_band_smoother.cpp @@ -79,8 +79,6 @@ ElasticBandSmoother::ElasticBandSmoother(const rclcpp::NodeOptions & node_option // interface subscriber path_sub_ = create_subscription( "~/input/path", 1, std::bind(&ElasticBandSmoother::onPath, this, std::placeholders::_1)); - odom_sub_ = create_subscription( - "~/input/odometry", 1, [this](const Odometry::ConstSharedPtr msg) { ego_state_ptr_ = msg; }); // debug publisher debug_extended_traj_pub_ = create_publisher("~/debug/extended_traj", 1); @@ -156,7 +154,8 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) time_keeper_ptr_->tic(__func__); // check if data is ready and valid - if (!isDataReady(*path_ptr, *get_clock())) { + const auto ego_state_ptr = odom_sub_.takeData(); + if (!isDataReady(*path_ptr, ego_state_ptr, *get_clock())) { return; } @@ -181,7 +180,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) // 1. calculate trajectory with Elastic Band // 1.a check if replan (= optimization) is required PlannerData planner_data( - input_traj_points, ego_state_ptr_->pose.pose, ego_state_ptr_->twist.twist.linear.x); + input_traj_points, ego_state_ptr->pose.pose, ego_state_ptr->twist.twist.linear.x); const bool is_replan_required = [&]() { if (replan_checker_ptr_->isResetRequired(planner_data)) { // NOTE: always replan when resetting previous optimization @@ -195,7 +194,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) replan_checker_ptr_->updateData(planner_data, is_replan_required, now()); time_keeper_ptr_->tic(__func__); auto smoothed_traj_points = is_replan_required ? eb_path_smoother_ptr_->smoothTrajectory( - input_traj_points, ego_state_ptr_->pose.pose) + input_traj_points, ego_state_ptr->pose.pose) : *prev_optimized_traj_points_ptr_; time_keeper_ptr_->toc(__func__, " "); @@ -203,7 +202,7 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) std::make_shared>(smoothed_traj_points); // 2. update velocity - applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr_->pose.pose); + applyInputVelocity(smoothed_traj_points, input_traj_points, ego_state_ptr->pose.pose); // 3. extend trajectory to connect the optimized trajectory and the following path smoothly auto full_traj_points = extendTrajectory(input_traj_points, smoothed_traj_points); @@ -230,9 +229,10 @@ void ElasticBandSmoother::onPath(const Path::ConstSharedPtr path_ptr) published_time_publisher_->publish_if_subscribed(path_pub_, path_ptr->header.stamp); } -bool ElasticBandSmoother::isDataReady(const Path & path, rclcpp::Clock clock) const +bool ElasticBandSmoother::isDataReady( + const Path & path, const Odometry::ConstSharedPtr ego_state_ptr, rclcpp::Clock clock) const { - if (!ego_state_ptr_) { + if (!ego_state_ptr) { RCLCPP_INFO_SKIPFIRST_THROTTLE(get_logger(), clock, 5000, "Waiting for ego pose and twist."); return false; }