Add LoadMap service to map_server

nav2_map_server/CMakeLists.txt

@@ -8,6 +8,7 @@ find_package(nav2_common REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(nav_msgs REQUIRED)
+find_package(nav2_msgs REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
@@ -40,6 +41,7 @@ set(map_server_dependencies
   rclcpp
   rclcpp_lifecycle
   nav_msgs
+  nav2_msgs
   yaml_cpp_vendor
   std_msgs
   tf2

nav2_map_server/README.md

@@ -1,89 +1,97 @@
-# Map Server
-
-The `Map Server` provides maps to the rest of the Navigation2 system using both topic and
-service interfaces. 
-
-## Changes from ROS1 Navigation Map Server
-
-While the nav2 map server provides the same general function as the nav1 map server, the new
-code has some changes to accomodate ROS2 as well as some architectural improvements.
-
-### Architecture
-
-In contrast to the ROS1 navigation map server, the nav2 map server will support a variety
-of map types, and thus some aspects of the original code have been refactored to support 
-this new extensible framework. In particular, there is now a `MapLoader` abstract base class 
-and type-specific map loaders which derive from this class. There is currently one such
-derived class, the `OccGridLoader`, which converts an input image to an OccupancyGrid and
-makes this available via topic and service interfaces. The `MapServer` class is a ROS2 node
-that uses the appropriate loader, based on an input parameter.
-
-### Command-line arguments, ROS2 Node Parameters, and YAML files
-
-The Map Server is a composable ROS2 node. By default, there is a map_server executable that
-instances one of these nodes, but it is possible to compose multiple map server nodes into
-a single process, if desired.
-
-The command line for the map server executable is slightly different that it was with ROS1.
-With ROS1, one invoked the map server and passing the map YAML filename, like this:
-
-```
-$ map_server map.yaml
-```
-
-Where the YAML file specified contained the various map metadata, such as:
-
-```
-image: testmap.png
-resolution: 0.1
-origin: [2.0, 3.0, 1.0]
-negate: 0
-occupied_thresh: 0.65
-free_thresh: 0.196
-```
-
-The Navigation2 software retains the map YAML file format from Nav1, but uses the ROS2 parameter
-mechanism to get the name of the YAML file to use. This effectively introduces a 
-level of indirection to get the map yaml filename. For example, for a node named 'map_server', 
-the parameter file would look like this:
-
-```
-# map_server_params.yaml
-map_server:
-    ros__parameters:
-        yaml_filename: "map.yaml"
-```
-
-One can invoke the map service executable directly, passing the params file on the command line,
-like this:
-
-```
-$ map_server __params:=map_server_params.yaml
-```
-
-There is also possibility of having multiple map server nodes in a single process, where the parameters file would separate the parameters by node name, like this:
-
-```
-# combined_params.yaml
-map_server1:
-    ros__parameters:
-        yaml_filename: "some_map.yaml"
-
-map_server2:
-    ros__parameters:
-        yaml_filename: "another_map.yaml"
-```
-
-Then, one would invoke this process with the params file that contains the parameters for both nodes:
-
-```
-$ process_with_multiple_map_servers __params:=combined_params.yaml
-```
-
-## Currently Supported Map Types
-- Occupancy grid (nav_msgs/msg/OccupancyGrid), via the OccGridLoader
-
-## Future Plans
-- Allow for dynamic configuration of conversion parameters
-- Support additional map types, e.g. GridMap (https://github.com/ros-planning/navigation2/issues/191)
-- Port and refactor Map Saver (https://github.com/ros-planning/navigation2/issues/188)
+# Map Server
+
+The `Map Server` provides maps to the rest of the Navigation2 system using both topic and
+service interfaces.
+
+## Changes from ROS1 Navigation Map Server
+
+While the nav2 map server provides the same general function as the nav1 map server, the new
+code has some changes to accomodate ROS2 as well as some architectural improvements.
+
+In addition, there is now a new "load_map" service which can be used to dynamically load a map.
+
+### Architecture
+
+In contrast to the ROS1 navigation map server, the nav2 map server will support a variety
+of map types, and thus some aspects of the original code have been refactored to support
+this new extensible framework. In particular, there is now a `MapLoader` abstract base class
+and type-specific map loaders which derive from this class. There is currently one such
+derived class, the `OccGridLoader`, which converts an input image to an OccupancyGrid and
+makes this available via topic and service interfaces. The `MapServer` class is a ROS2 node
+that uses the appropriate loader, based on an input parameter.
+
+### Command-line arguments, ROS2 Node Parameters, and YAML files
+
+The Map Server is a composable ROS2 node. By default, there is a map_server executable that
+instances one of these nodes, but it is possible to compose multiple map server nodes into
+a single process, if desired.
+
+The command line for the map server executable is slightly different that it was with ROS1.
+With ROS1, one invoked the map server and passing the map YAML filename, like this:
+
+```
+$ map_server map.yaml
+```
+
+Where the YAML file specified contained the various map metadata, such as:
+
+```
+image: testmap.png
+resolution: 0.1
+origin: [2.0, 3.0, 1.0]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+```
+
+The Navigation2 software retains the map YAML file format from Nav1, but uses the ROS2 parameter
+mechanism to get the name of the YAML file to use. This effectively introduces a
+level of indirection to get the map yaml filename. For example, for a node named 'map_server',
+the parameter file would look like this:
+
+```
+# map_server_params.yaml
+map_server:
+    ros__parameters:
+        yaml_filename: "map.yaml"
+```
+
+One can invoke the map service executable directly, passing the params file on the command line,
+like this:
+
+```
+$ map_server __params:=map_server_params.yaml
+```
+
+There is also possibility of having multiple map server nodes in a single process, where the parameters file would separate the parameters by node name, like this:
+
+```
+# combined_params.yaml
+map_server1:
+    ros__parameters:
+        yaml_filename: "some_map.yaml"
+
+map_server2:
+    ros__parameters:
+        yaml_filename: "another_map.yaml"
+```
+
+Then, one would invoke this process with the params file that contains the parameters for both nodes:
+
+```
+$ process_with_multiple_map_servers __params:=combined_params.yaml
+```
+
+## Currently Supported Map Types
+- Occupancy grid (nav_msgs/msg/OccupancyGrid), via the OccGridLoader
+
+## Services
+As in ROS navigation, the map_server node provides a "map" service to get the map. See the nav_msgs/srv/GetMap.srv file for details.
+
+NEW in ROS2 Eloquent, map_server also now provides a "load_map" service. See nav2_msgs/srv/LoadMap.srv for details.
+
+Example:
+```
+$ ros2 service call /load_map nav2_msgs/srv/LoadMap "{type: 0, map_id: /ros/maps/map.yaml}
+```
+

nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp

@@ -24,6 +24,7 @@
 #include "nav2_util/lifecycle_node.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav_msgs/srv/get_map.hpp"
+#include "nav2_msgs/srv/load_map.hpp"
 #include "rclcpp/rclcpp.hpp"
 
 namespace nav2_map_server
@@ -103,9 +104,17 @@ class OccGridLoader : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInt
   // Load the image and generate an OccupancyGrid
   void loadMapFromFile(const LoadParameters & loadParameters);
 
+  // Load the map yaml and image from yaml file name
+  bool loadMapFromYaml(
+    std::string yaml_file,
+    std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response = nullptr);
+
   // A service to provide the occupancy grid (GetMap) and the message to return
   rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;
 
+  // A service to load the occupancy grid from file at run time (LoadMap)
+  rclcpp::Service<nav2_msgs::srv::LoadMap>::SharedPtr load_map_service_;
+
   // A topic on which the occupancy grid will be published
   rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
 
@@ -121,6 +130,9 @@ class OccGridLoader : public rclcpp_lifecycle::node_interfaces::LifecycleNodeInt
   // The name of the service for getting a map
   static constexpr const char * service_name_{"map"};
 
+  // The name of the service for loading a map
+  static constexpr const char * load_map_service_name_{"load_map"};
+
   // Timer for republishing map
   rclcpp::TimerBase::SharedPtr timer_;
 };

nav2_map_server/src/occ_grid_loader.cpp

@@ -41,6 +41,7 @@
 #include "tf2/LinearMath/Quaternion.h"
 #include "yaml-cpp/yaml.h"
 #include "nav2_util/geometry_utils.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
 
 using namespace std::chrono_literals;
 
@@ -126,24 +127,31 @@ OccGridLoader::LoadParameters OccGridLoader::load_map_yaml(const std::string & y
   return loadParameters;
 }
 
-nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
+bool OccGridLoader::loadMapFromYaml(
+  std::string yaml_file,
+  std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response)
 {
-  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring");
-
-  msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+  if (yaml_file.empty()) {
+    RCLCPP_ERROR(node_->get_logger(), "YAML file name is empty, can't load!");
+    response->result = nav2_msgs::srv::LoadMap::Response::RESULT_MAP_DOES_NOT_EXIST;
+    return false;
+  }
+  RCLCPP_INFO(node_->get_logger(), "Loading yaml file: %s", yaml_file.c_str());
   LoadParameters loadParameters;
   try {
-    loadParameters = load_map_yaml(yaml_filename_);
+    loadParameters = load_map_yaml(yaml_file);
   } catch (YAML::Exception & e) {
     RCLCPP_ERROR(
       node_->get_logger(), "Failed processing YAML file %s at position (%d:%d) for reason: %s",
-      yaml_filename_.c_str(), e.mark.line, e.mark.column, e.what());
-    throw std::runtime_error("Failed to load map yaml file.");
+      yaml_file.c_str(), e.mark.line, e.mark.column, e.what());
+    response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA;
+    return false;
   } catch (std::exception & e) {
     RCLCPP_ERROR(
       node_->get_logger(), "Failed to parse map YAML loaded from file %s for reason: %s",
-      yaml_filename_.c_str(), e.what());
-    throw std::runtime_error("Failed to load map yaml file.");
+      yaml_file.c_str(), e.what());
+    response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_METADATA;
+    return false;
   }
 
   try {
@@ -152,25 +160,67 @@ nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::St
     RCLCPP_ERROR(
       node_->get_logger(), "Failed to load image file %s for reason: %s",
       loadParameters.image_file_name.c_str(), e.what());
+    response->result = nav2_msgs::srv::LoadMap::Response::RESULT_INVALID_MAP_DATA;
+    return false;
+  }
+  return true;
+}
+
+nav2_util::CallbackReturn OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring");
 
-    throw std::runtime_error("Failed to load map image file.");
+  // initialize Occupancy Grid msg - needed by loadMapFromYaml
+  msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+
+  if (!loadMapFromYaml(yaml_filename_)) {
+    throw std::runtime_error("Failed to load map yaml file: " + yaml_filename_);
   }
 
-  // Create a service callback handle
+  // Create GetMap service callback handle
   auto handle_occ_callback = [this](
     const std::shared_ptr<rmw_request_id_t>/*request_header*/,
     const std::shared_ptr<nav_msgs::srv::GetMap::Request>/*request*/,
     std::shared_ptr<nav_msgs::srv::GetMap::Response> response) -> void {
-      RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling map request");
+      if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+        RCLCPP_WARN(node_->get_logger(),
+          "Received GetMap request but not in ACTIVE state, ignoring!");
+        return;
+      }
+      RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling GetMap request");
       response->map = *msg_;
     };
 
   // Create a service that provides the occupancy grid
   occ_service_ = node_->create_service<nav_msgs::srv::GetMap>(service_name_, handle_occ_callback);
 
+  // Create the load_map service callback handle
+  auto load_map_callback = [this](
+    const std::shared_ptr<rmw_request_id_t>/*request_header*/,
+    const std::shared_ptr<nav2_msgs::srv::LoadMap::Request> request,
+    std::shared_ptr<nav2_msgs::srv::LoadMap::Response> response) -> void {
+      // if not in ACTIVE state, ignore request
+      if (node_->get_current_state().id() != lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) {
+        RCLCPP_WARN(node_->get_logger(),
+          "Received LoadMap request but not in ACTIVE state, ignoring!");
+        return;
+      }
+      RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Handling LoadMap request");
+      // Load from file
+      if (loadMapFromYaml(request->map_url, response)) {
+        response->map = *msg_;
+        response->result = nav2_msgs::srv::LoadMap::Response::RESULT_SUCCESS;
+        occ_pub_->publish(*msg_);  // publish new map
+      }
+    };
+
   // Create a publisher using the QoS settings to emulate a ROS1 latched topic
-  occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(
-    topic_name_, rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+  occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(topic_name_,
+      rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable());
+
+  // Create a service that loads the occupancy grid from a file
+  load_map_service_ = node_->create_service<nav2_msgs::srv::LoadMap>(load_map_service_name_,
+      load_map_callback);
 
   return nav2_util::CallbackReturn::SUCCESS;
 }
@@ -202,6 +252,7 @@ nav2_util::CallbackReturn OccGridLoader::on_cleanup(const rclcpp_lifecycle::Stat
 
   occ_pub_.reset();
   occ_service_.reset();
+  load_map_service_.reset();
   msg_.reset();
 
   return nav2_util::CallbackReturn::SUCCESS;
@@ -212,11 +263,14 @@ void OccGridLoader::loadMapFromFile(const LoadParameters & loadParameters)
   Magick::InitializeMagick(nullptr);
   nav_msgs::msg::OccupancyGrid msg;
 
+  RCLCPP_INFO(node_->get_logger(), "Loading image_file: %s",
+    loadParameters.image_file_name.c_str());
   Magick::Image img(loadParameters.image_file_name);
 
   // Copy the image data into the map structure
   msg.info.width = img.size().width();
   msg.info.height = img.size().height();
+
   msg.info.resolution = loadParameters.resolution;
   msg.info.origin.position.x = loadParameters.origin[0];
   msg.info.origin.position.y = loadParameters.origin[1];