Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Reduce map saver nodes #2454

Merged
merged 3 commits into from
Jul 21, 2021
Merged

Reduce map saver nodes #2454

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
2921270
reduce MapSaver nodes by using callback group/executor combo
gezp Jul 16, 2021
972f6ea
set use_rclcpp_node false
gezp Jul 16, 2021
08cbde0
a cleaner solution using a future and spin_until_future_complete()
gezp Jul 20, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
74 changes: 36 additions & 38 deletions nav2_map_server/src/map_saver/map_saver.cpp
View file
Open in desktop
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