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 i...

nav2_bringup/launch/nav2_bringup_launch.py

@@ -4,16 +4,13 @@ import launch.actions
 import launch_ros.actions
 
 def generate_launch_description():
-    map_file = launch.substitutions.LaunchConfiguration('map')
-    map_type = launch.substitutions.LaunchConfiguration('map_type', default='occupancy')
+    map_yaml_file = launch.substitutions.LaunchConfiguration('map')
     use_sim_time = launch.substitutions.LaunchConfiguration('use_sim_time', default='false')
     params_file = launch.substitutions.LaunchConfiguration('params', default='nav2_params.yaml')
 
     return LaunchDescription([
         launch.actions.DeclareLaunchArgument(
             'map', description='Full path to map file to load'),
-        launch.actions.DeclareLaunchArgument(
-            'map_type', default_value='occupancy', description='Type of map to load'),
         launch.actions.DeclareLaunchArgument(
             'use_sim_time', default_value='false', description='Use simulation (Gazebo) clock if true'),
 
@@ -32,8 +29,9 @@ def generate_launch_description():
         launch_ros.actions.Node(
             package='nav2_map_server',
             node_executable='map_server',
+            node_name='map_server',
             output='screen',
-            arguments=[map_file, map_type]),
+            parameters=[{ 'use_sim_time': use_sim_time}, { 'yaml_filename': map_yaml_file }]),
 
         launch_ros.actions.Node(
             package='nav2_world_model',
@@ -76,5 +74,4 @@ def generate_launch_description():
             node_name='mission_executor',
             output='screen',
             parameters=[{ 'use_sim_time': use_sim_time}]),
-
     ])

nav2_bringup/launch/nav2_params.yaml

+map_server:
+    ros__parameters:
+        yaml_filename: "test_map.yaml"

nav2_bringup/launch/test_map.yaml

-image: test_map.pgm
+map_type: "occupancy"
+image: "test_map.pgm"
 resolution: 0.050000
 origin: [-15.400000, -12.200000, 0.000000]
-negate: 0
-occupied_thresh: 0.65
 free_thresh: 0.196
-
+occupied_thresh: 0.65
+negate: 0
+mode: "trinary"

nav2_map_server/CMakeLists.txt

@@ -34,9 +34,8 @@ add_executable(${executable_name}
 set(library_name ${executable_name}_core)
 
 add_library(${library_name} SHARED
-  src/map_representations/occ_grid_server.cpp
-  src/map_representations/map_factory.cpp
-  src/map_server_ros.cpp
+  src/occ_grid_loader.cpp
+  src/map_server.cpp
 )
 
 set(dependencies

nav2_map_server/README.md

-# Nav2 Map Server
-
-The map server in the navigation2 stack is responsible for loading a map from an image file and providing a map message across 
-the ROS2 Interface for potential clients. In comparison to the ROS1 navigation map server, the nav2 map server intends to support
-a variety of potential map types, and thus some aspects of the original code have been refactored to support this extensible 
-framework.
-
-## Overview of changes from ROS1 Navigation Map Server
-- Implementation of the Map Server has been refactored into an abstract base class for map server and its respective map type-specific 
-implementations (i.e. nav_msgs/msg/OccupancyGrids)
-- Loading and Parsing of YAML file (path currently provided as command line argument) and loading of the directed image file have
-been refactored into function implementations from the map server abstract base class. 
-- A factory pattern has been introduced to decide at launch time which map type to use
-- It is optional whether the map is connected to the ROS Interface or is initialized as an object in another module (by
-instantiating the map type-specific class)
-- Map Saver not (yet) ported
+# 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)
+- Occupancy grid (nav_msgs/msg/OccupancyGrid), via the OccGridLoader
 
 ## Future Plans
-- Make abstract base class for map server derive from rclcpp::Node (https://github.com/ros-planning/navigation2/issues/189)
-- Support new map types, e.g. GridMap (https://github.com/ros-planning/navigation2/issues/191)
-- Load & Parse YAML file as ROS2 parameters via launch (https://github.com/ros-planning/navigation2/issues/190)
+- 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)

nav2_map_server/include/nav2_map_server/base_map_server.hpp

-// 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.
-
-#ifndef NAV2_MAP_SERVER__BASE_MAP_SERVER_HPP_
-#define NAV2_MAP_SERVER__BASE_MAP_SERVER_HPP_
-
-#include <string>
-
-namespace nav2_map_server
-{
-
-class BaseMapServer
-{
-public:
-  virtual ~BaseMapServer() {}
-
-  virtual void LoadMapInfoFromFile(const std::string & file_name) = 0;
-  virtual void LoadMapFromFile(const std::string & map_name) = 0;
-  virtual void PublishMap() = 0;
-  virtual void SetMap() = 0;
-  virtual void ConnectROS() = 0;
-};
-
-}  // namespace nav2_map_server
-
-#endif  // NAV2_MAP_SERVER__BASE_MAP_SERVER_HPP_

nav2_map_server/include/nav2_map_server/map_factory.hpp → nav2_map_server/include/nav2_map_server/map_loader.hpp

@@ -12,25 +12,23 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_MAP_SERVER__MAP_FACTORY_HPP_
-#define NAV2_MAP_SERVER__MAP_FACTORY_HPP_
+#ifndef NAV2_MAP_SERVER__MAP_LOADER_HPP_
+#define NAV2_MAP_SERVER__MAP_LOADER_HPP_
 
 #include <string>
 
-#include "rclcpp/rclcpp.hpp"
-#include "nav2_map_server/map_representations/map_reps.hpp"
-
 namespace nav2_map_server
 {
 
-class MapFactory
+class MapLoader
 {
 public:
-  BaseMapServer * CreateMap(
-    const std::string & map_type, rclcpp::Node::SharedPtr node,
-    const std::string & file_name);
+  virtual ~MapLoader() {}
+
+  virtual void loadMapFromFile(const std::string & filename) = 0;
+  virtual void startServices() = 0;
 };
 
 }  // namespace nav2_map_server
 
-#endif  // NAV2_MAP_SERVER__MAP_FACTORY_HPP_
+#endif  // NAV2_MAP_SERVER__MAP_LOADER_HPP_

nav2_map_server/include/nav2_map_server/map_representations/map_reps.hpp

-// 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.
-
-#ifndef NAV2_MAP_SERVER__MAP_REPS__MAP_REPS_HPP_
-#define NAV2_MAP_SERVER__MAP_REPS__MAP_REPS_HPP_
-
-#include "nav2_map_server/map_representations/occ_grid_server.hpp"
-
-#endif  // NAV2_MAP_SERVER__MAP_REPS__MAP_REPS_HPP_

nav2_map_server/include/nav2_map_server/map_server_ros.hpp → nav2_map_server/include/nav2_map_server/map_server.hpp

@@ -12,32 +12,43 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_MAP_SERVER__MAP_SERVER_ROS_HPP_
-#define NAV2_MAP_SERVER__MAP_SERVER_ROS_HPP_
+#ifndef NAV2_MAP_SERVER__MAP_SERVER_HPP_
+#define NAV2_MAP_SERVER__MAP_SERVER_HPP_
 
 #include <string>
-
+#include <memory>
+#include <vector>
 #include "rclcpp/rclcpp.hpp"
-#include "nav2_map_server/map_factory.hpp"
-#include "nav2_map_server/map_representations/map_reps.hpp"
+#include "nav2_map_server/map_loader.hpp"
+#include "yaml-cpp/yaml.h"
 
 namespace nav2_map_server
 {
 
-class MapServerROS
+class MapServer : public rclcpp::Node
 {
 public:
-  MapServerROS(const std::string & file_name, const std::string & map_type);
+  explicit MapServer(const std::string & node_name);
+  MapServer();
 
 private:
-  MapFactory * map_loader_;
-  BaseMapServer * map_;
+  void getParameters();
+
+  // The map server has one node parameter, the YAML filename
+  std::string yaml_filename_;
+
+  // The YAML document from which to get the conversion parameters
+  YAML::Node doc_;
 
-  // TODO(bpwilcox): Add converter for map representations
+  // The map type ("occupancy") from the YAML document which specifies
+  // the kind of loader to create
+  std::string map_type_;
+  std::unique_ptr<MapLoader> map_loader_;
 
-  rclcpp::Node::SharedPtr node_;
+  // The map filename ("image") from the YAML document to pass to the map loader
+  std::string map_filename_;
 };
 
 }  // namespace nav2_map_server
 
-#endif  // NAV2_MAP_SERVER__MAP_SERVER_ROS_HPP_
+#endif  // NAV2_MAP_SERVER__MAP_SERVER_HPP_

nav2_map_server/include/nav2_map_server/map_representations/occ_grid_server.hpp → nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp

@@ -12,65 +12,73 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef NAV2_MAP_SERVER__MAP_REPS__OCC_GRID_SERVER_HPP_
-#define NAV2_MAP_SERVER__MAP_REPS__OCC_GRID_SERVER_HPP_
+#ifndef NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_
+#define NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_
 
-#include <memory>
 #include <string>
+#include <vector>
 
+#include "rclcpp/rclcpp.hpp"
 #include "nav_msgs/msg/occupancy_grid.hpp"
 #include "nav_msgs/msg/map_meta_data.hpp"
 #include "nav_msgs/srv/get_map.hpp"
-#include "rclcpp/rclcpp.hpp"
-#include "nav2_map_server/base_map_server.hpp"
+#include "nav2_map_server/map_loader.hpp"
+#include "yaml-cpp/yaml.h"
 
 namespace nav2_map_server
 {
 
-class OccGridServer : public BaseMapServer
+class OccGridLoader : public MapLoader
 {
 public:
-  explicit OccGridServer(rclcpp::Node::SharedPtr node)
-  : node_(node) {}
-  OccGridServer(rclcpp::Node::SharedPtr node, std::string file_name);
-  OccGridServer() {}
-  ~OccGridServer() {}
-
-  void LoadMapInfoFromFile(const std::string & file_name);
-  void LoadMapFromFile(const std::string & map_name);
-  void PublishMap();
-  void SetMap();
-  void ConnectROS();
+  explicit OccGridLoader(rclcpp::Node * node, YAML::Node & doc);
+  OccGridLoader() = delete;
 
-protected:
-  enum MapMode { TRINARY, SCALE, RAW };
+  // Load the image and generate an OccupancyGrid
+  void loadMapFromFile(const std::string & filename) override;
 
-  // Info from parsing the YAML file
-  double origin_[3];
-  int negate_;
-  double occ_th_;
-  double free_th_;
-  double res_;
-  MapMode mode_ = TRINARY;
-  std::string frame_id_ = "map";
-  std::string map_name_;
+  // Make the OccupancyGrid available via ROS topic and service
+  void startServices() override;
 
+protected:
   // The ROS node to use for ROS-related operations such as creating a service
-  rclcpp::Node::SharedPtr node_;
+  rclcpp::Node * node_;
+
+  // The YAML file from which to get the conversion parameters
+  YAML::Node & doc_;
+
+  // Conversion parameters read from the YAML document
+  double resolution_;
+  std::vector<double> origin_;
+  double free_thresh_;
+  double occupied_thresh_;
+  enum MapMode { TRINARY, SCALE, RAW };
+  MapMode mode_;
+  int negate_;
 
   // A service to provide the ouccpancy grid (GetMap) and the message to return
   rclcpp::Service<nav_msgs::srv::GetMap>::SharedPtr occ_service_;
-  nav_msgs::srv::GetMap::Response occ_resp_;
 
-  // A topic on which the occupance grid will be published and the message to publish
+  // A topic on which the occupancy grid will be published
   rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
-  nav_msgs::msg::OccupancyGrid map_msg_;
 
-  // For now, publish the map periodically so that its sure to be received on the
-  // ROS1 side across the ROS1 bridge
+  // The message to publish on the occupancy grid topic
+  nav_msgs::msg::OccupancyGrid msg_;
+
+  // The frame ID used in the returned OccupancyGrid message
+  static const char * frame_id_;
+
+  // The name for the topic on which the map will be published
+  static const char * topic_name_;
+
+  // The name of the service for getting a map
+  static const char * service_name_;
+
+  // For now, use a timer to publish the map periodically so that it is sure
+  // to be received on the ROS1 side across the ROS1 bridge
   rclcpp::TimerBase::SharedPtr timer_;
 };
 
 }  // namespace nav2_map_server
 
-#endif  // NAV2_MAP_SERVER__MAP_REPS__OCC_GRID_SERVER_HPP_
+#endif  // NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_

nav2_map_server/package.xml

@@ -6,7 +6,6 @@
   <description>
     Refactored map server for ROS2 Navigation
   </description>
-  <author>Brian Gerkey, Tony Pratkanis</author>
   <maintainer email="brian.wilcox@intel.com">Brian Wilcox</maintainer>
   <license>Apache License 2.0</license>
 

nav2_map_server/src/main.cpp

@@ -13,29 +13,22 @@
 // limitations under the License.
 
 #include <string>
-#include "nav2_map_server/map_server_ros.hpp"
-
-#define USAGE    "\nUSAGE: map_server <map.yaml> <map_type>\n" \
-  "  map.yaml: map description file\n" \
-  "  map_type: the type of map to load (i.e. occupancy)\n"
+#include <memory>
+#include <stdexcept>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_map_server/map_server.hpp"
 
 int main(int argc, char ** argv)
 {
-  rclcpp::init(argc, argv);
-
-  if (argc != 3 && argc != 2) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"), "%s", USAGE);
-    return -1;
-  }
-
-  std::string file_name(argv[1]);
-  std::string map_type = (argc == 2) ? "occupancy" : std::string(argv[2]);
+  std::string node_name("map_server");
 
   try {
-    nav2_map_server::MapServerROS MapServer(file_name, map_type);
-  } catch (std::runtime_error & e) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"), "%s", e.what());
-    return -1;
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<nav2_map_server::MapServer>(node_name));
+    rclcpp::shutdown();
+  } catch (std::exception & ex) {
+    RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), ex.what());
+    RCLCPP_ERROR(rclcpp::get_logger(node_name.c_str()), "Exiting");
   }
 
   return 0;

nav2_map_server/src/map_representations/map_factory.cpp

-// 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 <string>
-
-#include "nav2_map_server/map_factory.hpp"
-
-namespace nav2_map_server
-{
-
-BaseMapServer * MapFactory::CreateMap(
-  const std::string & map_type,
-  rclcpp::Node::SharedPtr node, const std::string & file_name)
-{
-  if (map_type == "occupancy") {
-    return new OccGridServer(node, file_name);
-  } else {
-    RCLCPP_ERROR(node->get_logger(), "Cannot load map %s of type %s", file_name.c_str(),
-      map_type.c_str());
-    throw std::runtime_error("Map type not supported");
-  }
-}
-
-}  // namespace nav2_map_server

nav2_map_server/src/map_server.cpp

+// 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 "nav2_map_server/map_server.hpp"
+
+#include <libgen.h>
+#include <string>
+#include <memory>
+#include <fstream>
+#include <stdexcept>
+#include "nav2_map_server/occ_grid_loader.hpp"
+#include "yaml-cpp/yaml.h"
+
+using namespace std::chrono_literals;
+
+namespace nav2_map_server
+{
+
+MapServer::MapServer(const std::string & node_name)
+: Node(node_name)
+{
+  // Get the MAP YAML file, which includes the image filename and the map type
+  getParameters();
+
+  // Create the proper map loader for the specified map type
+  if (map_type_ == "occupancy") {
+    map_loader_ = std::make_unique<OccGridLoader>(this, doc_);
+  } else {
+    std::string msg = "Cannot load unknown map type: '" + map_type_ + "'";
+    throw std::runtime_error(msg);
+  }
+
+  // Load the map and provide access via topic and service interfaces
+  map_loader_->loadMapFromFile(map_filename_);
+  map_loader_->startServices();
+}
+
+MapServer::MapServer()
+: MapServer("map_server")
+{
+}
+
+void MapServer::getParameters()
+{
+  get_parameter_or_set("yaml_filename", yaml_filename_, std::string("map.yaml"));
+
+  // Make sure that there's a valid file there and open it up
+  std::ifstream fin(yaml_filename_.c_str());
+  if (fin.fail()) {
+    throw std::runtime_error("Could not open '" + yaml_filename_ + "': file not found");
+  }
+
+  doc_ = YAML::LoadFile(yaml_filename_);
+
+  // Get the name of the map file
+  try {
+    map_filename_ = doc_["image"].as<std::string>();
+    if (map_filename_.size() == 0) {
+      throw std::runtime_error("The image tag cannot be an empty string");
+    }
+    if (map_filename_[0] != '/') {
+      // dirname can modify what you pass it
+      char * fname_copy = strdup(yaml_filename_.c_str());
+      map_filename_ = std::string(dirname(fname_copy)) + '/' + map_filename_;
+      free(fname_copy);
+    }
+  } catch (YAML::Exception) {
+    std::string msg = "'" + yaml_filename_ + "' does not contain an image tag or it is invalid";
+    throw std::runtime_error(msg);
+  }
+
+  // Get the map type so that we can create the correct map loader
+  try {
+    map_type_ = doc_["map_type"].as<std::string>();
+  } catch (YAML::Exception) {
+    std::string msg = "'" + yaml_filename_ + "' does not contain a map_type tag or it is invalid";
+    throw std::runtime_error(msg);
+  }
+}
+
+}  // namespace nav2_map_server

nav2_map_server/src/map_server_ros.cpp

-// 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 <string>
-#include "nav2_map_server/map_factory.hpp"
-#include "nav2_map_server/map_server_ros.hpp"
-
-namespace nav2_map_server
-{
-
-MapServerROS::MapServerROS(const std::string & file_name, const std::string & map_type)
-{
-  node_ = rclcpp::Node::make_shared("map_server");
-
-  try {
-    map_loader_ = new MapFactory();
-    RCLCPP_INFO(node_->get_logger(), "Loading map %s of type '%s'", file_name.c_str(),
-      map_type.c_str());
-    map_ = map_loader_->CreateMap(map_type, node_, file_name);
-    rclcpp::spin(node_);
-  } catch (std::runtime_error e) {
-    RCLCPP_ERROR(node_->get_logger(), "Cannot load map %s of type %s because: %s.",
-      file_name.c_str(), map_type.c_str(), e.what());
-  }
-}
-
-}  // namespace nav2_map_server

nav2_map_server/src/map_representations/occ_grid_server.cpp → nav2_map_server/src/occ_grid_loader.cpp

@@ -32,19 +32,13 @@
  *
  * Author: Brian Gerkey
  */
-#include "nav2_map_server/map_representations/occ_grid_server.hpp"
+#include "nav2_map_server/occ_grid_loader.hpp"
 
-#include <libgen.h>
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-#include <fstream>
+#include <string>
+#include <vector>
 #include <memory>
 #include <stdexcept>
-#include <string>
-#include <chrono>
 
-#include "yaml-cpp/yaml.h"
 #include "LinearMath/btQuaternion.h"
 #include "SDL/SDL_image.h"
 
@@ -53,172 +47,113 @@ using namespace std::chrono_literals;
 namespace nav2_map_server
 {
 
-#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+const char * OccGridLoader::frame_id_ = "map";
+const char * OccGridLoader::topic_name_ = "occ_grid";
+const char * OccGridLoader::service_name_ = "occ_grid";
 
-template<typename T>
-void operator>>(const YAML::Node & node, T & i)
+OccGridLoader::OccGridLoader(rclcpp::Node * node, YAML::Node & doc)
+: node_(node), doc_(doc), origin_(3)
 {
-  i = node.as<T>();
-}
-
-OccGridServer::OccGridServer(rclcpp::Node::SharedPtr node, std::string file_name)
-: node_(node)
-{
-  RCLCPP_INFO(node_->get_logger(), "OccGridServer: Load map info for map file: %s",
-    file_name.c_str());
-  LoadMapInfoFromFile(file_name);
-
-  RCLCPP_INFO(node_->get_logger(), "OccGridServer: Loading Map: %s", map_name_.c_str());
-  LoadMapFromFile(map_name_);
-
-  ConnectROS();
-  SetMap();
-  PublishMap();
-  RCLCPP_INFO(node_->get_logger(), "OccGridServer: Set up map request service and publisher.");
-}
-
-void OccGridServer::LoadMapInfoFromFile(const std::string & file_name)
-{
-  std::ifstream fin(file_name.c_str());
-  if (fin.fail()) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-      "Map_server could not open %s", file_name.c_str());
-    throw std::runtime_error("File path invalid");
-  }
-
-  YAML::Node doc = YAML::LoadFile(file_name);
-
   try {
-    doc["resolution"] >> res_;
+    resolution_ = doc_["resolution"].as<double>();
   } catch (YAML::Exception) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-      "The map %s does not contain a resolution tag or it is invalid.", file_name.c_str());
-    throw std::runtime_error("The map does not contain a resolution tag or it is invalid.");
+    throw std::runtime_error("The map does not contain a resolution tag or it is invalid");
   }
 
   try {
-    doc["negate"] >> negate_;
+    origin_[0] = doc_["origin"][0].as<double>();
+    origin_[1] = doc_["origin"][1].as<double>();
+    origin_[2] = doc_["origin"][2].as<double>();
   } catch (YAML::Exception) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-      "The map %s does not contain a negate tag or it is invalid.", file_name.c_str());
-    throw std::runtime_error("The map does not contain a negate tag or it is invalid.");
+    throw std::runtime_error("The map does not contain an origin tag or it is invalid");
   }
 
   try {
-    doc["occupied_thresh"] >> occ_th_;
+    free_thresh_ = doc_["free_thresh"].as<double>();
   } catch (YAML::Exception) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-      "The map %s does not contain an occupied_thresh tag or it is invalid.", file_name.c_str());
-    throw std::runtime_error("The map does not contain an occupied_thresh tag or it is invalid.");
+    throw std::runtime_error("The map does not contain a free_thresh tag or it is invalid");
   }
 
   try {
-    doc["free_thresh"] >> free_th_;
+    occupied_thresh_ = doc_["occupied_thresh"].as<double>();
   } catch (YAML::Exception) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-      "The map %s does not contain a free_thresh tag or it is invalid.", file_name.c_str());
-    throw std::runtime_error("The map does not contain a free_thresh tag or it is invalid.");
+    throw std::runtime_error("The map does not contain an occupied_thresh tag or it is invalid");
   }
 
+  std::string mode_str;
   try {
-    std::string modeS = "";
-    doc["mode"] >> modeS;
-    if (modeS == "trinary") {
+    mode_str = doc_["mode"].as<std::string>();
+
+    // Convert the string version of the mode name to one of the enumeration values
+    if (mode_str == "trinary") {
       mode_ = TRINARY;
-    } else if (modeS == "scale") {
+    } else if (mode_str == "scale") {
       mode_ = SCALE;
-    } else if (modeS == "raw") {
+    } else if (mode_str == "raw") {
       mode_ = RAW;
     } else {
-      RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-        "The map %s has invalid mode tag \"%s\".", file_name.c_str(), modeS.c_str());
-      throw std::runtime_error("Invalid mode tag.");
+      RCLCPP_WARN(node_->get_logger(),
+        "Mode parameter not recognized: '%s', using default value (trinary)",
+        mode_str.c_str());
+      mode_ = TRINARY;
     }
-  } catch (YAML::Exception) {
-    RCLCPP_DEBUG(rclcpp::get_logger("map_server"),
-      "The map %s does not contain a mode tag or it is invalid.", file_name.c_str());
+  } catch (YAML::Exception &) {
+    RCLCPP_WARN(node_->get_logger(), "Mode parameter not set, using default value (trinary)");
     mode_ = TRINARY;
   }
 
   try {
-    doc["origin"][0] >> origin_[0];
-    doc["origin"][1] >> origin_[1];
-    doc["origin"][2] >> origin_[2];
+    negate_ = doc_["negate"].as<int>();
   } catch (YAML::Exception) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-      "The map %s does not contain an origin tag or it is invalid.", file_name.c_str());
-    throw std::runtime_error("The map does not contain an origin tag or it is invalid.");
+    throw std::runtime_error("The map does not contain a negate tag or it is invalid");
   }
 
-  try {
-    doc["image"] >> map_name_;
-    // TODO(bpwilcox): make this path-handling more robust
-    if (map_name_.size() == 0) {
-      RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-        "The image tag in map %s cannot be an empty string.", file_name.c_str());
-      throw std::runtime_error("The image tag cannot be an empty string.");
-    }
-    if (map_name_[0] != '/') {
-      // dirname can modify what you pass it
-      char * fname_copy = strdup(file_name.c_str());
-      map_name_ = std::string(dirname(fname_copy)) + '/' + map_name_;
-      free(fname_copy);
-    }
-  } catch (YAML::Exception) {
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"),
-      "The map %s does not contain an image tag or it is invalid.", file_name.c_str());
-    throw std::runtime_error("The map does not contain an image tag or it is invalid.");
-  }
+  RCLCPP_DEBUG(node_->get_logger(), "resolution: %f", resolution_);
+  RCLCPP_DEBUG(node_->get_logger(), "origin[0]: %f", origin_[0]);
+  RCLCPP_DEBUG(node_->get_logger(), "origin[1]: %f", origin_[1]);
+  RCLCPP_DEBUG(node_->get_logger(), "origin[2]: %f", origin_[2]);
+  RCLCPP_DEBUG(node_->get_logger(), "free_thresh: %f", free_thresh_);
+  RCLCPP_DEBUG(node_->get_logger(), "occupied_thresh: %f", occupied_thresh_);
+  RCLCPP_DEBUG(node_->get_logger(), "mode_str: %s", mode_str.c_str());
+  RCLCPP_DEBUG(node_->get_logger(), "mode: %d", mode_);
+  RCLCPP_DEBUG(node_->get_logger(), "negate: %d", negate_);
 }
 
-void OccGridServer::LoadMapFromFile(const std::string & map_name_)
+void OccGridLoader::loadMapFromFile(const std::string & map_name)
 {
-  SDL_Surface * img;
-  const char * name = map_name_.c_str();
-  unsigned char * pixels;
-  unsigned char * p;
-  unsigned char value;
-  int rowstride, n_channels, avg_channels;
-  unsigned int i, j;
-  int k;
-  double occ;
-  int alpha;
-  int color_sum;
-  double color_avg;
-
   // Load the image using SDL.  If we get NULL back, the image load failed.
-  if (!(img = IMG_Load(name))) {
+  SDL_Surface * img;
+  if (!(img = IMG_Load(map_name.c_str()))) {
     std::string errmsg = std::string("failed to open image file \"") +
-      std::string(name) + std::string("\": ") + IMG_GetError();
-    RCLCPP_ERROR(rclcpp::get_logger("map_server"), "%s", errmsg.c_str());
-
+      map_name + std::string("\": ") + IMG_GetError();
     throw std::runtime_error(errmsg);
   }
 
   // Copy the image data into the map structure
-  map_msg_.info.width = img->w;
-  map_msg_.info.height = img->h;
-  map_msg_.info.resolution = res_;
-  map_msg_.info.origin.position.x = *(origin_);
-  map_msg_.info.origin.position.y = *(origin_ + 1);
-  map_msg_.info.origin.position.z = 0.0;
+  msg_.info.width = img->w;
+  msg_.info.height = img->h;
+  msg_.info.resolution = resolution_;
+  msg_.info.origin.position.x = origin_[0];
+  msg_.info.origin.position.y = origin_[1];
+  msg_.info.origin.position.z = 0.0;
   btQuaternion q;
   // setEulerZYX(yaw, pitch, roll)
-  q.setEulerZYX(*(origin_ + 2), 0, 0);
-  map_msg_.info.origin.orientation.x = q.x();
-  map_msg_.info.origin.orientation.y = q.y();
-  map_msg_.info.origin.orientation.z = q.z();
-  map_msg_.info.origin.orientation.w = q.w();
+  q.setEulerZYX(origin_[2], 0, 0);
+  msg_.info.origin.orientation.x = q.x();
+  msg_.info.origin.orientation.y = q.y();
+  msg_.info.origin.orientation.z = q.z();
+  msg_.info.origin.orientation.w = q.w();
 
   // Allocate space to hold the data
-  map_msg_.data.resize(map_msg_.info.width * map_msg_.info.height);
+  msg_.data.resize(msg_.info.width * msg_.info.height);
 
   // Get values that we'll need to iterate through the pixels
-  rowstride = img->pitch;
-  n_channels = img->format->BytesPerPixel;
+  int rowstride = img->pitch;
+  int n_channels = img->format->BytesPerPixel;
 
   // NOTE: Trinary mode still overrides here to preserve existing behavior.
   // Alpha will be averaged in with color channels when using trinary mode.
+  int avg_channels;
   if (mode_ == TRINARY || !img->format->Amask) {
     avg_channels = n_channels;
   } else {
@@ -226,102 +161,103 @@ void OccGridServer::LoadMapFromFile(const std::string & map_name_)
   }
 
   // Copy pixel data into the map structure
-  pixels = (unsigned char *)(img->pixels);
-  for (j = 0; j < map_msg_.info.height; j++) {
-    for (i = 0; i < map_msg_.info.width; i++) {
+  unsigned char * pixels = (unsigned char *)(img->pixels);
+  int color_sum;
+  for (unsigned int j = 0; j < msg_.info.height; j++) {
+    for (unsigned int i = 0; i < msg_.info.width; i++) {
       // Compute mean of RGB for this pixel
-      p = pixels + j * rowstride + i * n_channels;
+      unsigned char * p = pixels + j * rowstride + i * n_channels;
       color_sum = 0;
-      for (k = 0; k < avg_channels; k++) {
+      for (int k = 0; k < avg_channels; k++) {
         color_sum += *(p + (k));
       }
-      color_avg = color_sum / static_cast<double>(avg_channels);
+      double color_avg = color_sum / static_cast<double>(avg_channels);
 
+      int alpha;
       if (n_channels == 1) {
         alpha = 1;
       } else {
         alpha = *(p + n_channels - 1);
       }
+
       if (negate_) {
         color_avg = 255 - color_avg;
       }
+
+#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
+
+      unsigned char value;
       if (mode_ == RAW) {
         value = color_avg;
-        map_msg_.data[MAP_IDX(map_msg_.info.width, i, map_msg_.info.height - j - 1)] = value;
+        msg_.data[MAP_IDX(msg_.info.width, i, msg_.info.height - j - 1)] = value;
         continue;
       }
 
       // If negate is true, we consider blacker pixels free, and whiter
       // pixels free.  Otherwise, it's vice versa.
-      occ = (255 - color_avg) / 255.0;
+      double occ = (255 - color_avg) / 255.0;
 
       // Apply thresholds to RGB means to determine occupancy values for
       // map.  Note that we invert the graphics-ordering of the pixels to
       // produce a map with cell (0,0) in the lower-left corner.
-      if (occ > occ_th_) {
+      if (occ > occupied_thresh_) {
         value = +100;
-      } else if (occ < free_th_) {
+      } else if (occ < free_thresh_) {
         value = 0;
       } else if (mode_ == TRINARY || alpha < 1.0) {
         value = -1;
       } else {
-        double ratio = (occ - free_th_) / (occ_th_ - free_th_);
+        double ratio = (occ - free_thresh_) / (occupied_thresh_ - free_thresh_);
         value = 99 * ratio;
       }
 
-      map_msg_.data[MAP_IDX(map_msg_.info.width, i, map_msg_.info.height - j - 1)] = value;
+      msg_.data[MAP_IDX(msg_.info.width, i, msg_.info.height - j - 1)] = value;
     }
   }
 
   SDL_FreeSurface(img);
 
-  map_msg_.info.map_load_time = node_->now();
-  map_msg_.header.frame_id = frame_id_;
-  map_msg_.header.stamp = node_->now();
+  msg_.info.map_load_time = node_->now();
+  msg_.header.frame_id = frame_id_;
+  msg_.header.stamp = node_->now();
 
-  RCLCPP_INFO(rclcpp::get_logger("map_server"), "Read map %s: %d X %d map @ %.3lf m/cell",
-    map_name_.c_str(),
-    map_msg_.info.width,
-    map_msg_.info.height,
-    map_msg_.info.resolution);
+  RCLCPP_DEBUG(node_->get_logger(), "Read map %s: %d X %d map @ %.3lf m/cell",
+    map_name.c_str(),
+    msg_.info.width,
+    msg_.info.height,
+    msg_.info.resolution);
 }
 
-void OccGridServer::ConnectROS()
+void OccGridLoader::startServices()
 {
-  // Create a publisher using the QoS settings to emulate a ROS1 latched topic
-  rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
-  custom_qos_profile.depth = 1;
-  custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
-  custom_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
-  occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(
-    "occ_grid", custom_qos_profile);
-
   // Create a 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(), "OccGridServer: handle_occ_callback");
-      response->map = occ_resp_.map;
+      RCLCPP_INFO(node_->get_logger(), "Handling map request");
+      response->map = msg_;
     };
 
   // Create a service that provides the occupancy grid
-  occ_service_ = node_->create_service<nav_msgs::srv::GetMap>("occ_grid", handle_occ_callback);
-}
+  occ_service_ = node_->create_service<nav_msgs::srv::GetMap>(service_name_, handle_occ_callback);
 
-void OccGridServer::SetMap()
-{
-  occ_resp_.map = map_msg_;
-}
+  // Create a publisher using the QoS settings to emulate a ROS1 latched topic
+  rmw_qos_profile_t custom_qos_profile = rmw_qos_profile_default;
+  custom_qos_profile.depth = 1;
+  custom_qos_profile.durability = RMW_QOS_POLICY_DURABILITY_TRANSIENT_LOCAL;
+  custom_qos_profile.reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE;
+  occ_pub_ = node_->create_publisher<nav_msgs::msg::OccupancyGrid>(
+    topic_name_, custom_qos_profile);
 
-void OccGridServer::PublishMap()
-{
-  occ_pub_->publish(map_msg_);
+  // Publish the map using the latched topic
+  occ_pub_->publish(msg_);
 
-  // For now, periodically publish the map so that the ros1 bridge will be sure the proxy the
+  // TODO(mjeronimo): Remove the following once we've got everything on the ROS2 side
+  //
+  // Periodically publish the map so that the ros1 bridge will be sure the proxy the
   // message to rviz on the ROS1 side
-  // TODO(mjeronimo): Remove this once we've got everything on the ROS2 side
-  auto timer_callback = [this]() -> void {occ_pub_->publish(map_msg_);};
+  auto timer_callback = [this]() -> void {occ_pub_->publish(msg_);};
   timer_ = node_->create_wall_timer(2s, timer_callback);
 }
 

nav2_map_server/test/component/test_occ_grid_launch.py

@@ -40,6 +40,7 @@ def main(argv=sys.argv[1:]):
     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)
 
 

nav2_map_server/test/component/test_occ_grid_node.cpp

@@ -16,7 +16,7 @@
 #include <rclcpp/rclcpp.hpp>
 #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"
 
 // TODO(bpwilcox): test publisher

nav2_map_server/test/map_server_params.yaml

+map_server:
+    ros__parameters:
+        yaml_filename: "../testmap.yaml"

nav2_map_server/test/test_constants.cpp

@@ -57,6 +57,5 @@ const char g_valid_image_content[] = {
 
 const char * g_valid_png_file = "testmap.png";
 const char * g_valid_bmp_file = "testmap.bmp";
-const char * g_valid_yaml_file = "testmap.yaml";
 
 const float g_valid_image_res = 0.1;