diff --git a/include/robot_localization/ros_robot_localization_listener.hpp b/include/robot_localization/ros_robot_localization_listener.hpp index 333c53800..4f50d4a32 100644 --- a/include/robot_localization/ros_robot_localization_listener.hpp +++ b/include/robot_localization/ros_robot_localization_listener.hpp @@ -49,18 +49,6 @@ namespace robot_localization { -namespace detail -{ -rclcpp::SubscriptionOptions -get_subscription_options_with_default_qos_override_policies() -{ - auto subscription_options = rclcpp::SubscriptionOptions(); - subscription_options.qos_overriding_options = - rclcpp::QosOverridingOptions::with_default_policies(); - return subscription_options; -} -} // namespace detail - //! @brief RosRobotLocalizationListener class //! //! This class wraps the RobotLocalizationEstimator. It listens to topics over @@ -82,10 +70,7 @@ class RosRobotLocalizationListener //! //! @param[in] node - rclcpp node shared pointer //! - explicit RosRobotLocalizationListener( - rclcpp::Node::SharedPtr node, - rclcpp::SubscriptionOptions options = - detail::get_subscription_options_with_default_qos_override_policies()); + explicit RosRobotLocalizationListener(rclcpp::Node::SharedPtr node); //! @brief Destructor //! diff --git a/src/navsat_transform.cpp b/src/navsat_transform.cpp index 74be08f0a..1df5c1f18 100644 --- a/src/navsat_transform.cpp +++ b/src/navsat_transform.cpp @@ -170,32 +170,23 @@ NavSatTransform::NavSatTransform(const rclcpp::NodeOptions & options) auto custom_qos = rclcpp::SensorDataQoS(rclcpp::KeepLast(1)); - auto subscriber_options = rclcpp::SubscriptionOptions(); - subscriber_options.qos_overriding_options = - rclcpp::QosOverridingOptions::with_default_policies(); odom_sub_ = this->create_subscription( - "odometry/filtered", custom_qos, std::bind( - &NavSatTransform::odomCallback, this, _1), subscriber_options); + "odometry/filtered", custom_qos, std::bind(&NavSatTransform::odomCallback, this, _1)); gps_sub_ = this->create_subscription( - "gps/fix", custom_qos, std::bind(&NavSatTransform::gpsFixCallback, this, _1), - subscriber_options); + "gps/fix", custom_qos, std::bind(&NavSatTransform::gpsFixCallback, this, _1)); if (!use_odometry_yaw_ && !use_manual_datum_) { imu_sub_ = this->create_subscription( - "imu", custom_qos, std::bind(&NavSatTransform::imuCallback, this, _1), subscriber_options); + "imu", custom_qos, std::bind(&NavSatTransform::imuCallback, this, _1)); } - rclcpp::PublisherOptions publisher_options; - publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); gps_odom_pub_ = - this->create_publisher( - "odometry/gps", rclcpp::QoS(10), publisher_options); + this->create_publisher("odometry/gps", rclcpp::QoS(10)); if (publish_gps_) { filtered_gps_pub_ = - this->create_publisher( - "gps/filtered", rclcpp::QoS(10), publisher_options); + this->create_publisher("gps/filtered", rclcpp::QoS(10)); } // Sleep for the parameterized amount of time, to give diff --git a/src/ros_filter.cpp b/src/ros_filter.cpp index 58a73393b..13fa46344 100644 --- a/src/ros_filter.cpp +++ b/src/ros_filter.cpp @@ -1970,17 +1970,14 @@ void RosFilter::initialize() tf2::toMsg(tf2::Transform::getIdentity()); // Position publisher - rclcpp::PublisherOptions publisher_options; - publisher_options.qos_overriding_options = rclcpp::QosOverridingOptions::with_default_policies(); position_pub_ = - this->create_publisher( - "odometry/filtered", rclcpp::QoS(10), publisher_options); + this->create_publisher("odometry/filtered", rclcpp::QoS(10)); // Optional acceleration publisher if (publish_acceleration_) { accel_pub_ = this->create_publisher( - "accel/filtered", rclcpp::QoS(10), publisher_options); + "accel/filtered", rclcpp::QoS(10)); } const std::chrono::duration timespan{1.0 / frequency_}; diff --git a/src/ros_robot_localization_listener.cpp b/src/ros_robot_localization_listener.cpp index a6faeae32..da6917fcb 100644 --- a/src/ros_robot_localization_listener.cpp +++ b/src/ros_robot_localization_listener.cpp @@ -75,12 +75,11 @@ FilterTypes::FilterType filterTypeFromString( } RosRobotLocalizationListener::RosRobotLocalizationListener( - rclcpp::Node::SharedPtr node, - rclcpp::SubscriptionOptions options) + rclcpp::Node::SharedPtr node) : qos1_(1), qos10_(10), - odom_sub_(node, "odom/filtered", qos1_.get_rmw_qos_profile(), options), - accel_sub_(node, "acceleration/filtered", qos1_.get_rmw_qos_profile(), options), + odom_sub_(node, "odom/filtered", qos1_.get_rmw_qos_profile()), + accel_sub_(node, "acceleration/filtered", qos1_.get_rmw_qos_profile()), sync_(odom_sub_, accel_sub_, 10u), node_clock_(node->get_node_clock_interface()->get_clock()), node_logger_(node->get_node_logging_interface()),