Skip to content

Commit

Permalink
Reduce map saver nodes (#2454)
Browse files Browse the repository at this point in the history
* reduce MapSaver nodes by using callback group/executor combo

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>

* set use_rclcpp_node false

* a cleaner solution using a future and spin_until_future_complete()

Signed-off-by: zhenpeng ge <zhenpeng.ge@qq.com>
  • Loading branch information
gezp authored and SteveMacenski committed Sep 14, 2021
1 parent 0534487 commit c1b506e
Showing 1 changed file with 36 additions and 38 deletions.
74 changes: 36 additions & 38 deletions nav2_map_server/src/map_saver/map_saver.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -42,7 +42,7 @@ using namespace std::placeholders;
namespace nav2_map_server
{
MapSaver::MapSaver()
: nav2_util::LifecycleNode("map_saver", "", true)
: nav2_util::LifecycleNode("map_saver", "")
{
RCLCPP_INFO(get_logger(), "Creating");

Expand Down Expand Up @@ -149,12 +149,6 @@ bool MapSaver::saveMapTopicToFile(
map_topic_loc.c_str(), save_parameters_loc.map_file_name.c_str());

try {
// Pointer to map message received in the subscription callback
nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = nullptr;

// Mutex for handling map_msg shared resource
std::recursive_mutex access;

// Correct map_topic_loc if necessary
if (map_topic_loc == "") {
map_topic_loc = "map";
Expand All @@ -179,47 +173,51 @@ bool MapSaver::saveMapTopicToFile(
save_parameters_loc.occupied_thresh = occupied_thresh_default_;
}

std::promise<nav_msgs::msg::OccupancyGrid::SharedPtr> prom;
std::future<nav_msgs::msg::OccupancyGrid::SharedPtr> future_result = prom.get_future();
// A callback function that receives map message from subscribed topic
auto mapCallback = [&map_msg, &access](
auto mapCallback = [&prom](
const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void {
std::lock_guard<std::recursive_mutex> guard(access);
map_msg = msg;
prom.set_value(msg);
};

// Add new subscription for incoming map topic.
// Utilizing local rclcpp::Node (rclcpp_node_) from nav2_util::LifecycleNode
// as a map listener.
rclcpp::QoS map_qos(10); // initialize to default
if (map_subscribe_transient_local_) {
map_qos.transient_local();
map_qos.reliable();
map_qos.keep_last(1);
}
auto map_sub = rclcpp_node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic_loc, map_qos, mapCallback);

rclcpp::Time start_time = now();
while (rclcpp::ok()) {
if ((now() - start_time) > *save_map_timeout_) {
RCLCPP_ERROR(get_logger(), "Failed to save the map: timeout");
return false;
}

if (map_msg) {
std::lock_guard<std::recursive_mutex> guard(access);
// map_sub is no more needed
map_sub.reset();
// Map message received. Saving it to file
if (saveMapToFile(*map_msg, save_parameters_loc)) {
RCLCPP_INFO(get_logger(), "Map saved successfully");
return true;
} else {
RCLCPP_ERROR(get_logger(), "Failed to save the map");
return false;
}
}

rclcpp::sleep_for(std::chrono::milliseconds(100));

// Create new CallbackGroup for map_sub
auto callback_group = create_callback_group(
rclcpp::CallbackGroupType::MutuallyExclusive,
false);

auto option = rclcpp::SubscriptionOptions();
option.callback_group = callback_group;
auto map_sub = create_subscription<nav_msgs::msg::OccupancyGrid>(
map_topic_loc, map_qos, mapCallback, option);

// Create SingleThreadedExecutor to spin map_sub in callback_group
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_callback_group(callback_group, get_node_base_interface());
// Spin until map message received
auto timeout = save_map_timeout_->to_chrono<std::chrono::nanoseconds>();
auto status = executor.spin_until_future_complete(future_result, timeout);
if (status != rclcpp::FutureReturnCode::SUCCESS) {
RCLCPP_ERROR(get_logger(), "Failed to spin map subscription");
return false;
}
// map_sub is no more needed
map_sub.reset();
// Map message received. Saving it to file
nav_msgs::msg::OccupancyGrid::SharedPtr map_msg = future_result.get();
if (saveMapToFile(*map_msg, save_parameters_loc)) {
RCLCPP_INFO(get_logger(), "Map saved successfully");
return true;
} else {
RCLCPP_ERROR(get_logger(), "Failed to save the map");
return false;
}
} catch (std::exception & e) {
RCLCPP_ERROR(get_logger(), "Failed to save the map: %s", e.what());
Expand Down

0 comments on commit c1b506e

Please sign in to comment.