Skip to content

Commit 41996f4

Browse files
Optional transient map saver (#2215)
* Added transient local subscription qos profile parameter to map saver (#1871) * Added transient local subscription qos profile parameter to map saver * Made transient local default true * Fixed linter problem * switched back house world to waffle model * Make transient map subscribe backwards compatible for foxy Co-authored-by: Michael Equi <[email protected]>
1 parent 64bf146 commit 41996f4

3 files changed

Lines changed: 13 additions & 1 deletion

File tree

nav2_bringup/bringup/params/nav2_params.yaml

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -243,6 +243,7 @@ map_saver:
243243
save_map_timeout: 5000
244244
free_thresh_default: 0.25
245245
occupied_thresh_default: 0.65
246+
map_subscribe_transient_local: False
246247

247248
planner_server:
248249
ros__parameters:

nav2_map_server/include/nav2_map_server/map_saver.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -103,6 +103,8 @@ class MapSaver : public nav2_util::LifecycleNode
103103
// Default values for map thresholds
104104
double free_thresh_default_;
105105
double occupied_thresh_default_;
106+
// param for handling QoS configuration
107+
bool map_subscribe_transient_local_;
106108

107109
// The name of the service for saving a map from topic
108110
const std::string save_map_service_name_{"save_map"};

nav2_map_server/src/map_saver/map_saver.cpp

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -50,6 +50,8 @@ MapSaver::MapSaver()
5050

5151
free_thresh_default_ = declare_parameter("free_thresh_default", 0.25),
5252
occupied_thresh_default_ = declare_parameter("occupied_thresh_default", 0.65);
53+
// false only of foxy for backwards compatibility
54+
map_subscribe_transient_local_ = declare_parameter("map_subscribe_transient_local", false);
5355
}
5456

5557
MapSaver::~MapSaver()
@@ -173,8 +175,15 @@ bool MapSaver::saveMapTopicToFile(
173175
// Add new subscription for incoming map topic.
174176
// Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode
175177
// as a map listener.
178+
rclcpp::QoS map_qos = rclcpp::SystemDefaultsQoS(); // initialize to default
179+
if (map_subscribe_transient_local_) {
180+
map_qos = rclcpp::QoS(10);
181+
map_qos.transient_local();
182+
map_qos.reliable();
183+
map_qos.keep_last(1);
184+
}
176185
auto map_sub = rclcpp_node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
177-
map_topic_loc, rclcpp::SystemDefaultsQoS(), mapCallback);
186+
map_topic_loc, map_qos, mapCallback);
178187

179188
rclcpp::Time start_time = now();
180189
while (rclcpp::ok()) {

0 commit comments

Comments
 (0)