Map server params update and refactoring (#265)

* Simplify some of the map server code
* Use a YAML file to configure the map server. This allows the map server to be used as a standalone node or composed with other nodes into a single executable. Also, update the costmap_world_model to make sure its got the latest map
from the map_server.
* Rename the factory method to be createMapServer since that's what it's creating
* Fixes for cpplint/uncrustify
* Continue to simplify map server
* Introduce a MapLoader that is used by the MapServer
* Get the tests working again
* Change the default resolution to what it was before
* Add some comments
* Update README for the map server to be consistent with recent code changes
* Remove unneeded param from YAML file
* Start a system YAML file seeded with the map server params
* Remove obsolete dependency on yaml_cpp_vendor package
* Reintroduce the factory pattern. Each instance of the map server will load a single map of a particular type. Based on the incoming map type parameter, the map server will create the
corresponding map loader to load the file.
* Remove author fields (rely on maintainer fields only)
* Integrate feedback from the Naviation WG. Split the map-specific parameters and occupancy grid conversion parameters. The map parameters remain in the map YAML file and can reside with the map. The occupancy grid conversion parameters are parameters of the map server node. There can be multiple map nodes, each with different conversion parameters.
* Update the map server README
* Update sample YAML files
* Fix a merge problem after rebasing
* Update the nav2 launch file
* Move the map parameters back to the map YAML file
* Fix unintended change of resolution in YAML file
* Update README again
* Clarify a statement in the README

nav2_map_server/test/test_constants/test_constants.h

@@ -38,7 +38,6 @@ extern const unsigned int g_valid_image_height;
 extern const char g_valid_image_content[];
 extern const char * g_valid_png_file;
 extern const char * g_valid_bmp_file;
-extern const char * g_valid_yaml_file;
 
 extern const float g_valid_image_res;
 

nav2_map_server/test/test_launch_files/map_server_node.launch.py

@@ -26,5 +26,6 @@ def generate_launch_description():
             package='nav2_map_server',
             node_executable='map_server',
             output='screen',
-            arguments=[os.path.join(os.getenv('TEST_DIR'), 'testmap.yaml')])
+            arguments=['__params:=' + os.path.join(os.getenv('TEST_DIR'),
+                                                   'map_server_params.yaml')])
     ])

nav2_map_server/test/testmap.yaml

-image: testmap.png
+map_type: "occupancy"
+image: "testmap.png"
 resolution: 0.1
 origin: [2.0, 3.0, 1.0]
-negate: 0
 occupied_thresh: 0.65
 free_thresh: 0.196
+negate: 0
+mode: "trinary"

nav2_map_server/test/unit/test_occ_grid.cpp

@@ -33,8 +33,10 @@
 #include <experimental/filesystem>
 #include <stdexcept>
 #include <string>
+#include <vector>
+#include <memory>
 
-#include "nav2_map_server/map_representations/occ_grid_server.hpp"
+#include "nav2_map_server/occ_grid_loader.hpp"
 #include "test_constants/test_constants.h"
 
 #define TEST_DIR TEST_DIRECTORY
@@ -42,28 +44,51 @@
 using namespace std; // NOLINT
 using std::experimental::filesystem::path;
 
-class OccGridTest : public nav2_map_server::OccGridServer
+class RclCppFixture
 {
 public:
-  OccGridTest() {}
-  ~OccGridTest() {}
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class TestMapLoader : public nav2_map_server::OccGridLoader
+{
+public:
+  explicit TestMapLoader(rclcpp::Node * node, YAML::Node & doc)
+  : OccGridLoader(node, doc)
+  {
+  }
 
-  nav_msgs::srv::GetMap::Response GetMap() {return occ_resp_;}
+  nav_msgs::msg::OccupancyGrid getOccupancyGrid()
+  {
+    return msg_;
+  }
 };
 
-class MapServerTest : public ::testing::Test
+class MapLoaderTest : public ::testing::Test
 {
 public:
-  MapServerTest()
+  MapLoaderTest()
   {
-    OccTest = new OccGridTest;
-    auto test_yaml = path(TEST_DIR) / path(g_valid_yaml_file);
-    OccTest->LoadMapInfoFromFile(test_yaml.string());
+    // Set up a fake YAML document with the required fields
+    doc_["resolution"] = "0.1";
+    doc_["origin"][0] = "2.0";
+    doc_["origin"][1] = "3.0";
+    doc_["origin"][2] = "1.0";
+    doc_["negate"] = "0";
+    doc_["occupied_thresh"] = "0.65";
+    doc_["free_thresh"] = "0.196";
+
+    node_ = std::make_shared<rclcpp::Node>("map_server");
+    map_loader_ = new TestMapLoader(node_.get(), doc_);
   }
 
 protected:
-  OccGridTest * OccTest;
-  nav_msgs::srv::GetMap::Response map_resp;
+  rclcpp::Node::SharedPtr node_;
+  TestMapLoader * map_loader_;
+  YAML::Node doc_;
 };
 
 /* Try to load a valid PNG file.  Succeeds if no exception is thrown, and if
@@ -72,43 +97,44 @@ protected:
  * This test can fail on OS X, due to an apparent limitation of the
  * underlying SDL_Image library. */
 
-TEST_F(MapServerTest, loadValidPNG)
+TEST_F(MapLoaderTest, loadValidPNG)
 {
   auto test_png = path(TEST_DIR) / path(g_valid_png_file);
-  ASSERT_NO_THROW(OccTest->LoadMapFromFile(test_png.string()));
-  OccTest->SetMap();
-  map_resp = OccTest->GetMap();
-
-  EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
-  EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
-  EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
-  for (unsigned int i = 0; i < map_resp.map.info.width * map_resp.map.info.height; i++) {
-    EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
+
+  ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_png.string()));
+  nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid();
+
+  EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+  EXPECT_EQ(map_msg.info.width, g_valid_image_width);
+  EXPECT_EQ(map_msg.info.height, g_valid_image_height);
+  for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+    EXPECT_EQ(g_valid_image_content[i], map_msg.data[i]);
   }
 }
 
 /* Try to load a valid BMP file.  Succeeds if no exception is thrown, and if
  * the loaded image matches the known dimensions and content of the file. */
 
-TEST_F(MapServerTest, loadValidBMP)
+TEST_F(MapLoaderTest, loadValidBMP)
 {
   auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file);
-  ASSERT_NO_THROW(OccTest->LoadMapFromFile(test_bmp.string()));
-  OccTest->SetMap();
-  map_resp = OccTest->GetMap();
-
-  EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
-  EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
-  EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
-  for (unsigned int i = 0; i < map_resp.map.info.width * map_resp.map.info.height; i++) {
-    EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
+
+
+  ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_bmp.string()));
+  nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid();
+
+  EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+  EXPECT_EQ(map_msg.info.width, g_valid_image_width);
+  EXPECT_EQ(map_msg.info.height, g_valid_image_height);
+  for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+    EXPECT_EQ(g_valid_image_content[i], map_msg.data[i]);
   }
 }
 
 /* Try to load an invalid file.  Succeeds if a std::runtime exception is thrown */
 
-TEST_F(MapServerTest, loadInvalidFile)
+TEST_F(MapLoaderTest, loadInvalidFile)
 {
   auto test_invalid = path(TEST_DIR) / path("foo");
-  ASSERT_THROW(OccTest->LoadMapFromFile(test_invalid.string()), std::runtime_error);
+  ASSERT_THROW(map_loader_->loadMapFromFile(test_invalid.string()), std::runtime_error);
 }