Completed PR 2929 (ros-navigation#3038)

* accepting empty yaml_filename if no initial map is available

* invalid load_map-request does not invalidate existing map, added Testcase

* style

* finish PR 2929

* finish linting

* removing change

* removing change

* Update test_map_server_node.cpp

* Update test_map_server_node.cpp

Co-authored-by: Nikolas Engelhard <[email protected]>

nav2_map_server/README.md

@@ -94,6 +94,11 @@ Then, one would invoke this process with the params file that contains the param
 $ process_with_multiple_map_servers __params:=combined_params.yaml
 ```
 
+
+The parameter for the initial map (yaml_filename) has to be set, but an empty string can be used if no initial map should be loaded. In this case, no map is loaded during
+on_configure or published during on_activate. The _load_map_-service should the be used to load and publish a map. 
+
+
 #### Map Saver
 
 Like in ROS1 `map_saver` could be used as CLI-executable. It was renamed to `map_saver_cli`

nav2_map_server/include/nav2_map_server/map_server.hpp

@@ -138,6 +138,9 @@ class MapServer : public nav2_util::LifecycleNode
 
   // The message to publish on the occupancy grid topic
   nav_msgs::msg::OccupancyGrid msg_;
+
+  // true if msg_ was initialized
+  bool map_available_;
 };
 
 }  // namespace nav2_map_server

nav2_map_server/src/map_server/map_server.cpp

@@ -63,7 +63,7 @@ namespace nav2_map_server
 {
 
 MapServer::MapServer(const rclcpp::NodeOptions & options)
-: nav2_util::LifecycleNode("map_server", "", options)
+: nav2_util::LifecycleNode("map_server", "", options), map_available_(false)
 {
   RCLCPP_INFO(get_logger(), "Creating");
 
@@ -82,19 +82,26 @@ MapServer::on_configure(const rclcpp_lifecycle::State & /*state*/)
 {
   RCLCPP_INFO(get_logger(), "Configuring");
 
-  // Get the name of the YAML file to use
+  // Get the name of the YAML file to use (can be empty if no initial map should be used)
   std::string yaml_filename = get_parameter("yaml_filename").as_string();
-
   std::string topic_name = get_parameter("topic_name").as_string();
   frame_id_ = get_parameter("frame_id").as_string();
 
-  // Shared pointer to LoadMap::Response is also should be initialized
-  // in order to avoid null-pointer dereference
-  std::shared_ptr<nav2_msgs::srv::LoadMap::Response> rsp =
-    std::make_shared<nav2_msgs::srv::LoadMap::Response>();
-
-  if (!loadMapResponseFromYaml(yaml_filename, rsp)) {
-    throw std::runtime_error("Failed to load map yaml file: " + yaml_filename);
+  // only try to load map if parameter was set
+  if (!yaml_filename.empty()) {
+    // Shared pointer to LoadMap::Response is also should be initialized
+    // in order to avoid null-pointer dereference
+    std::shared_ptr<nav2_msgs::srv::LoadMap::Response> rsp =
+      std::make_shared<nav2_msgs::srv::LoadMap::Response>();
+
+    if (!loadMapResponseFromYaml(yaml_filename, rsp)) {
+      throw std::runtime_error("Failed to load map yaml file: " + yaml_filename);
+    }
+  } else {
+    RCLCPP_INFO(
+      get_logger(),
+      "yaml-filename parameter is empty, set map through '%s'-service",
+      load_map_service_name_.c_str());
   }
 
   // Make name prefix for services
@@ -125,8 +132,10 @@ MapServer::on_activate(const rclcpp_lifecycle::State & /*state*/)
 
   // Publish the map using the latched topic
   occ_pub_->on_activate();
-  auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>(msg_);
-  occ_pub_->publish(std::move(occ_grid));
+  if (map_available_) {
+    auto occ_grid = std::make_unique<nav_msgs::msg::OccupancyGrid>(msg_);
+    occ_pub_->publish(std::move(occ_grid));
+  }
 
   // create bond connection
   createBond();
@@ -155,6 +164,8 @@ MapServer::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
   occ_pub_.reset();
   occ_service_.reset();
   load_map_service_.reset();
+  map_available_ = false;
+  msg_ = nav_msgs::msg::OccupancyGrid();
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -192,6 +203,7 @@ void MapServer::loadMapCallback(
     RCLCPP_WARN(
       get_logger(),
       "Received LoadMap request but not in ACTIVE state, ignoring!");
+    response->result = response->RESULT_UNDEFINED_FAILURE;
     return;
   }
   RCLCPP_INFO(get_logger(), "Handling LoadMap request");
@@ -217,9 +229,10 @@ bool MapServer::loadMapResponseFromYaml(
       response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA;
       return false;
     case LOAD_MAP_SUCCESS:
-      // Correcting msg_ header when it belongs to spiecific node
+      // Correcting msg_ header when it belongs to specific node
       updateMsgHeader();
 
+      map_available_ = true;
       response->map = msg_;
       response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS;
   }

nav2_map_server/test/component/test_map_server_node.cpp

@@ -24,7 +24,9 @@
 #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::experimental::filesystem::path;
@@ -191,3 +193,45 @@ TEST_F(MapServerTestFixture, LoadMapInvalidImage)
 
   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);
+}