Fix Map Saver and testing: (#2084)

AlexeyMerzlyakov · web-flow · commit 13e01d2d6bcd · 2020-11-10T10:48:11.000-08:00
- Add Map Saver resource sharing
- Add forgotten reset of save_map_service_
- Remove unnecessary map republisher
- Increase save_map_timeout
diff --git a/nav2_map_server/src/map_saver/map_saver.cpp b/nav2_map_server/src/map_saver/map_saver.cpp
@@ -35,6 +35,7 @@
 #include <memory>
 #include <stdexcept>
 #include <functional>
+#include <mutex>
 
 using namespace std::placeholders;
 
@@ -99,6 +100,9 @@ nav2_util::CallbackReturn
 MapSaver::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  save_map_service_.reset();
+
   return nav2_util::CallbackReturn::SUCCESS;
 }
 
@@ -148,6 +152,9 @@ bool MapSaver::saveMapTopicToFile(
     // 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";
@@ -173,8 +180,9 @@ bool MapSaver::saveMapTopicToFile(
     }
 
     // A callback function that receives map message from subscribed topic
-    auto mapCallback = [&map_msg](
+    auto mapCallback = [&map_msg, &access](
       const nav_msgs::msg::OccupancyGrid::SharedPtr msg) -> void {
+        std::lock_guard<std::recursive_mutex> guard(access);
         map_msg = msg;
       };
 
@@ -198,6 +206,9 @@ bool MapSaver::saveMapTopicToFile(
       }
 
       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");
diff --git a/nav2_map_server/test/component/test_map_saver_publisher.cpp b/nav2_map_server/test/component/test_map_saver_publisher.cpp
@@ -15,15 +15,13 @@
 #include <experimental/filesystem>
 #include <string>
 #include <memory>
-#include <chrono>
 
 #include "rclcpp/rclcpp.hpp"
 #include "nav2_map_server/map_io.hpp"
 #include "test_constants/test_constants.h"
 
 #define TEST_DIR TEST_DIRECTORY
 
-using namespace std::chrono_literals;
 using namespace nav2_map_server;  // NOLINT
 using std::experimental::filesystem::path;
 
@@ -34,7 +32,8 @@ class TestPublisher : public rclcpp::Node
   : Node("map_publisher")
   {
     std::string pub_map_file = path(TEST_DIR) / path(g_valid_yaml_file);
-    LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg_);
+    nav_msgs::msg::OccupancyGrid msg;
+    LOAD_MAP_STATUS status = loadMapFromYaml(pub_map_file, msg);
     if (status != LOAD_MAP_SUCCESS) {
       RCLCPP_ERROR(get_logger(), "Can not load %s map file", pub_map_file);
       return;
@@ -43,19 +42,11 @@ class TestPublisher : public rclcpp::Node
     map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
       "map",
       rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
-
-    timer_ = create_wall_timer(300ms, std::bind(&TestPublisher::mapPublishCallback, this));
+    map_pub_->publish(msg);
   }
 
 protected:
-  void mapPublishCallback()
-  {
-    map_pub_->publish(msg_);
-  }
-
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
-  rclcpp::TimerBase::SharedPtr timer_;
-  nav_msgs::msg::OccupancyGrid msg_;
 };
 
 int main(int argc, char ** argv)
diff --git a/nav2_map_server/test/map_saver_params.yaml b/nav2_map_server/test/map_saver_params.yaml
@@ -1,5 +1,5 @@
 map_saver:
     ros__parameters:
-        save_map_timeout: 1000
+        save_map_timeout: 5000
         free_thresh_default: 0.196
         occupied_thresh_default: 0.65
