diff --git a/nav2_bringup/launch/nav2_bringup_launch.py b/nav2_bringup/launch/nav2_bringup_launch.py
index 0dec4f4bb8dd7dfb133d908972bfa90d42d01ea5..620abc0ebded2c3bc8e3d1fac40cff40b72aa09b 100644
--- a/nav2_bringup/launch/nav2_bringup_launch.py
+++ b/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}]),
-
])
diff --git a/nav2_bringup/launch/nav2_params.yaml b/nav2_bringup/launch/nav2_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..409f9668028a6dc49ef403517c0071ef7244df6f
--- /dev/null
+++ b/nav2_bringup/launch/nav2_params.yaml
@@ -0,0 +1,3 @@
+map_server:
+ ros__parameters:
+ yaml_filename: "test_map.yaml"
diff --git a/nav2_bringup/launch/test_map.yaml b/nav2_bringup/launch/test_map.yaml
index 6f3b4d4c91d51580e4982b44a64e61e55f14544b..5587e0c71c402c1df7695bae2f515b59f12dc439 100644
--- a/nav2_bringup/launch/test_map.yaml
+++ b/nav2_bringup/launch/test_map.yaml
@@ -1,7 +1,8 @@
-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"
diff --git a/nav2_map_server/CMakeLists.txt b/nav2_map_server/CMakeLists.txt
index e08d9cf9879f9a9b6e11ad489799caaf2ba02196..6a1a4fcc3957d8e24a5b8016fbdbfe1824f995e9 100644
--- a/nav2_map_server/CMakeLists.txt
+++ b/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
diff --git a/nav2_map_server/README.md b/nav2_map_server/README.md
index 1c09763bb65f80f85794a24b344579ca44ea2f6e..272a2ff1874977f16116b96c3041288d61569ee2 100644
--- a/nav2_map_server/README.md
+++ b/nav2_map_server/README.md
@@ -1,25 +1,89 @@
-# 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)
diff --git a/nav2_map_server/include/nav2_map_server/base_map_server.hpp b/nav2_map_server/include/nav2_map_server/base_map_server.hpp
deleted file mode 100644
index 986d177fd5eac89a6019d6b1ba3e788f47d84f01..0000000000000000000000000000000000000000
--- a/nav2_map_server/include/nav2_map_server/base_map_server.hpp
+++ /dev/null
@@ -1,37 +0,0 @@
-// 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_
diff --git a/nav2_map_server/include/nav2_map_server/map_factory.hpp b/nav2_map_server/include/nav2_map_server/map_loader.hpp
similarity index 65%
rename from nav2_map_server/include/nav2_map_server/map_factory.hpp
rename to nav2_map_server/include/nav2_map_server/map_loader.hpp
index af189b5176ababf9baf83ebd0e2036f42a54765d..90c2c2e3092e72cc7532e4613182b03180b989b9 100644
--- a/nav2_map_server/include/nav2_map_server/map_factory.hpp
+++ b/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_
diff --git a/nav2_map_server/include/nav2_map_server/map_representations/map_reps.hpp b/nav2_map_server/include/nav2_map_server/map_representations/map_reps.hpp
deleted file mode 100644
index 7306111df24fc010ee1779a1889fb7af73806f00..0000000000000000000000000000000000000000
--- a/nav2_map_server/include/nav2_map_server/map_representations/map_reps.hpp
+++ /dev/null
@@ -1,20 +0,0 @@
-// 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_
diff --git a/nav2_map_server/include/nav2_map_server/map_representations/occ_grid_server.hpp b/nav2_map_server/include/nav2_map_server/map_representations/occ_grid_server.hpp
deleted file mode 100644
index 6e9b3f6620e64d4d49842c7df6dd7154165d7b35..0000000000000000000000000000000000000000
--- a/nav2_map_server/include/nav2_map_server/map_representations/occ_grid_server.hpp
+++ /dev/null
@@ -1,76 +0,0 @@
-// 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__OCC_GRID_SERVER_HPP_
-#define NAV2_MAP_SERVER__MAP_REPS__OCC_GRID_SERVER_HPP_
-
-#include <memory>
-#include <string>
-
-#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"
-
-namespace nav2_map_server
-{
-
-class OccGridServer : public BaseMapServer
-{
-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();
-
-protected:
- enum MapMode { TRINARY, SCALE, RAW };
-
- // 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_;
-
- // The ROS node to use for ROS-related operations such as creating a service
- rclcpp::Node::SharedPtr node_;
-
- // 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
- 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
- rclcpp::TimerBase::SharedPtr timer_;
-};
-
-} // namespace nav2_map_server
-
-#endif // NAV2_MAP_SERVER__MAP_REPS__OCC_GRID_SERVER_HPP_
diff --git a/nav2_map_server/include/nav2_map_server/map_server.hpp b/nav2_map_server/include/nav2_map_server/map_server.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..fdb178aeb1f6bf5e2a70543a54a32dac8b1a8ba8
--- /dev/null
+++ b/nav2_map_server/include/nav2_map_server/map_server.hpp
@@ -0,0 +1,54 @@
+// 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_SERVER_HPP_
+#define NAV2_MAP_SERVER__MAP_SERVER_HPP_
+
+#include <string>
+#include <memory>
+#include <vector>
+#include "rclcpp/rclcpp.hpp"
+#include "nav2_map_server/map_loader.hpp"
+#include "yaml-cpp/yaml.h"
+
+namespace nav2_map_server
+{
+
+class MapServer : public rclcpp::Node
+{
+public:
+ explicit MapServer(const std::string & node_name);
+ MapServer();
+
+private:
+ 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_;
+
+ // 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_;
+
+ // 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_HPP_
diff --git a/nav2_map_server/include/nav2_map_server/map_server_ros.hpp b/nav2_map_server/include/nav2_map_server/map_server_ros.hpp
deleted file mode 100644
index 288d9779f2260958802e7809ee6c74166636fd95..0000000000000000000000000000000000000000
--- a/nav2_map_server/include/nav2_map_server/map_server_ros.hpp
+++ /dev/null
@@ -1,43 +0,0 @@
-// 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_SERVER_ROS_HPP_
-#define NAV2_MAP_SERVER__MAP_SERVER_ROS_HPP_
-
-#include <string>
-
-#include "rclcpp/rclcpp.hpp"
-#include "nav2_map_server/map_factory.hpp"
-#include "nav2_map_server/map_representations/map_reps.hpp"
-
-namespace nav2_map_server
-{
-
-class MapServerROS
-{
-public:
- MapServerROS(const std::string & file_name, const std::string & map_type);
-
-private:
- MapFactory * map_loader_;
- BaseMapServer * map_;
-
- // TODO(bpwilcox): Add converter for map representations
-
- rclcpp::Node::SharedPtr node_;
-};
-
-} // namespace nav2_map_server
-
-#endif // NAV2_MAP_SERVER__MAP_SERVER_ROS_HPP_
diff --git a/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e2909ca5dd8e857ba6a92aa2a9db1c8037058b9c
--- /dev/null
+++ b/nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp
@@ -0,0 +1,84 @@
+// 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__OCC_GRID_LOADER_HPP_
+#define NAV2_MAP_SERVER__OCC_GRID_LOADER_HPP_
+
+#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 "nav2_map_server/map_loader.hpp"
+#include "yaml-cpp/yaml.h"
+
+namespace nav2_map_server
+{
+
+class OccGridLoader : public MapLoader
+{
+public:
+ explicit OccGridLoader(rclcpp::Node * node, YAML::Node & doc);
+ OccGridLoader() = delete;
+
+ // Load the image and generate an OccupancyGrid
+ void loadMapFromFile(const std::string & filename) override;
+
+ // 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 * 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_;
+
+ // A topic on which the occupancy grid will be published
+ rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
+
+ // 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__OCC_GRID_LOADER_HPP_
diff --git a/nav2_map_server/package.xml b/nav2_map_server/package.xml
index 9d308badd0019777dd1b14e23282272dd5a519a2..447e436d256359232df40c4bc3195d166ab064d8 100644
--- a/nav2_map_server/package.xml
+++ b/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>
diff --git a/nav2_map_server/src/main.cpp b/nav2_map_server/src/main.cpp
index d51d83391c868aa92b79230f69d3bb7721f92fe2..7bac49e9c17cf0bf7d4b37d1b58915308bca8e9d 100644
--- a/nav2_map_server/src/main.cpp
+++ b/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;
diff --git a/nav2_map_server/src/map_representations/map_factory.cpp b/nav2_map_server/src/map_representations/map_factory.cpp
deleted file mode 100644
index cb0af373ebac553d37387b362c80620d8fb87428..0000000000000000000000000000000000000000
--- a/nav2_map_server/src/map_representations/map_factory.cpp
+++ /dev/null
@@ -1,35 +0,0 @@
-// 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
diff --git a/nav2_map_server/src/map_representations/occ_grid_server.cpp b/nav2_map_server/src/map_representations/occ_grid_server.cpp
deleted file mode 100644
index 2ab3d70edb1d96b7626f428ae848eb6c9283be1f..0000000000000000000000000000000000000000
--- a/nav2_map_server/src/map_representations/occ_grid_server.cpp
+++ /dev/null
@@ -1,328 +0,0 @@
-/*
- * 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.
- */
-
-/*
- * This file contains helper functions for loading images as maps.
- *
- * Author: Brian Gerkey
- */
-#include "nav2_map_server/map_representations/occ_grid_server.hpp"
-
-#include <libgen.h>
-#include <cstdio>
-#include <cstdlib>
-#include <cstring>
-#include <fstream>
-#include <memory>
-#include <stdexcept>
-#include <string>
-#include <chrono>
-
-#include "yaml-cpp/yaml.h"
-#include "LinearMath/btQuaternion.h"
-#include "SDL/SDL_image.h"
-
-using namespace std::chrono_literals;
-
-namespace nav2_map_server
-{
-
-#define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
-
-template<typename T>
-void operator>>(const YAML::Node & node, T & i)
-{
- 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_;
- } 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.");
- }
-
- try {
- doc["negate"] >> negate_;
- } 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.");
- }
-
- try {
- doc["occupied_thresh"] >> occ_th_;
- } 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.");
- }
-
- try {
- doc["free_thresh"] >> free_th_;
- } 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.");
- }
-
- try {
- std::string modeS = "";
- doc["mode"] >> modeS;
- if (modeS == "trinary") {
- mode_ = TRINARY;
- } else if (modeS == "scale") {
- mode_ = SCALE;
- } else if (modeS == "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.");
- }
- } 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());
- mode_ = TRINARY;
- }
-
- try {
- doc["origin"][0] >> origin_[0];
- doc["origin"][1] >> origin_[1];
- doc["origin"][2] >> origin_[2];
- } 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.");
- }
-
- 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.");
- }
-}
-
-void OccGridServer::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))) {
- 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());
-
- 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;
- 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();
-
- // Allocate space to hold the data
- map_msg_.data.resize(map_msg_.info.width * map_msg_.info.height);
-
- // Get values that we'll need to iterate through the pixels
- rowstride = img->pitch;
- 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.
- if (mode_ == TRINARY || !img->format->Amask) {
- avg_channels = n_channels;
- } else {
- avg_channels = n_channels - 1;
- }
-
- // 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++) {
- // Compute mean of RGB for this pixel
- p = pixels + j * rowstride + i * n_channels;
- color_sum = 0;
- for (k = 0; k < avg_channels; k++) {
- color_sum += *(p + (k));
- }
- color_avg = color_sum / static_cast<double>(avg_channels);
-
- if (n_channels == 1) {
- alpha = 1;
- } else {
- alpha = *(p + n_channels - 1);
- }
- if (negate_) {
- color_avg = 255 - color_avg;
- }
- if (mode_ == RAW) {
- value = color_avg;
- map_msg_.data[MAP_IDX(map_msg_.info.width, i, map_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;
-
- // 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_) {
- value = +100;
- } else if (occ < free_th_) {
- value = 0;
- } else if (mode_ == TRINARY || alpha < 1.0) {
- value = -1;
- } else {
- double ratio = (occ - free_th_) / (occ_th_ - free_th_);
- value = 99 * ratio;
- }
-
- map_msg_.data[MAP_IDX(map_msg_.info.width, i, map_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();
-
- 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);
-}
-
-void OccGridServer::ConnectROS()
-{
- // 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;
- };
-
- // Create a service that provides the occupancy grid
- occ_service_ = node_->create_service<nav_msgs::srv::GetMap>("occ_grid", handle_occ_callback);
-}
-
-void OccGridServer::SetMap()
-{
- occ_resp_.map = map_msg_;
-}
-
-void OccGridServer::PublishMap()
-{
- occ_pub_->publish(map_msg_);
-
- // For now, 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_);};
- timer_ = node_->create_wall_timer(2s, timer_callback);
-}
-
-} // namespace nav2_map_server
diff --git a/nav2_map_server/src/map_server.cpp b/nav2_map_server/src/map_server.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e96e1860995815128096af2496517cc7d23068a0
--- /dev/null
+++ b/nav2_map_server/src/map_server.cpp
@@ -0,0 +1,92 @@
+// 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
diff --git a/nav2_map_server/src/map_server_ros.cpp b/nav2_map_server/src/map_server_ros.cpp
deleted file mode 100644
index b4ccc1d8b06f7388766cd36cdd19751653c3bdec..0000000000000000000000000000000000000000
--- a/nav2_map_server/src/map_server_ros.cpp
+++ /dev/null
@@ -1,38 +0,0 @@
-// 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
diff --git a/nav2_map_server/src/occ_grid_loader.cpp b/nav2_map_server/src/occ_grid_loader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0bafabc58fe9328adfba39650b6aa1280d080d87
--- /dev/null
+++ b/nav2_map_server/src/occ_grid_loader.cpp
@@ -0,0 +1,264 @@
+/*
+ * 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.
+ */
+
+/*
+ * This file contains helper functions for loading images as maps.
+ *
+ * Author: Brian Gerkey
+ */
+#include "nav2_map_server/occ_grid_loader.hpp"
+
+#include <string>
+#include <vector>
+#include <memory>
+#include <stdexcept>
+
+#include "LinearMath/btQuaternion.h"
+#include "SDL/SDL_image.h"
+
+using namespace std::chrono_literals;
+
+namespace nav2_map_server
+{
+
+const char * OccGridLoader::frame_id_ = "map";
+const char * OccGridLoader::topic_name_ = "occ_grid";
+const char * OccGridLoader::service_name_ = "occ_grid";
+
+OccGridLoader::OccGridLoader(rclcpp::Node * node, YAML::Node & doc)
+: node_(node), doc_(doc), origin_(3)
+{
+ try {
+ resolution_ = doc_["resolution"].as<double>();
+ } catch (YAML::Exception) {
+ throw std::runtime_error("The map does not contain a resolution tag or it is invalid");
+ }
+
+ try {
+ origin_[0] = doc_["origin"][0].as<double>();
+ origin_[1] = doc_["origin"][1].as<double>();
+ origin_[2] = doc_["origin"][2].as<double>();
+ } catch (YAML::Exception) {
+ throw std::runtime_error("The map does not contain an origin tag or it is invalid");
+ }
+
+ try {
+ free_thresh_ = doc_["free_thresh"].as<double>();
+ } catch (YAML::Exception) {
+ throw std::runtime_error("The map does not contain a free_thresh tag or it is invalid");
+ }
+
+ try {
+ occupied_thresh_ = doc_["occupied_thresh"].as<double>();
+ } catch (YAML::Exception) {
+ throw std::runtime_error("The map does not contain an occupied_thresh tag or it is invalid");
+ }
+
+ std::string mode_str;
+ try {
+ 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 (mode_str == "scale") {
+ mode_ = SCALE;
+ } else if (mode_str == "raw") {
+ mode_ = RAW;
+ } else {
+ RCLCPP_WARN(node_->get_logger(),
+ "Mode parameter not recognized: '%s', using default value (trinary)",
+ mode_str.c_str());
+ mode_ = TRINARY;
+ }
+ } catch (YAML::Exception &) {
+ RCLCPP_WARN(node_->get_logger(), "Mode parameter not set, using default value (trinary)");
+ mode_ = TRINARY;
+ }
+
+ try {
+ negate_ = doc_["negate"].as<int>();
+ } catch (YAML::Exception) {
+ throw std::runtime_error("The map does not contain a negate 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 OccGridLoader::loadMapFromFile(const std::string & map_name)
+{
+ // Load the image using SDL. If we get NULL back, the image load failed.
+ SDL_Surface * img;
+ if (!(img = IMG_Load(map_name.c_str()))) {
+ std::string errmsg = std::string("failed to open image file \"") +
+ map_name + std::string("\": ") + IMG_GetError();
+ throw std::runtime_error(errmsg);
+ }
+
+ // Copy the image data into the map structure
+ 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);
+ 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
+ msg_.data.resize(msg_.info.width * msg_.info.height);
+
+ // Get values that we'll need to iterate through the pixels
+ 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 {
+ avg_channels = n_channels - 1;
+ }
+
+ // Copy pixel data into the map structure
+ 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
+ unsigned char * p = pixels + j * rowstride + i * n_channels;
+ color_sum = 0;
+ for (int k = 0; k < avg_channels; k++) {
+ color_sum += *(p + (k));
+ }
+ 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;
+ 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.
+ 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 > occupied_thresh_) {
+ value = +100;
+ } else if (occ < free_thresh_) {
+ value = 0;
+ } else if (mode_ == TRINARY || alpha < 1.0) {
+ value = -1;
+ } else {
+ double ratio = (occ - free_thresh_) / (occupied_thresh_ - free_thresh_);
+ value = 99 * ratio;
+ }
+
+ msg_.data[MAP_IDX(msg_.info.width, i, msg_.info.height - j - 1)] = value;
+ }
+ }
+
+ SDL_FreeSurface(img);
+
+ msg_.info.map_load_time = node_->now();
+ msg_.header.frame_id = frame_id_;
+ msg_.header.stamp = node_->now();
+
+ 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 OccGridLoader::startServices()
+{
+ // 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(), "Handling map 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 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);
+
+ // Publish the map using the latched topic
+ occ_pub_->publish(msg_);
+
+ // 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
+ auto timer_callback = [this]() -> void {occ_pub_->publish(msg_);};
+ timer_ = node_->create_wall_timer(2s, timer_callback);
+}
+
+} // namespace nav2_map_server
diff --git a/nav2_map_server/test/component/test_occ_grid_launch.py b/nav2_map_server/test/component/test_occ_grid_launch.py
index a01c8658f37438e954fc54ddeb2059e096d2f6c7..e9f9295a61f7a53480fb31f10b5e26d5e402e94c 100755
--- a/nav2_map_server/test/component/test_occ_grid_launch.py
+++ b/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)
diff --git a/nav2_map_server/test/component/test_occ_grid_node.cpp b/nav2_map_server/test/component/test_occ_grid_node.cpp
index 6de3474f7679a7eaabc709a48b60c835c8f08ec6..118814ff9117fb388f01032c73cc54cdbc468c7c 100644
--- a/nav2_map_server/test/component/test_occ_grid_node.cpp
+++ b/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
diff --git a/nav2_map_server/test/map_server_params.yaml b/nav2_map_server/test/map_server_params.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..5bbe4c91e127341a2a0f844bcf9e0147d9cbe745
--- /dev/null
+++ b/nav2_map_server/test/map_server_params.yaml
@@ -0,0 +1,3 @@
+map_server:
+ ros__parameters:
+ yaml_filename: "../testmap.yaml"
diff --git a/nav2_map_server/test/test_constants.cpp b/nav2_map_server/test/test_constants.cpp
index 5d17cb8b312e8dff5ffddce249d116130e11605e..0a49c713e862a6e0f13dc9329e5f0ba23ebfcf3b 100644
--- a/nav2_map_server/test/test_constants.cpp
+++ b/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;
diff --git a/nav2_map_server/test/test_constants/test_constants.h b/nav2_map_server/test/test_constants/test_constants.h
index 08c7da9d8df797713b922977ec398641768fcb3b..7aeafc8708f38546b41d60d7f5f05065dd1dc6a1 100644
--- a/nav2_map_server/test/test_constants/test_constants.h
+++ b/nav2_map_server/test/test_constants/test_constants.h
@@ -38,7 +38,6 @@ extern const unsigned int g_valid_image_height;
extern const char g_valid_image_content[];
extern const char * g_valid_png_file;
extern const char * g_valid_bmp_file;
-extern const char * g_valid_yaml_file;
extern const float g_valid_image_res;
diff --git a/nav2_map_server/test/test_launch_files/map_server_node.launch.py b/nav2_map_server/test/test_launch_files/map_server_node.launch.py
index 5c04cf64f5d9aa312f4fc78b939fd90ccf06b858..dc6ba7880edbadee4dd8d9389cf53b32bac3078a 100644
--- a/nav2_map_server/test/test_launch_files/map_server_node.launch.py
+++ b/nav2_map_server/test/test_launch_files/map_server_node.launch.py
@@ -26,5 +26,6 @@ def generate_launch_description():
package='nav2_map_server',
node_executable='map_server',
output='screen',
- arguments=[os.path.join(os.getenv('TEST_DIR'), 'testmap.yaml')])
+ arguments=['__params:=' + os.path.join(os.getenv('TEST_DIR'),
+ 'map_server_params.yaml')])
])
diff --git a/nav2_map_server/test/testmap.yaml b/nav2_map_server/test/testmap.yaml
index 8cf0d7304f2a363f48f8a0eed0c0cf6f85e57b80..fe6b2a681bbfa5dabdc822ac8fff9bf7d3930dc8 100644
--- a/nav2_map_server/test/testmap.yaml
+++ b/nav2_map_server/test/testmap.yaml
@@ -1,6 +1,8 @@
-image: testmap.png
+map_type: "occupancy"
+image: "testmap.png"
resolution: 0.1
origin: [2.0, 3.0, 1.0]
-negate: 0
occupied_thresh: 0.65
free_thresh: 0.196
+negate: 0
+mode: "trinary"
diff --git a/nav2_map_server/test/unit/test_occ_grid.cpp b/nav2_map_server/test/unit/test_occ_grid.cpp
index 6b47e38cf8e247a521c7501f88b73e7a07abaca1..0291f078dfc72787453fb5f3847a9da6d4e5124e 100644
--- a/nav2_map_server/test/unit/test_occ_grid.cpp
+++ b/nav2_map_server/test/unit/test_occ_grid.cpp
@@ -33,8 +33,10 @@
#include <experimental/filesystem>
#include <stdexcept>
#include <string>
+#include <vector>
+#include <memory>
-#include "nav2_map_server/map_representations/occ_grid_server.hpp"
+#include "nav2_map_server/occ_grid_loader.hpp"
#include "test_constants/test_constants.h"
#define TEST_DIR TEST_DIRECTORY
@@ -42,28 +44,51 @@
using namespace std; // NOLINT
using std::experimental::filesystem::path;
-class OccGridTest : public nav2_map_server::OccGridServer
+class RclCppFixture
{
public:
- OccGridTest() {}
- ~OccGridTest() {}
+ RclCppFixture() {rclcpp::init(0, nullptr);}
+ ~RclCppFixture() {rclcpp::shutdown();}
+};
+
+RclCppFixture g_rclcppfixture;
+
+class TestMapLoader : public nav2_map_server::OccGridLoader
+{
+public:
+ explicit TestMapLoader(rclcpp::Node * node, YAML::Node & doc)
+ : OccGridLoader(node, doc)
+ {
+ }
- nav_msgs::srv::GetMap::Response GetMap() {return occ_resp_;}
+ nav_msgs::msg::OccupancyGrid getOccupancyGrid()
+ {
+ return msg_;
+ }
};
-class MapServerTest : public ::testing::Test
+class MapLoaderTest : public ::testing::Test
{
public:
- MapServerTest()
+ MapLoaderTest()
{
- OccTest = new OccGridTest;
- auto test_yaml = path(TEST_DIR) / path(g_valid_yaml_file);
- OccTest->LoadMapInfoFromFile(test_yaml.string());
+ // Set up a fake YAML document with the required fields
+ doc_["resolution"] = "0.1";
+ doc_["origin"][0] = "2.0";
+ doc_["origin"][1] = "3.0";
+ doc_["origin"][2] = "1.0";
+ doc_["negate"] = "0";
+ doc_["occupied_thresh"] = "0.65";
+ doc_["free_thresh"] = "0.196";
+
+ node_ = std::make_shared<rclcpp::Node>("map_server");
+ map_loader_ = new TestMapLoader(node_.get(), doc_);
}
protected:
- OccGridTest * OccTest;
- nav_msgs::srv::GetMap::Response map_resp;
+ rclcpp::Node::SharedPtr node_;
+ TestMapLoader * map_loader_;
+ YAML::Node doc_;
};
/* Try to load a valid PNG file. Succeeds if no exception is thrown, and if
@@ -72,43 +97,44 @@ protected:
* This test can fail on OS X, due to an apparent limitation of the
* underlying SDL_Image library. */
-TEST_F(MapServerTest, loadValidPNG)
+TEST_F(MapLoaderTest, loadValidPNG)
{
auto test_png = path(TEST_DIR) / path(g_valid_png_file);
- ASSERT_NO_THROW(OccTest->LoadMapFromFile(test_png.string()));
- OccTest->SetMap();
- map_resp = OccTest->GetMap();
-
- EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
- EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
- EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
- for (unsigned int i = 0; i < map_resp.map.info.width * map_resp.map.info.height; i++) {
- EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
+
+ ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_png.string()));
+ nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid();
+
+ EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+ EXPECT_EQ(map_msg.info.width, g_valid_image_width);
+ EXPECT_EQ(map_msg.info.height, g_valid_image_height);
+ for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+ EXPECT_EQ(g_valid_image_content[i], map_msg.data[i]);
}
}
/* Try to load a valid BMP file. Succeeds if no exception is thrown, and if
* the loaded image matches the known dimensions and content of the file. */
-TEST_F(MapServerTest, loadValidBMP)
+TEST_F(MapLoaderTest, loadValidBMP)
{
auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file);
- ASSERT_NO_THROW(OccTest->LoadMapFromFile(test_bmp.string()));
- OccTest->SetMap();
- map_resp = OccTest->GetMap();
-
- EXPECT_FLOAT_EQ(map_resp.map.info.resolution, g_valid_image_res);
- EXPECT_EQ(map_resp.map.info.width, g_valid_image_width);
- EXPECT_EQ(map_resp.map.info.height, g_valid_image_height);
- for (unsigned int i = 0; i < map_resp.map.info.width * map_resp.map.info.height; i++) {
- EXPECT_EQ(g_valid_image_content[i], map_resp.map.data[i]);
+
+
+ ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_bmp.string()));
+ nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid();
+
+ EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
+ EXPECT_EQ(map_msg.info.width, g_valid_image_width);
+ EXPECT_EQ(map_msg.info.height, g_valid_image_height);
+ for (unsigned int i = 0; i < map_msg.info.width * map_msg.info.height; i++) {
+ EXPECT_EQ(g_valid_image_content[i], map_msg.data[i]);
}
}
/* Try to load an invalid file. Succeeds if a std::runtime exception is thrown */
-TEST_F(MapServerTest, loadInvalidFile)
+TEST_F(MapLoaderTest, loadInvalidFile)
{
auto test_invalid = path(TEST_DIR) / path("foo");
- ASSERT_THROW(OccTest->LoadMapFromFile(test_invalid.string()), std::runtime_error);
+ ASSERT_THROW(map_loader_->loadMapFromFile(test_invalid.string()), std::runtime_error);
}