diff --git a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp index 91f08b1bb50..d47c06235ad 100644 --- a/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp +++ b/nav2_dwb_controller/nav_2d_utils/include/nav_2d_utils/odom_subscriber.hpp @@ -42,8 +42,9 @@ #include "nav_2d_msgs/msg/twist2_d_stamped.hpp" #include "nav_msgs/msg/odometry.hpp" #include "nav2_util/lifecycle_node.hpp" -#include "rclcpp/rclcpp.hpp" #include "nav2_util/node_utils.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/subscription_options.hpp" namespace nav_2d_utils { @@ -70,11 +71,15 @@ class OdomSubscriber std::string odom_topic; nh->get_parameter_or("odom_topic", odom_topic, default_topic); + + rclcpp::SubscriptionOptions sub_options; + sub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); odom_sub_ = nh->create_subscription( odom_topic, rclcpp::SystemDefaultsQoS(), - std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1)); + std::bind(&OdomSubscriber::odomCallback, this, std::placeholders::_1), + sub_options); } inline nav_2d_msgs::msg::Twist2D getTwist() {return odom_vel_.velocity;} diff --git a/nav2_util/src/odometry_utils.cpp b/nav2_util/src/odometry_utils.cpp index 9bf585bdfe2..565df740730 100644 --- a/nav2_util/src/odometry_utils.cpp +++ b/nav2_util/src/odometry_utils.cpp @@ -16,6 +16,7 @@ #include #include "nav2_util/odometry_utils.hpp" +#include "rclcpp/subscription_options.hpp" using namespace std::chrono; // NOLINT using namespace std::chrono_literals; // NOLINT @@ -30,10 +31,14 @@ OdomSmoother::OdomSmoother( : odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { auto node = parent.lock(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); odom_sub_ = node->create_subscription( odom_topic, rclcpp::SystemDefaultsQoS(), - std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1)); + std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1), + sub_options); odom_cumulate_.twist.twist.linear.x = 0; odom_cumulate_.twist.twist.linear.y = 0; @@ -50,10 +55,14 @@ OdomSmoother::OdomSmoother( : odom_history_duration_(rclcpp::Duration::from_seconds(filter_duration)) { auto node = parent.lock(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); odom_sub_ = node->create_subscription( odom_topic, rclcpp::SystemDefaultsQoS(), - std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1)); + std::bind(&OdomSmoother::odomCallback, this, std::placeholders::_1), + sub_options); odom_cumulate_.twist.twist.linear.x = 0; odom_cumulate_.twist.twist.linear.y = 0;