diff --git a/nav2_amcl/src/amcl_node.cpp b/nav2_amcl/src/amcl_node.cpp index e766224ac87..8076af72ac2 100644 --- a/nav2_amcl/src/amcl_node.cpp +++ b/nav2_amcl/src/amcl_node.cpp @@ -1217,7 +1217,7 @@ AmclNode::updateParametersCallback( map_sub_ = create_subscription( map_topic_, std::bind(&AmclNode::mapReceived, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS()); + nav2::qos::LatchedSubscriptionQoS(3)); } } @@ -1389,7 +1389,7 @@ AmclNode::initPubSub() map_sub_ = create_subscription( map_topic_, std::bind(&AmclNode::mapReceived, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS()); + nav2::qos::LatchedSubscriptionQoS(3)); RCLCPP_INFO(get_logger(), "Subscribed to map topic."); } diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp index c817125f50d..b2ad27ebadf 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_subscriber.hpp @@ -53,7 +53,7 @@ class CostmapSubscriber costmap_sub_ = nav2::interfaces::create_subscription( parent, topic_name_, std::bind(&CostmapSubscriber::costmapCallback, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS()); + nav2::qos::LatchedSubscriptionQoS(3)); costmap_update_sub_ = nav2::interfaces::create_subscription( parent, topic_name_ + "_updates", diff --git a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp index 8041c9d3a50..dc83064c116 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/binary_filter.cpp @@ -144,7 +144,7 @@ void BinaryFilter::filterInfoCallback( mask_sub_ = node->create_subscription( mask_topic_, std::bind(&BinaryFilter::maskCallback, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS()); + nav2::qos::LatchedSubscriptionQoS(3)); } void BinaryFilter::maskCallback( diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index cef37c8a17c..643fda4c9de 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -133,7 +133,7 @@ void KeepoutFilter::filterInfoCallback( mask_sub_ = node->create_subscription( mask_topic_, std::bind(&KeepoutFilter::maskCallback, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS()); + nav2::qos::LatchedSubscriptionQoS(3)); } void KeepoutFilter::maskCallback( diff --git a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp index 1d2f54d43c8..4dcfc92ca89 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/speed_filter.cpp @@ -152,7 +152,7 @@ void SpeedFilter::filterInfoCallback( mask_sub_ = node->create_subscription( mask_topic_, std::bind(&SpeedFilter::maskCallback, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS()); + nav2::qos::LatchedSubscriptionQoS(3)); } void SpeedFilter::maskCallback( diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index 8ba7fc623db..13f20ef3b07 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -77,7 +77,7 @@ StaticLayer::onInitialize() rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS(); // initialize to default if (map_subscribe_transient_local_) { - map_qos = nav2::qos::LatchedSubscriptionQoS(); + map_qos = nav2::qos::LatchedSubscriptionQoS(3); } RCLCPP_INFO( diff --git a/nav2_costmap_2d/plugins/voxel_layer.cpp b/nav2_costmap_2d/plugins/voxel_layer.cpp index 4a2b17e1f14..c90e7781cce 100644 --- a/nav2_costmap_2d/plugins/voxel_layer.cpp +++ b/nav2_costmap_2d/plugins/voxel_layer.cpp @@ -96,7 +96,7 @@ void VoxelLayer::onInitialize() if (publish_voxel_) { voxel_pub_ = node->create_publisher( - "voxel_grid", nav2::qos::LatchedPublisherQoS()); + "voxel_grid", nav2::qos::LatchedPublisherQoS(1)); voxel_pub_->on_activate(); } diff --git a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp index 5252ea70547..4da1c7beaf0 100644 --- a/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp +++ b/nav2_costmap_2d/test/integration/test_costmap_2d_publisher.cpp @@ -87,7 +87,7 @@ class LayerSubscriber layer_sub_ = node->create_subscription( topic_name, std::bind(&LayerSubscriber::layerCallback, this, std::placeholders::_1), - nav2::qos::LatchedSubscriptionQoS(), + nav2::qos::LatchedSubscriptionQoS(3), callback_group_); executor_ = std::make_shared(); diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp index 1a54ac77a1c..60a7959be4b 100644 --- a/nav2_map_server/src/map_saver/map_saver.cpp +++ b/nav2_map_server/src/map_saver/map_saver.cpp @@ -188,7 +188,7 @@ bool MapSaver::saveMapTopicToFile( rclcpp::QoS map_qos = nav2::qos::StandardTopicQoS(); // initialize to default if (map_subscribe_transient_local_) { - map_qos = nav2::qos::LatchedSubscriptionQoS(); + map_qos = nav2::qos::LatchedSubscriptionQoS(3); } // Create new CallbackGroup for map_sub diff --git a/nav2_map_server/test/component/test_map_saver_publisher.cpp b/nav2_map_server/test/component/test_map_saver_publisher.cpp index 9ae644c3f7a..1fc758ab2c5 100644 --- a/nav2_map_server/test/component/test_map_saver_publisher.cpp +++ b/nav2_map_server/test/component/test_map_saver_publisher.cpp @@ -16,6 +16,7 @@ #include #include +#include "nav2_ros_common/qos_profiles.hpp" #include "rclcpp/rclcpp.hpp" #include "nav2_map_server/map_io.hpp" #include "test_constants/test_constants.h" @@ -41,7 +42,7 @@ class TestPublisher : public rclcpp::Node map_pub_ = create_publisher( "map", - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + nav2::qos::LatchedPublisherQoS()); map_pub_->publish(msg); } diff --git a/nav2_map_server/test/map_saver_cli/CMakeLists.txt b/nav2_map_server/test/map_saver_cli/CMakeLists.txt index 3731e541ac5..ca4eb15f4b3 100644 --- a/nav2_map_server/test/map_saver_cli/CMakeLists.txt +++ b/nav2_map_server/test/map_saver_cli/CMakeLists.txt @@ -6,4 +6,5 @@ ament_add_gtest(test_map_saver_cli target_link_libraries(test_map_saver_cli rclcpp::rclcpp ${nav_msgs_TARGETS} + nav2_ros_common::nav2_ros_common ) diff --git a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp index 75d6651300e..ad43a75ac67 100644 --- a/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp +++ b/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp @@ -18,6 +18,7 @@ #include #include +#include "nav2_ros_common/qos_profiles.hpp" #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/occupancy_grid.hpp" @@ -34,7 +35,7 @@ TEST(MapSaverCLI, CLITest) auto publisher = node->create_publisher( "/map", - rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable()); + nav2::qos::LatchedPublisherQoS()); auto msg = std::make_unique(); msg->header.frame_id = "map";