add humble-navigation2

navigation2/nav2_map_server/test/CMakeLists.txt

@@ -0,0 +1,8 @@
+set(TEST_LAUNCH_DIR ${CMAKE_CURRENT_SOURCE_DIR}/test_launch_files)
+set(TEST_DIR ${CMAKE_CURRENT_SOURCE_DIR})
+
+add_definitions( -DTEST_DIRECTORY=\"${CMAKE_CURRENT_SOURCE_DIR}\")
+
+add_subdirectory(unit)
+add_subdirectory(component)
+add_subdirectory(map_saver_cli)

navigation2/nav2_map_server/test/component/CMakeLists.txt

@@ -0,0 +1,51 @@
+include_directories(${PROJECT_SOURCE_DIR}/test)
+
+# map_server component test
+ament_add_gtest_executable(test_map_server_node
+  test_map_server_node.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+ament_target_dependencies(test_map_server_node rclcpp nav_msgs)
+target_link_libraries(test_map_server_node
+  ${library_name}
+)
+
+ament_add_test(test_map_server_node
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_server_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_DIR=${TEST_DIR}
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:test_map_server_node>
+)
+
+# map_saver component test
+ament_add_gtest_executable(test_map_saver_node
+  test_map_saver_node.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+
+ament_target_dependencies(test_map_saver_node rclcpp nav_msgs)
+target_link_libraries(test_map_saver_node
+  ${library_name}
+)
+
+add_executable(test_map_saver_publisher
+  test_map_saver_publisher.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+
+target_link_libraries(test_map_saver_publisher
+  ${map_io_library_name}
+)
+
+ament_add_test(test_map_saver_node
+  GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+  COMMAND "${CMAKE_CURRENT_SOURCE_DIR}/test_map_saver_launch.py"
+  WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  ENV
+    TEST_DIR=${TEST_DIR}
+    TEST_LAUNCH_DIR=${TEST_LAUNCH_DIR}
+    TEST_EXECUTABLE=$<TARGET_FILE:test_map_saver_node>
+)

navigation2/nav2_map_server/test/component/test_map_saver_launch.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_saver_node.launch.py')
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+    ld = LaunchDescription([
+        IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
+    ])
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_map_saver_node',
+    )
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    os.chdir(os.getenv('TEST_LAUNCH_DIR'))
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_map_server/test/component/test_map_saver_node.cpp

@@ -0,0 +1,222 @@
+// Copyright (c) 2020 Samsung Research Russia
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <filesystem>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "test_constants/test_constants.h"
+#include "nav2_map_server/map_saver.hpp"
+#include "nav2_util/lifecycle_service_client.hpp"
+#include "nav2_msgs/srv/save_map.hpp"
+
+#define TEST_DIR TEST_DIRECTORY
+
+using std::filesystem::path;
+using lifecycle_msgs::msg::Transition;
+using namespace nav2_map_server;  // NOLINT
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class MapSaverTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = rclcpp::Node::make_shared("map_client_test");
+    lifecycle_client_ =
+      std::make_shared<nav2_util::LifecycleServiceClient>("map_saver", node_);
+    RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
+
+    std::this_thread::sleep_for(std::chrono::seconds(5));  // allow node to start up
+    const std::chrono::seconds timeout(5);
+    lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
+    lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
+  }
+
+  static void TearDownTestCase()
+  {
+    lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
+    lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
+    lifecycle_client_.reset();
+    node_.reset();
+  }
+
+  template<class T>
+  typename T::Response::SharedPtr send_request(
+
+    rclcpp::Node::SharedPtr node,
+    typename rclcpp::Client<T>::SharedPtr client,
+    typename T::Request::SharedPtr request)
+  {
+    auto result = client->async_send_request(request);
+
+    // Wait for the result
+    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
+      return result.get();
+    } else {
+      return nullptr;
+    }
+  }
+
+protected:
+  // Check that map_msg corresponds to reference pattern
+  // Input: map_msg
+  void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
+  {
+    ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+    ASSERT_EQ(map_msg.info.width, g_valid_image_width);
+    ASSERT_EQ(map_msg.info.height, g_valid_image_height);
+    for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+      ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
+    }
+  }
+
+  static rclcpp::Node::SharedPtr node_;
+  static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
+};
+
+
+rclcpp::Node::SharedPtr MapSaverTestFixture::node_ = nullptr;
+std::shared_ptr<nav2_util::LifecycleServiceClient> MapSaverTestFixture::lifecycle_client_ =
+  nullptr;
+
+// Send map saving service request.
+// Load saved map and verify obtained OccupancyGrid.
+TEST_F(MapSaverTestFixture, SaveMap)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
+  auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
+    "/map_saver/save_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  // 1. Send valid save_map serivce request
+  req->map_topic = "map";
+  req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
+  req->image_format = "png";
+  req->map_mode = "trinary";
+  req->free_thresh = g_default_free_thresh;
+  req->occupied_thresh = g_default_occupied_thresh;
+  auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+
+  // 2. Load saved map and verify it
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+}
+
+// Send map saving service request with default parameters.
+// Load saved map and verify obtained OccupancyGrid.
+TEST_F(MapSaverTestFixture, SaveMapDefaultParameters)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
+  auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
+    "/map_saver/save_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  // 1. Send save_map serivce request with default parameters
+  req->map_topic = "";
+  req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
+  req->image_format = "";
+  req->map_mode = "";
+  req->free_thresh = 0.0;
+  req->occupied_thresh = 0.0;
+  auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+
+  // 2. Load saved map and verify it
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+}
+
+// Send map saving service requests with different sets of parameters.
+// In case of map is expected to be saved correctly, load map from a saved
+// file and verify obtained OccupancyGrid.
+TEST_F(MapSaverTestFixture, SaveMapInvalidParameters)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing SaveMap service");
+  auto req = std::make_shared<nav2_msgs::srv::SaveMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::SaveMap>(
+    "/map_saver/save_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for save_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  // 1. Trying to send save_map serivce request with different sets of parameters
+  // In case of map is expected to be saved correctly, verify it
+  req->map_topic = "invalid_map";
+  req->map_url = path(g_tmp_dir) / path(g_valid_map_name);
+  req->image_format = "png";
+  req->map_mode = "trinary";
+  req->free_thresh = g_default_free_thresh;
+  req->occupied_thresh = g_default_occupied_thresh;
+  auto resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+
+  req->map_topic = "map";
+  req->image_format = "invalid_format";
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+
+  req->image_format = "png";
+  req->map_mode = "invalid_mode";
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, true);
+  status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+  verifyMapMsg(map_msg);
+
+  req->map_mode = "trinary";
+  req->free_thresh = 2.0;
+  req->occupied_thresh = 2.0;
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+
+  req->free_thresh = -2.0;
+  req->occupied_thresh = -2.0;
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+
+  req->free_thresh = 0.7;
+  req->occupied_thresh = 0.2;
+  resp = send_request<nav2_msgs::srv::SaveMap>(node_, client, req);
+  ASSERT_EQ(resp->result, false);
+}

navigation2/nav2_map_server/test/component/test_map_saver_publisher.cpp

@@ -0,0 +1,59 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <filesystem>
+#include <string>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_map_server/map_io.hpp"
+#include "test_constants/test_constants.h"
+
+#define TEST_DIR TEST_DIRECTORY
+
+using namespace nav2_map_server;  // NOLINT
+using std::filesystem::path;
+
+class TestPublisher : public rclcpp::Node
+{
+public:
+  TestPublisher()
+  : Node("map_publisher")
+  {
+    std::string pub_map_file = path(TEST_DIR) / path(g_valid_yaml_file);
+    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.c_str());
+      return;
+    }
+
+    map_pub_ = create_publisher<nav_msgs::msg::OccupancyGrid>(
+      "map",
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+    map_pub_->publish(msg);
+  }
+
+protected:
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr map_pub_;
+};
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+  auto pub_node = std::make_shared<TestPublisher>();
+  rclcpp::spin(pub_node);
+  rclcpp::shutdown();
+  return 0;
+}

navigation2/nav2_map_server/test/component/test_map_server_launch.py

@@ -0,0 +1,47 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+import sys
+
+from launch import LaunchDescription
+from launch import LaunchService
+from launch.actions import ExecuteProcess
+from launch.actions import IncludeLaunchDescription
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch_testing.legacy import LaunchTestService
+
+
+def main(argv=sys.argv[1:]):
+    launchFile = os.path.join(os.getenv('TEST_LAUNCH_DIR'), 'map_server_node.launch.py')
+    testExecutable = os.getenv('TEST_EXECUTABLE')
+    ld = LaunchDescription([
+        IncludeLaunchDescription(PythonLaunchDescriptionSource([launchFile])),
+    ])
+    test1_action = ExecuteProcess(
+        cmd=[testExecutable],
+        name='test_map_server_node',
+    )
+    lts = LaunchTestService()
+    lts.add_test_action(ld, test1_action)
+    ls = LaunchService(argv=argv)
+    ls.include_launch_description(ld)
+    os.chdir(os.getenv('TEST_LAUNCH_DIR'))
+    return lts.run(ls)
+
+
+if __name__ == '__main__':
+    sys.exit(main())

navigation2/nav2_map_server/test/component/test_map_server_node.cpp

@@ -0,0 +1,237 @@
+// Copyright (c) 2018 Intel Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <filesystem>
+
+#include <rclcpp/rclcpp.hpp>
+
+#include "test_constants/test_constants.h"
+#include "nav2_map_server/map_server.hpp"
+#include "nav2_util/lifecycle_service_client.hpp"
+#include "nav2_msgs/srv/load_map.hpp"
+using namespace std::chrono_literals;
+using namespace rclcpp;  // NOLINT
+
+#define TEST_DIR TEST_DIRECTORY
+
+using std::filesystem::path;
+
+using lifecycle_msgs::msg::Transition;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class MapServerTestFixture : public ::testing::Test
+{
+public:
+  static void SetUpTestCase()
+  {
+    node_ = rclcpp::Node::make_shared("map_client_test");
+    lifecycle_client_ =
+      std::make_shared<nav2_util::LifecycleServiceClient>("map_server", node_);
+    RCLCPP_INFO(node_->get_logger(), "Creating Test Node");
+
+    std::this_thread::sleep_for(std::chrono::seconds(5));  // allow node to start up
+    const std::chrono::seconds timeout(5);
+    lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, timeout);
+    lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, timeout);
+  }
+
+  static void TearDownTestCase()
+  {
+    lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
+    lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
+    lifecycle_client_->change_state(Transition::TRANSITION_UNCONFIGURED_SHUTDOWN);
+    lifecycle_client_.reset();
+    node_.reset();
+  }
+
+  template<class T>
+  typename T::Response::SharedPtr send_request(
+    rclcpp::Node::SharedPtr node,
+    typename rclcpp::Client<T>::SharedPtr client,
+    typename T::Request::SharedPtr request)
+  {
+    auto result = client->async_send_request(request);
+
+    // Wait for the result
+    if (rclcpp::spin_until_future_complete(node, result) == rclcpp::FutureReturnCode::SUCCESS) {
+      return result.get();
+    } else {
+      return nullptr;
+    }
+  }
+
+protected:
+  // Check that map_msg corresponds to reference pattern
+  // Input: map_msg
+  void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
+  {
+    ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+    ASSERT_EQ(map_msg.info.width, g_valid_image_width);
+    ASSERT_EQ(map_msg.info.height, g_valid_image_height);
+    for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+      ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
+    }
+  }
+
+  static rclcpp::Node::SharedPtr node_;
+  static std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
+};
+
+
+rclcpp::Node::SharedPtr MapServerTestFixture::node_ = nullptr;
+std::shared_ptr<nav2_util::LifecycleServiceClient> MapServerTestFixture::lifecycle_client_ =
+  nullptr;
+
+
+// Send map getting service request and verify obtained OccupancyGrid
+TEST_F(MapServerTestFixture, GetMap)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing GetMap service");
+  auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
+  auto client = node_->create_client<nav_msgs::srv::GetMap>(
+    "/map_server/map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  auto resp = send_request<nav_msgs::srv::GetMap>(node_, client, req);
+
+  verifyMapMsg(resp->map);
+}
+
+// Send map loading service request and verify obtained OccupancyGrid
+TEST_F(MapServerTestFixture, LoadMap)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = path(TEST_DIR) / path(g_valid_yaml_file);
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS);
+  verifyMapMsg(resp->map);
+}
+
+// Send map loading service request without specifying which map to load
+TEST_F(MapServerTestFixture, LoadMapNull)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = "";
+  RCLCPP_INFO(node_->get_logger(), "Sending load_map request with null file name");
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST);
+}
+
+// Send map loading service request with non-existing yaml file
+TEST_F(MapServerTestFixture, LoadMapInvalidYaml)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = "invalid_file.yaml";
+  RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid yaml file name");
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA);
+}
+
+// Send map loading service request with yaml file containing non-existing map
+TEST_F(MapServerTestFixture, LoadMapInvalidImage)
+{
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service");
+  auto req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto client = node_->create_client<nav2_msgs::srv::LoadMap>(
+    "/map_server/load_map");
+
+  RCLCPP_INFO(node_->get_logger(), "Waiting for load_map service");
+  ASSERT_TRUE(client->wait_for_service());
+
+  req->map_url = path(TEST_DIR) / "invalid_image.yaml";
+  RCLCPP_INFO(node_->get_logger(), "Sending load_map request with invalid image file name");
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, client, req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA);
+}
+
+/**
+ * Test behaviour of server if yaml_filename is set to an empty string.
+ */
+TEST_F(MapServerTestFixture, NoInitialMap)
+{
+  // turn off node into unconfigured state
+  lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
+  lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
+
+  auto client = node_->create_client<nav_msgs::srv::GetMap>("/map_server/map");
+  auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
+
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node_, "map_server");
+  ASSERT_TRUE(parameters_client->wait_for_service(3s));
+
+  // set yaml_filename-parameter to empty string (essentially restart the node)
+  RCLCPP_INFO(node_->get_logger(), "Removing yaml_filename-parameter before restarting");
+  parameters_client->set_parameters({Parameter("yaml_filename", ParameterValue(""))});
+
+  // only configure node, to test behaviour of service while node is not active
+  lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE, 3s);
+
+  RCLCPP_INFO(node_->get_logger(), "Testing LoadMap service while not being active");
+  auto load_map_req = std::make_shared<nav2_msgs::srv::LoadMap::Request>();
+  auto load_map_cl = node_->create_client<nav2_msgs::srv::LoadMap>("/map_server/load_map");
+
+  ASSERT_TRUE(load_map_cl->wait_for_service(3s));
+  auto resp = send_request<nav2_msgs::srv::LoadMap>(node_, load_map_cl, load_map_req);
+
+  ASSERT_EQ(resp->result, nav2_msgs::srv::LoadMap::Response::RESULT_UNDEFINED_FAILURE);
+
+  // activate server and load map:
+  lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE, 3s);
+  RCLCPP_INFO(node_->get_logger(), "active again");
+
+  load_map_req->map_url = path(TEST_DIR) / path(g_valid_yaml_file);
+  auto load_res = send_request<nav2_msgs::srv::LoadMap>(node_, load_map_cl, load_map_req);
+
+  ASSERT_EQ(load_res->result, nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS);
+  verifyMapMsg(load_res->map);
+}

navigation2/nav2_map_server/test/invalid_image.yaml

@@ -0,0 +1,7 @@
+image: "invalid.png"
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+occupied_thresh: 0.65
+free_thresh: 0.196
+negate: 0
+mode: "trinary"

navigation2/nav2_map_server/test/map_saver_cli/CMakeLists.txt

@@ -0,0 +1,12 @@
+include_directories(${PROJECT_SOURCE_DIR}/test)
+
+# map_saver CLI
+ament_add_gtest(test_map_saver_cli
+  test_map_saver_cli.cpp
+  ${PROJECT_SOURCE_DIR}/test/test_constants.cpp
+)
+
+ament_target_dependencies(test_map_saver_cli rclcpp nav_msgs)
+target_link_libraries(test_map_saver_cli
+  ${dependencies}
+)

navigation2/nav2_map_server/test/map_saver_cli/test_map_saver_cli.cpp

@@ -0,0 +1,146 @@
+// Copyright (c) 2020 Samsung Research America
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <filesystem>
+#include <string>
+#include <memory>
+#include <utility>
+
+#include "rclcpp/rclcpp.hpp"
+#include "nav_msgs/msg/occupancy_grid.hpp"
+
+TEST(MapSaverCLI, CLITest)
+{
+  std::string path = "/tmp/";
+  std::string file = "test_map";
+  std::string file_path = path + file;
+
+  rclcpp::init(0, nullptr);
+
+  auto node = std::make_shared<rclcpp::Node>("CLI_Test_Node");
+  RCLCPP_INFO(node->get_logger(), "Testing Map Saver CLI");
+
+  auto publisher = node->create_publisher<nav_msgs::msg::OccupancyGrid>(
+    "/map",
+    rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+  auto msg = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+  msg->header.frame_id = "map";
+  msg->header.stamp = node->now();
+  msg->info.map_load_time = node->now();
+  msg->info.resolution = 0.05;
+  msg->info.width = 3;
+  msg->info.height = 3;
+  msg->info.origin.position.x = 0.0;
+  msg->info.origin.position.y = 0.0;
+  msg->info.origin.orientation.w = 1.0;
+  msg->data.resize(9);
+  msg->data[0] = 0;
+  msg->data[2] = 100;
+  msg->data[1] = 101;
+  msg->data[3] = 50;
+
+  RCLCPP_INFO(node->get_logger(), "Publishing occupancy grid...");
+
+  publisher->publish(std::move(msg));
+
+  rclcpp::Rate(1).sleep();
+
+  // succeed on real map
+  RCLCPP_INFO(node->get_logger(), "Calling saver...");
+
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
+
+  std::string command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli -f ") + file_path;
+  auto return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+
+  rclcpp::Rate(0.25).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Checking on file...");
+
+  EXPECT_TRUE(std::filesystem::exists(file_path + ".pgm"));
+  EXPECT_EQ(std::filesystem::file_size(file_path + ".pgm"), 20ul);
+
+  if (std::filesystem::exists(file_path + ".yaml")) {
+    std::filesystem::remove(file_path + ".yaml");
+  }
+  if (std::filesystem::exists(file_path + ".pgm")) {
+    std::filesystem::remove(file_path + ".pgm");
+  }
+
+  // fail on bogus map
+  RCLCPP_INFO(node->get_logger(), "Calling saver...");
+
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
+
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli "
+    "-t map_failure --occ 100 --free 2 --mode trinary --fmt png -f ") + file_path +
+    std::string("--ros-args __node:=map_saver_test_node");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 65280);
+
+  rclcpp::Rate(0.25).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Checking on file...");
+
+  EXPECT_FALSE(std::filesystem::exists(file_path + ".yaml"));
+
+  RCLCPP_INFO(node->get_logger(), "Testing help...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli -h");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+
+  rclcpp::Rate(0.5).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Testing invalid mode...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --mode fake_mode");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+
+  rclcpp::Rate(0.5).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Testing missing argument...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --mode");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 65280);
+
+  rclcpp::Rate(0.5).sleep();
+
+  RCLCPP_INFO(node->get_logger(), "Testing wrong argument...");
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --free 0 0");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 65280);
+
+  rclcpp::Rate(0.5).sleep();
+
+  command =
+    std::string(
+    "ros2 run nav2_map_server map_saver_cli --ros-args -r __node:=map_saver_test_node");
+  return_code = system(command.c_str());
+  EXPECT_EQ(return_code, 0);
+}

navigation2/nav2_map_server/test/map_saver_params.yaml

@@ -0,0 +1,5 @@
+map_saver:
+    ros__parameters:
+        save_map_timeout: 5.0
+        free_thresh_default: 0.196
+        occupied_thresh_default: 0.65

navigation2/nav2_map_server/test/map_server_params.yaml

@@ -0,0 +1,3 @@
+map_server:
+    ros__parameters:
+        yaml_filename: "../testmap.yaml"

navigation2/nav2_map_server/test/test_constants.cpp

@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Author: Brian Gerkey */
+
+/* This file contains global constants shared among tests */
+
+/* Note that these must be changed if the test image changes */
+
+#include "test_constants/test_constants.h"
+
+#include <vector>
+
+const unsigned int g_valid_image_width = 10;
+const unsigned int g_valid_image_height = 10;
+// Note that the image content is given in row-major order, with the
+// lower-left pixel first.  This is different from a graphics coordinate
+// system, which starts with the upper-left pixel.  The loadMapFromFile
+// call converts from the latter to the former when it loads the image, and
+// we want to compare against the result of that conversion.
+const char g_valid_image_content[] = {
+  0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+  100, 100, 100, 100, 0, 0, 100, 100, 100, 0,
+  100, 100, 100, 100, 0, 0, 100, 100, 100, 0,
+  100, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+  100, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 100, 100, 0, 0,
+  100, 0, 0, 0, 0, 0, 0, 0, 0, 0,
+};
+
+const char * g_valid_map_name = "testmap";
+const char * g_valid_png_file = "testmap.png";
+const char * g_valid_bmp_file = "testmap.bmp";
+const char * g_valid_pgm_file = "testmap.pgm";
+const char * g_valid_yaml_file = "testmap.yaml";
+const char * g_tmp_dir = "/tmp";
+
+const double g_valid_image_res = 0.1;
+const std::vector<double> g_valid_origin{2.0, 3.0, 1.0};
+const double g_default_free_thresh = 0.196;
+const double g_default_occupied_thresh = 0.65;

navigation2/nav2_map_server/test/test_constants/test_constants.h

@@ -0,0 +1,56 @@
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#ifndef TEST_CONSTANTS__TEST_CONSTANTS_H_
+#define TEST_CONSTANTS__TEST_CONSTANTS_H_
+
+/* Author: Brian Gerkey */
+
+/* This file externs global constants shared among tests */
+
+#include <vector>
+
+extern const unsigned int g_valid_image_width;
+extern const unsigned int g_valid_image_height;
+extern const char g_valid_image_content[];
+extern const char * g_valid_map_name;
+extern const char * g_valid_png_file;
+extern const char * g_valid_bmp_file;
+extern const char * g_valid_pgm_file;
+extern const char * g_valid_yaml_file;
+extern const char * g_tmp_dir;
+
+extern const double g_valid_image_res;
+// *INDENT-OFF*
+// Uncrustify may incorrectly guide to add extra spaces in < double > during CI tests
+extern const std::vector<double> g_valid_origin;
+// *INDENT-ON*
+extern const double g_default_free_thresh;
+extern const double g_default_occupied_thresh;
+
+#endif  // TEST_CONSTANTS__TEST_CONSTANTS_H_

navigation2/nav2_map_server/test/test_launch_files/map_saver_node.launch.py

@@ -0,0 +1,42 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from launch import LaunchDescription
+from launch.actions import ExecuteProcess
+import launch_ros.actions
+
+
+def generate_launch_description():
+    map_publisher = f"{os.path.dirname(os.getenv('TEST_EXECUTABLE'))}/test_map_saver_publisher"
+
+    ld = LaunchDescription()
+
+    map_saver_server_cmd = launch_ros.actions.Node(
+        package='nav2_map_server',
+        executable='map_saver_server',
+        output='screen',
+        parameters=[os.path.join(os.getenv('TEST_DIR'),
+                    'map_saver_params.yaml')])
+
+    map_publisher_cmd = ExecuteProcess(
+        cmd=[map_publisher])
+
+    ld.add_action(map_saver_server_cmd)
+    ld.add_action(map_publisher_cmd)
+
+    return ld

navigation2/nav2_map_server/test/test_launch_files/map_server_node.launch.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3
+
+# Copyright (c) 2018 Intel Corporation
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import os
+
+from launch import LaunchDescription
+import launch_ros.actions
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        launch_ros.actions.Node(
+            package='nav2_map_server',
+            executable='map_server',
+            output='screen',
+            parameters=[os.path.join(os.getenv('TEST_DIR'),
+                        'map_server_params.yaml')])
+    ])

navigation2/nav2_map_server/test/testmap.yaml

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

navigation2/nav2_map_server/test/unit/CMakeLists.txt

@@ -0,0 +1,21 @@
+include_directories(${PROJECT_SOURCE_DIR}/test)
+
+# map_io unit test
+ament_add_gtest(test_map_io test_map_io.cpp ${PROJECT_SOURCE_DIR}/test/test_constants.cpp)
+
+ament_target_dependencies(test_map_io rclcpp nav_msgs)
+
+target_link_libraries(test_map_io
+  ${map_io_library_name}
+)
+
+# costmap_filter_info_server unit test
+ament_add_gtest(test_costmap_filter_info_server
+  test_costmap_filter_info_server.cpp
+)
+
+ament_target_dependencies(test_costmap_filter_info_server rclcpp)
+
+target_link_libraries(test_costmap_filter_info_server
+  ${library_name}
+)

navigation2/nav2_map_server/test/unit/test_costmap_filter_info_server.cpp

@@ -0,0 +1,177 @@
+// Copyright (c) 2020 Samsung Research Russia
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <string>
+#include <memory>
+#include <chrono>
+#include <limits>
+#include <mutex>
+
+#include "rclcpp/rclcpp.hpp"
+
+#include "nav2_map_server/costmap_filter_info_server.hpp"
+
+using namespace std::chrono_literals;
+
+typedef std::recursive_mutex mutex_t;
+
+static const char FILTER_INFO_TOPIC[] = "filter_info";
+static const int TYPE = 1;
+static const char MASK_TOPIC[] = "mask";
+static const double BASE = 0.1;
+static const double MULTIPLIER = 0.2;
+
+static const double EPSILON = std::numeric_limits<float>::epsilon();
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+RclCppFixture g_rclcppfixture;
+
+class InfoServerWrapper : public nav2_map_server::CostmapFilterInfoServer
+{
+public:
+  void start()
+  {
+    on_configure(get_current_state());
+    on_activate(get_current_state());
+  }
+
+  void stop()
+  {
+    on_deactivate(get_current_state());
+    on_cleanup(get_current_state());
+    on_shutdown(get_current_state());
+  }
+
+  void deactivate()
+  {
+    on_deactivate(get_current_state());
+  }
+
+  void activate()
+  {
+    on_activate(get_current_state());
+  }
+};
+
+class InfoServerTester : public ::testing::Test
+{
+public:
+  InfoServerTester()
+  : info_server_(nullptr), info_(nullptr), subscription_(nullptr)
+  {
+    access_ = new mutex_t();
+
+    info_server_ = std::make_shared<InfoServerWrapper>();
+    try {
+      info_server_->set_parameter(rclcpp::Parameter("filter_info_topic", FILTER_INFO_TOPIC));
+      info_server_->set_parameter(rclcpp::Parameter("type", TYPE));
+      info_server_->set_parameter(rclcpp::Parameter("mask_topic", MASK_TOPIC));
+      info_server_->set_parameter(rclcpp::Parameter("base", BASE));
+      info_server_->set_parameter(rclcpp::Parameter("multiplier", MULTIPLIER));
+    } catch (rclcpp::exceptions::ParameterNotDeclaredException & ex) {
+      RCLCPP_ERROR(
+        info_server_->get_logger(),
+        "Error while setting parameters for CostmapFilterInfoServer: %s", ex.what());
+      throw;
+    }
+
+    info_server_->start();
+
+    subscription_ = info_server_->create_subscription<nav2_msgs::msg::CostmapFilterInfo>(
+      FILTER_INFO_TOPIC, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+      std::bind(&InfoServerTester::infoCallback, this, std::placeholders::_1));
+  }
+
+  ~InfoServerTester()
+  {
+    info_server_->stop();
+    info_server_.reset();
+    subscription_.reset();
+  }
+
+  bool isReceived()
+  {
+    std::lock_guard<mutex_t> guard(*getMutex());
+    if (info_) {
+      return true;
+    } else {
+      return false;
+    }
+  }
+
+  mutex_t * getMutex()
+  {
+    return access_;
+  }
+
+protected:
+  std::shared_ptr<InfoServerWrapper> info_server_;
+  nav2_msgs::msg::CostmapFilterInfo::SharedPtr info_;
+
+private:
+  void infoCallback(const nav2_msgs::msg::CostmapFilterInfo::SharedPtr msg)
+  {
+    std::lock_guard<mutex_t> guard(*getMutex());
+    info_ = msg;
+  }
+
+  rclcpp::Subscription<nav2_msgs::msg::CostmapFilterInfo>::SharedPtr subscription_;
+
+  mutex_t * access_;
+};
+
+TEST_F(InfoServerTester, testCostmapFilterInfoPublish)
+{
+  rclcpp::Time start_time = info_server_->now();
+  while (!isReceived()) {
+    rclcpp::spin_some(info_server_->get_node_base_interface());
+    std::this_thread::sleep_for(100ms);
+    // Waiting no more than 5 seconds
+    ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms));
+  }
+
+  // Checking received CostmapFilterInfo for consistency
+  EXPECT_EQ(info_->type, TYPE);
+  EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC);
+  EXPECT_NEAR(info_->base, BASE, EPSILON);
+  EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
+}
+
+TEST_F(InfoServerTester, testCostmapFilterInfoDeactivateActivate)
+{
+  info_server_->deactivate();
+  info_ = nullptr;
+  info_server_->activate();
+
+  rclcpp::Time start_time = info_server_->now();
+  while (!isReceived()) {
+    rclcpp::spin_some(info_server_->get_node_base_interface());
+    std::this_thread::sleep_for(100ms);
+    // Waiting no more than 5 seconds
+    ASSERT_TRUE((info_server_->now() - start_time) <= rclcpp::Duration(5000ms));
+  }
+
+  // Checking received CostmapFilterInfo for consistency
+  EXPECT_EQ(info_->type, TYPE);
+  EXPECT_EQ(info_->filter_mask_topic, MASK_TOPIC);
+  EXPECT_NEAR(info_->base, BASE, EPSILON);
+  EXPECT_NEAR(info_->multiplier, MULTIPLIER, EPSILON);
+}

navigation2/nav2_map_server/test/unit/test_map_io.cpp

@@ -0,0 +1,340 @@
+// Copyright 2019 Rover Robotics
+
+/*
+ * Copyright (c) 2008, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the Willow Garage, Inc. nor the names of its
+ *       contributors may be used to endorse or promote products derived from
+ *       this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/* Author: Brian Gerkey */
+
+#include <gtest/gtest.h>
+#include <filesystem>
+#include <stdexcept>
+#include <string>
+#include <vector>
+#include <memory>
+#include <iostream>
+#include <fstream>
+
+#include "yaml-cpp/yaml.h"
+#include "nav2_map_server/map_io.hpp"
+#include "nav2_map_server/map_server.hpp"
+#include "nav2_util/lifecycle_node.hpp"
+#include "test_constants/test_constants.h"
+
+#define TEST_DIR TEST_DIRECTORY
+
+using namespace std;  // NOLINT
+using namespace nav2_map_server;  // NOLINT
+using std::filesystem::path;
+
+class RclCppFixture
+{
+public:
+  RclCppFixture() {rclcpp::init(0, nullptr);}
+  ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class MapIOTester : public ::testing::Test
+{
+protected:
+  // Fill LoadParameters with standard for testing values
+  // Input: image_file_name
+  // Output: load_parameters
+  void fillLoadParameters(
+    const std::string & image_file_name,
+    LoadParameters & load_parameters)
+  {
+    load_parameters.image_file_name = image_file_name;
+    load_parameters.resolution = g_valid_image_res;
+    load_parameters.origin = g_valid_origin;
+    load_parameters.free_thresh = g_default_free_thresh;
+    load_parameters.occupied_thresh = g_default_occupied_thresh;
+    load_parameters.mode = MapMode::Trinary;
+    load_parameters.negate = 0;
+  }
+
+  // Fill SaveParameters with standard for testing values
+  // Input: map_file_name, image_format
+  // Output: save_parameters
+  void fillSaveParameters(
+    const std::string & map_file_name,
+    const std::string & image_format,
+    SaveParameters & save_parameters)
+  {
+    save_parameters.map_file_name = map_file_name;
+    save_parameters.image_format = image_format;
+    save_parameters.free_thresh = g_default_free_thresh;
+    save_parameters.occupied_thresh = g_default_occupied_thresh;
+    save_parameters.mode = MapMode::Trinary;
+  }
+
+  // Check that map_msg corresponds to reference pattern
+  // Input: map_msg
+  void verifyMapMsg(const nav_msgs::msg::OccupancyGrid & map_msg)
+  {
+    ASSERT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+    ASSERT_EQ(map_msg.info.width, g_valid_image_width);
+    ASSERT_EQ(map_msg.info.height, g_valid_image_height);
+    for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+      ASSERT_EQ(g_valid_image_content[i], map_msg.data[i]);
+    }
+  }
+};
+
+// Load a valid reference PGM file. Check obtained OccupancyGrid message for consistency:
+// loaded image should match the known dimensions and content of the file.
+// Save obtained OccupancyGrid message into a tmp PGM file. Then load back saved tmp file
+// and check for consistency.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveValidPGM)
+{
+  // 1. Load reference map file and verify obtained OccupancyGrid
+  LoadParameters loadParameters;
+  fillLoadParameters(path(TEST_DIR) / path(g_valid_pgm_file), loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  verifyMapMsg(map_msg);
+
+  // 2. Save OccupancyGrid into a tmp file
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "pgm", saveParameters);
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Load a valid reference PNG file. Check obtained OccupancyGrid message for consistency:
+// loaded image should match the known dimensions and content of the file.
+// Save obtained OccupancyGrid message into a tmp PNG file. Then load back saved tmp file
+// and check for consistency.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveValidPNG)
+{
+  // 1. Load reference map file and verify obtained OccupancyGrid
+  LoadParameters loadParameters;
+  fillLoadParameters(path(TEST_DIR) / path(g_valid_png_file), loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  verifyMapMsg(map_msg);
+
+  // 2. Save OccupancyGrid into a tmp file
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "png", saveParameters);
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Load a valid reference BMP file. Check obtained OccupancyGrid message for consistency:
+// loaded image should match the known dimensions and content of the file.
+// Save obtained OccupancyGrid message into a tmp BMP file. Then load back saved tmp file
+// and check for consistency.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveValidBMP)
+{
+  // 1. Load reference map file and verify obtained OccupancyGrid
+  auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file);
+
+  LoadParameters loadParameters;
+  fillLoadParameters(test_bmp, loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_NO_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  verifyMapMsg(map_msg);
+
+  // 2. Save OccupancyGrid into a tmp file
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "bmp", saveParameters);
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Load map from a valid file. Trying to save map with different modes.
+// Succeeds all steps were passed without a problem or expection.
+TEST_F(MapIOTester, loadSaveMapModes)
+{
+  // 1. Load map from YAML file
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(TEST_DIR) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  // No need to check Trinary mode. This already verified in previous testcases.
+  // 2. Save map in Scale mode.
+  SaveParameters saveParameters;
+  fillSaveParameters(path(g_tmp_dir) / path(g_valid_map_name), "png", saveParameters);
+  saveParameters.mode = MapMode::Scale;
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 3. Load saved map and verify it
+  status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+
+  // 4. Save map in Raw mode.
+  saveParameters.mode = MapMode::Raw;
+
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  // 5. Load saved map and verify it
+  status = loadMapFromYaml(path(g_tmp_dir) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  verifyMapMsg(map_msg);
+}
+
+// Try to load an invalid file with different ways.
+// Succeeds if all cases are got expected fail behaviours.
+TEST_F(MapIOTester, loadInvalidFile)
+{
+  // 1. Trying to load incorrect map by loadMapFromFile()
+  auto test_invalid = path(TEST_DIR) / path("foo");
+
+  LoadParameters loadParameters;
+  fillLoadParameters(test_invalid, loadParameters);
+
+  nav_msgs::msg::OccupancyGrid map_msg;
+  ASSERT_ANY_THROW(loadMapFromFile(loadParameters, map_msg));
+
+  // 2. Trying to load incorrect map by loadMapFromYaml()
+  LOAD_MAP_STATUS status = loadMapFromYaml("", map_msg);
+  ASSERT_EQ(status, MAP_DOES_NOT_EXIST);
+
+  status = loadMapFromYaml(std::string(test_invalid) + ".yaml", map_msg);
+  ASSERT_EQ(status, INVALID_MAP_METADATA);
+}
+
+// Load map from a valid file. Trying to save map with different sets of parameters.
+// Succeeds if all cases got expected behaviours.
+TEST_F(MapIOTester, saveInvalidParameters)
+{
+  // 1. Load map from YAML file
+  nav_msgs::msg::OccupancyGrid map_msg;
+  LOAD_MAP_STATUS status = loadMapFromYaml(path(TEST_DIR) / path(g_valid_yaml_file), map_msg);
+  ASSERT_EQ(status, LOAD_MAP_SUCCESS);
+
+  // 2. Trying to save map with different sets of parameters
+  SaveParameters saveParameters;
+
+  saveParameters.map_file_name = path(g_tmp_dir) / path(g_valid_map_name);
+  saveParameters.image_format = "";
+  saveParameters.free_thresh = 2.0;
+  saveParameters.occupied_thresh = 2.0;
+  saveParameters.mode = MapMode::Trinary;
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.free_thresh = -2.0;
+  saveParameters.occupied_thresh = -2.0;
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.free_thresh = 0.7;
+  saveParameters.occupied_thresh = 0.2;
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.free_thresh = 0.0;
+  saveParameters.occupied_thresh = 0.0;
+  ASSERT_TRUE(saveMapToFile(map_msg, saveParameters));
+
+  saveParameters.map_file_name = path("/invalid_path") / path(g_valid_map_name);
+  ASSERT_FALSE(saveMapToFile(map_msg, saveParameters));
+}
+
+// Load valid YAML file and check for consistency
+TEST_F(MapIOTester, loadValidYAML)
+{
+  LoadParameters loadParameters;
+  ASSERT_NO_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path(g_valid_yaml_file)));
+
+  LoadParameters refLoadParameters;
+  fillLoadParameters(path(TEST_DIR) / path(g_valid_png_file), refLoadParameters);
+  ASSERT_EQ(loadParameters.image_file_name, refLoadParameters.image_file_name);
+  ASSERT_FLOAT_EQ(loadParameters.resolution, refLoadParameters.resolution);
+  ASSERT_EQ(loadParameters.origin, refLoadParameters.origin);
+  ASSERT_FLOAT_EQ(loadParameters.free_thresh, refLoadParameters.free_thresh);
+  ASSERT_FLOAT_EQ(loadParameters.occupied_thresh, refLoadParameters.occupied_thresh);
+  ASSERT_EQ(loadParameters.mode, refLoadParameters.mode);
+  ASSERT_EQ(loadParameters.negate, refLoadParameters.negate);
+}
+
+// Try to load invalid YAML file
+TEST_F(MapIOTester, loadInvalidYAML)
+{
+  LoadParameters loadParameters;
+  ASSERT_ANY_THROW(loadParameters = loadMapYaml(path(TEST_DIR) / path("invalid_file.yaml")));
+}
+
+TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenShorterThanTwo)
+{
+  const std::string emptyFileName{};
+  ASSERT_EQ(emptyFileName, expand_user_home_dir_if_needed(emptyFileName, "/home/user"));
+}
+
+TEST(
+  HomeUserExpanderTestSuite,
+  homeUserExpanderShouldNotChangeInputStringWhenInputStringDoesNotStartWithHomeSequence)
+{
+  const std::string fileName{"valid_file.yaml"};
+  ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, "/home/user"));
+}
+
+TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldNotChangeInputStringWhenHomeVariableNotFound)
+{
+  const std::string fileName{"~/valid_file.yaml"};
+  ASSERT_EQ(fileName, expand_user_home_dir_if_needed(fileName, ""));
+}
+
+TEST(HomeUserExpanderTestSuite, homeUserExpanderShouldExpandHomeSequenceWhenHomeVariableSet)
+{
+  const std::string fileName{"~/valid_file.yaml"};
+  const std::string expectedOutputFileName{"/home/user/valid_file.yaml"};
+  ASSERT_EQ(expectedOutputFileName, expand_user_home_dir_if_needed(fileName, "/home/user"));
+}