Port the map server to lifecycle nodes (#649)

* Port the map server to lifecycle nodes
* Address review feedback

nav2_map_server/CMakeLists.txt

@@ -10,6 +10,7 @@ find_package(SDL_image REQUIRED)
 find_package(yaml_cpp_vendor REQUIRED)
 find_package(std_msgs REQUIRED)
 find_package(tf2 REQUIRED)
+find_package(nav2_lifecycle REQUIRED)
 find_package(nav2_util REQUIRED)
 
 nav2_package()
@@ -47,6 +48,7 @@ set(map_server_dependencies
   std_msgs
   tf2
   nav2_util
+  nav2_lifecycle
 )
 
 set(map_saver_dependencies

nav2_map_server/include/nav2_map_server/map_server.hpp

@@ -15,38 +15,30 @@
 #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"
+#include "nav2_lifecycle/lifecycle_node.hpp"
+#include "nav2_lifecycle/lifecycle_helper_interface.hpp"
 
 namespace nav2_map_server
 {
 
-class MapServer : public rclcpp::Node
+class MapServer : public nav2_lifecycle::LifecycleNode
 {
 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_;
+  ~MapServer();
+
+protected:
+  // Implement the lifecycle interface
+  nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_shutdown(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_error(const rclcpp_lifecycle::State & state) override;
+
+  // The map loader that will actually do the work
+  std::unique_ptr<nav2_lifecycle::LifecycleHelperInterface> map_loader_;
 };
 
 }  // namespace nav2_map_server

nav2_map_server/include/nav2_map_server/occ_grid_loader.hpp

@@ -15,68 +15,68 @@
 #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 "nav2_map_server/map_loader.hpp"
-#include "yaml-cpp/yaml.h"
+#include "nav2_lifecycle/lifecycle_node.hpp"
 
 namespace nav2_map_server
 {
 
-class OccGridLoader : public MapLoader
+class OccGridLoader : public nav2_lifecycle::LifecycleHelperInterface
 {
 public:
-  explicit OccGridLoader(rclcpp::Node * node, YAML::Node & doc);
+  OccGridLoader(rclcpp_lifecycle::LifecycleNode::SharedPtr node, std::string & yaml_filename);
   OccGridLoader() = delete;
+  ~OccGridLoader();
 
-  // 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;
+  nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State & state) override;
+  nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State & state) override;
 
 protected:
   // The ROS node to use for ROS-related operations such as creating a service
-  rclcpp::Node * node_;
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
+
+  // The name of the YAML file from which to get the conversion parameters
+  std::string yaml_filename_;
 
-  // The YAML file from which to get the conversion parameters
-  YAML::Node & doc_;
+  typedef enum { TRINARY, SCALE, RAW } MapMode;
+  typedef struct
+  {
+    double resolution{0};
+    std::vector<double> origin{0, 0, 0};
+    double free_thresh{0};
+    double occupied_thresh{0};
+    MapMode mode{TRINARY};
+    int negate{0};
+  } LoadParameters;
 
-  // 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_;
+  // Load the image and generate an OccupancyGrid
+  void loadMapFromFile(const std::string & filename, LoadParameters * loadParameters);
 
-  // A service to provide the ouccpancy grid (GetMap) and the message to return
+  // A service to provide the occupancy 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_;
+  rclcpp_lifecycle::LifecyclePublisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occ_pub_;
 
   // The message to publish on the occupancy grid topic
-  nav_msgs::msg::OccupancyGrid msg_;
+  std::unique_ptr<nav_msgs::msg::OccupancyGrid> msg_;
 
   // The frame ID used in the returned OccupancyGrid message
-  static const char * frame_id_;
+  static constexpr const char * frame_id_{"map"};
 
   // The name for the topic on which the map will be published
-  static const char * topic_name_;
+  static constexpr const char * topic_name_{"map"};
 
   // 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_;
+  static constexpr const char * service_name_{"map"};
 };
 
 }  // namespace nav2_map_server

nav2_map_server/package.xml

@@ -10,7 +10,6 @@
   <license>Apache License 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
-  <build_depend>nav2_common</build_depend>
 
   <depend>nav_msgs</depend>
   <depend>std_msgs</depend>
@@ -22,6 +21,7 @@
   <depend>launch_testing</depend>
   <depend>tf2</depend>
   <depend>nav2_util</depend>
+  <depend>nav2_lifecycle</depend>
 
   <test_depend>ament_lint_common</test_depend>
   <test_depend>ament_lint_auto</test_depend>

nav2_map_server/src/main.cpp

@@ -12,11 +12,12 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#include <string>
 #include <memory>
 #include <stdexcept>
-#include "rclcpp/rclcpp.hpp"
+#include <string>
+
 #include "nav2_map_server/map_server.hpp"
+#include "rclcpp/rclcpp.hpp"
 
 int main(int argc, char ** argv)
 {
@@ -24,12 +25,13 @@ int main(int argc, char ** argv)
 
   try {
     rclcpp::init(argc, argv);
-    rclcpp::spin(std::make_shared<nav2_map_server::MapServer>(node_name));
+    auto node = std::make_shared<nav2_map_server::MapServer>();
+    rclcpp::spin(node->get_node_base_interface());
     rclcpp::shutdown();
+    return 0;
   } 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 -1;
   }
-
-  return 0;
 }

nav2_map_server/src/map_server.cpp

@@ -14,11 +14,11 @@
 
 #include "nav2_map_server/map_server.hpp"
 
-#include <libgen.h>
-#include <string>
-#include <memory>
 #include <fstream>
+#include <memory>
 #include <stdexcept>
+#include <string>
+
 #include "nav2_map_server/occ_grid_loader.hpp"
 #include "nav2_util/node_utils.hpp"
 #include "yaml-cpp/yaml.h"
@@ -28,66 +28,99 @@ using namespace std::chrono_literals;
 namespace nav2_map_server
 {
 
-MapServer::MapServer(const std::string & node_name)
-: Node(node_name, nav2_util::get_node_options_default())
+MapServer::MapServer()
+: nav2_lifecycle::LifecycleNode("map_server")
 {
-  // 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);
-  }
+  RCLCPP_INFO(get_logger(), "Creating");
 
-  // Load the map and provide access via topic and service interfaces
-  map_loader_->loadMapFromFile(map_filename_);
-  map_loader_->startServices();
+  // Declare the node parameters
+  declare_parameter("yaml_filename", rclcpp::ParameterValue(std::string("map.yaml")));
 }
 
-MapServer::MapServer()
-: MapServer("map_server")
+MapServer::~MapServer()
 {
+  RCLCPP_INFO(get_logger(), "Destroying");
 }
 
-void MapServer::getParameters()
+nav2_lifecycle::CallbackReturn
+MapServer::on_configure(const rclcpp_lifecycle::State & state)
 {
-  get_parameter_or_set("yaml_filename", yaml_filename_, std::string("map.yaml"));
+  RCLCPP_INFO(get_logger(), "Configuring");
+
+  // Get the name of the YAML file to use
+  std::string yaml_filename;
+  get_parameter("yaml_filename", yaml_filename);
 
   // Make sure that there's a valid file there and open it up
-  std::ifstream fin(yaml_filename_.c_str());
+  std::ifstream fin(yaml_filename.c_str());
   if (fin.fail()) {
-    throw std::runtime_error("Could not open '" + yaml_filename_ + "': file not found");
+    throw std::runtime_error("Could not open '" + yaml_filename + "': file not found");
   }
 
-  doc_ = YAML::LoadFile(yaml_filename_);
+  // The YAML document from which to get the conversion parameters
+  YAML::Node doc = YAML::LoadFile(yaml_filename);
 
-  // Get the name of the map file
+  // Get the map type so that we can create the correct map loader
+  std::string map_type;
   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);
-    }
+    map_type = doc["map_type"].as<std::string>();
   } catch (YAML::Exception) {
-    std::string msg = "'" + yaml_filename_ + "' does not contain an image tag or it is invalid";
+    // Default to occupancy grid if not specified in the YAML file
+    map_type = "occupancy";
+  }
+
+  // Create the correct map loader for the specified map type
+  if (map_type == "occupancy") {
+    map_loader_ = std::make_unique<OccGridLoader>(shared_from_this(), yaml_filename);
+  } else {
+    std::string msg = "Cannot load unknown map type: '" + map_type + "'";
     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) {
-    // Default to occupancy grid if not specified in the YAML file
-    map_type_ = "occupancy";
+  map_loader_->on_configure(state);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+MapServer::on_activate(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Activating");
+  map_loader_->on_activate(state);
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+MapServer::on_deactivate(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Deactivating");
+  map_loader_->on_deactivate(state);
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
 }
+
+nav2_lifecycle::CallbackReturn
+MapServer::on_cleanup(const rclcpp_lifecycle::State & state)
+{
+  RCLCPP_INFO(get_logger(), "Cleaning up");
+
+  map_loader_->on_cleanup(state);
+  map_loader_.reset();
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+MapServer::on_error(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_ERROR(get_logger(), "Handling error state");
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+MapServer::on_shutdown(const rclcpp_lifecycle::State &)
+{
+  RCLCPP_INFO(get_logger(), "Shutting down");
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
 }
 
 }  // namespace nav2_map_server

nav2_map_server/src/occ_grid_loader.cpp

@@ -27,100 +27,195 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-/*
- * This file contains helper functions for loading images as maps.
- *
- * Author: Brian Gerkey
- */
+// 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 <libgen.h>
 #include <memory>
 #include <stdexcept>
+#include <string>
+#include <vector>
 
 #include "tf2/LinearMath/Quaternion.h"
 #include "SDL/SDL_image.h"
+#include "yaml-cpp/yaml.h"
 
 using namespace std::chrono_literals;
 
 namespace nav2_map_server
 {
 
-const char * OccGridLoader::frame_id_ = "map";
-const char * OccGridLoader::topic_name_ = "map";
-const char * OccGridLoader::service_name_ = "map";
+OccGridLoader::OccGridLoader(
+  rclcpp_lifecycle::LifecycleNode::SharedPtr node,
+  std::string & yaml_filename)
+: node_(node), yaml_filename_(yaml_filename)
+{
+  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Creating");
+}
 
-OccGridLoader::OccGridLoader(rclcpp::Node * node, YAML::Node & doc)
-: node_(node), doc_(doc), origin_(3)
+OccGridLoader::~OccGridLoader()
 {
+  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Destroying");
+}
+
+nav2_lifecycle::CallbackReturn
+OccGridLoader::on_configure(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Configuring");
+
+  msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+
+  // The YAML document from which to get the conversion parameters
+  YAML::Node doc = YAML::LoadFile(yaml_filename_);
+  LoadParameters loadParameters;
+
+  // Get the name of the map file
+  std::string map_filename;
   try {
-    resolution_ = doc_["resolution"].as<double>();
+    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) {
+    throw std::runtime_error("The map does not contain an image tag or it is invalid");
+  }
+
+  try {
+    loadParameters.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>();
+    loadParameters.origin[0] = doc["origin"][0].as<double>();
+    loadParameters.origin[1] = doc["origin"][1].as<double>();
+    loadParameters.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>();
+    loadParameters.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>();
+    loadParameters.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>();
+    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;
+      loadParameters.mode = TRINARY;
     } else if (mode_str == "scale") {
-      mode_ = SCALE;
+      loadParameters.mode = SCALE;
     } else if (mode_str == "raw") {
-      mode_ = RAW;
+      loadParameters.mode = RAW;
     } else {
       RCLCPP_WARN(node_->get_logger(),
         "Mode parameter not recognized: '%s', using default value (trinary)",
         mode_str.c_str());
-      mode_ = TRINARY;
+      loadParameters.mode = TRINARY;
     }
   } catch (YAML::Exception &) {
     RCLCPP_WARN(node_->get_logger(), "Mode parameter not set, using default value (trinary)");
-    mode_ = TRINARY;
+    loadParameters.mode = TRINARY;
   }
 
   try {
-    negate_ = doc_["negate"].as<int>();
+    loadParameters.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(), "resolution: %f", loadParameters.resolution);
+  RCLCPP_DEBUG(node_->get_logger(), "origin[0]: %f", loadParameters.origin[0]);
+  RCLCPP_DEBUG(node_->get_logger(), "origin[1]: %f", loadParameters.origin[1]);
+  RCLCPP_DEBUG(node_->get_logger(), "origin[2]: %f", loadParameters.origin[2]);
+  RCLCPP_DEBUG(node_->get_logger(), "free_thresh: %f", loadParameters.free_thresh);
+  RCLCPP_DEBUG(node_->get_logger(), "occupied_thresh: %f", loadParameters.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_);
+  RCLCPP_DEBUG(node_->get_logger(), "mode: %d", loadParameters.mode);
+  RCLCPP_DEBUG(node_->get_logger(), "negate: %d", loadParameters.negate);
+
+  loadMapFromFile(map_filename, &loadParameters);
+
+  // 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(), "OccGridLoader: 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);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+OccGridLoader::on_activate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Activating");
+
+  // Publish the map using the latched topic
+  occ_pub_->on_activate();
+  occ_pub_->publish(*msg_);
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+OccGridLoader::on_deactivate(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Deactivating");
+
+  occ_pub_->on_deactivate();
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
+}
+
+nav2_lifecycle::CallbackReturn
+OccGridLoader::on_cleanup(const rclcpp_lifecycle::State & /*state*/)
+{
+  RCLCPP_INFO(node_->get_logger(), "OccGridLoader: Cleaning up");
+
+  occ_pub_.reset();
+  occ_service_.reset();
+  msg_.reset();
+
+  return nav2_lifecycle::CallbackReturn::SUCCESS;
 }
 
-void OccGridLoader::loadMapFromFile(const std::string & map_name)
+void
+OccGridLoader::loadMapFromFile(const std::string & map_name, LoadParameters * loadParameters)
 {
+  RCLCPP_DEBUG(node_->get_logger(), "OccGridLoader: loadMapFromFile");
+
   // 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()))) {
@@ -130,21 +225,21 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name)
   }
 
   // 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;
+  msg_->info.width = img->w;
+  msg_->info.height = img->h;
+  msg_->info.resolution = loadParameters->resolution;
+  msg_->info.origin.position.x = loadParameters->origin[0];
+  msg_->info.origin.position.y = loadParameters->origin[1];
+  msg_->info.origin.position.z = 0.0;
   tf2::Quaternion q;
-  q.setRPY(0, 0, origin_[2]);
-  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();
+  q.setRPY(0, 0, loadParameters->origin[2]);
+  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);
+  msg_->data.resize(msg_->info.width * msg_->info.height);
 
   // Get values that we'll need to iterate through the pixels
   int rowstride = img->pitch;
@@ -153,7 +248,7 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name)
   // 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) {
+  if (loadParameters->mode == TRINARY || !img->format->Amask) {
     avg_channels = n_channels;
   } else {
     avg_channels = n_channels - 1;
@@ -162,8 +257,8 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name)
   // 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++) {
+  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;
@@ -179,16 +274,16 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name)
         alpha = *(p + n_channels - 1);
       }
 
-      if (negate_) {
+      if (loadParameters->negate) {
         color_avg = 255 - color_avg;
       }
 
 #define MAP_IDX(sx, i, j) ((sx) * (j) + (i))
 
       unsigned char value;
-      if (mode_ == RAW) {
+      if (loadParameters->mode == RAW) {
         value = color_avg;
-        msg_.data[MAP_IDX(msg_.info.width, i, msg_.info.height - j - 1)] = value;
+        msg_->data[MAP_IDX(msg_->info.width, i, msg_->info.height - j - 1)] = value;
         continue;
       }
 
@@ -199,65 +294,33 @@ void OccGridLoader::loadMapFromFile(const std::string & map_name)
       // 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_) {
+      if (occ > loadParameters->occupied_thresh) {
         value = +100;
-      } else if (occ < free_thresh_) {
+      } else if (occ < loadParameters->free_thresh) {
         value = 0;
-      } else if (mode_ == TRINARY || alpha < 1.0) {
+      } else if (loadParameters->mode == TRINARY || alpha < 1.0) {
         value = -1;
       } else {
-        double ratio = (occ - free_thresh_) / (occupied_thresh_ - free_thresh_);
+        double ratio = (occ - loadParameters->free_thresh) /
+          (loadParameters->occupied_thresh - loadParameters->free_thresh);
         value = 99 * ratio;
       }
 
-      msg_.data[MAP_IDX(msg_.info.width, i, msg_.info.height - j - 1)] = value;
+      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();
+  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);
+    msg_->info.width,
+    msg_->info.height,
+    msg_->info.resolution);
 }
 
 }  // namespace nav2_map_server

nav2_map_server/test/component/test_occ_grid_node.cpp

@@ -16,10 +16,11 @@
 #include <rclcpp/rclcpp.hpp>
 #include <memory>
 
-#include "nav2_map_server/occ_grid_loader.hpp"
 #include "test_constants/test_constants.h"
+#include "nav2_map_server/occ_grid_loader.hpp"
+#include "nav2_util/lifecycle_service_client.hpp"
 
-// TODO(bpwilcox): test publisher
+using lifecycle_msgs::msg::Transition;
 
 class RclCppFixture
 {
@@ -27,6 +28,7 @@ public:
   RclCppFixture() {rclcpp::init(0, nullptr);}
   ~RclCppFixture() {rclcpp::shutdown();}
 };
+
 RclCppFixture g_rclcppfixture;
 
 class TestNode : public ::testing::Test
@@ -35,6 +37,17 @@ public:
   TestNode()
   {
     node_ = rclcpp::Node::make_shared("map_client_test");
+    lifecycle_client_ =
+      std::make_shared<nav2_util::LifecycleServiceClient>("map_server", node_);
+
+    lifecycle_client_->change_state(Transition::TRANSITION_CONFIGURE);
+    lifecycle_client_->change_state(Transition::TRANSITION_ACTIVATE);
+  }
+
+  ~TestNode()
+  {
+    lifecycle_client_->change_state(Transition::TRANSITION_DEACTIVATE);
+    lifecycle_client_->change_state(Transition::TRANSITION_CLEANUP);
   }
 
   template<class T>
@@ -45,29 +58,35 @@ public:
     typename T::Request::SharedPtr request)
   {
     auto result = client->async_send_request(request);
-    // Wait for the result.
+
+    // Wait for the result
     if (rclcpp::spin_until_future_complete(node, result) ==
       rclcpp::executor::FutureReturnCode::SUCCESS)
     {
       return result.get();
     } else {
-      return NULL;
+      return nullptr;
     }
   }
 
 protected:
   rclcpp::Node::SharedPtr node_;
+  std::shared_ptr<nav2_util::LifecycleServiceClient> lifecycle_client_;
 };
 
 TEST_F(TestNode, ResultReturned)
 {
   auto req = std::make_shared<nav_msgs::srv::GetMap::Request>();
   auto client = node_->create_client<nav_msgs::srv::GetMap>("map");
+
   ASSERT_TRUE(client->wait_for_service());
+
   auto resp = send_request<nav_msgs::srv::GetMap>(node_, client, req);
+
   ASSERT_FLOAT_EQ(resp->map.info.resolution, g_valid_image_res);
   ASSERT_EQ(resp->map.info.width, g_valid_image_width);
   ASSERT_EQ(resp->map.info.height, g_valid_image_height);
+
   for (unsigned int i = 0; i < resp->map.info.width * resp->map.info.height; i++) {
     ASSERT_EQ(g_valid_image_content[i], resp->map.data[i]);
   }

nav2_map_server/test/unit/test_occ_grid.cpp

@@ -35,8 +35,12 @@
 #include <string>
 #include <vector>
 #include <memory>
+#include <iostream>
+#include <fstream>
 
+#include "yaml-cpp/yaml.h"
 #include "nav2_map_server/occ_grid_loader.hpp"
+#include "nav2_lifecycle/lifecycle_node.hpp"
 #include "test_constants/test_constants.h"
 
 #define TEST_DIR TEST_DIRECTORY
@@ -55,15 +59,43 @@ RclCppFixture g_rclcppfixture;
 
 class TestMapLoader : public nav2_map_server::OccGridLoader
 {
+  FRIEND_TEST(MapLoaderTest, loadValidPNG);
+  FRIEND_TEST(MapLoaderTest, loadValidBMP);
+  FRIEND_TEST(MapLoaderTest, loadInvalidFile);
+
 public:
-  explicit TestMapLoader(rclcpp::Node * node, YAML::Node & doc)
-  : OccGridLoader(node, doc)
+  explicit TestMapLoader(nav2_lifecycle::LifecycleNode::SharedPtr node, std::string yaml_filename)
+  : OccGridLoader(node, yaml_filename)
   {
   }
 
   nav_msgs::msg::OccupancyGrid getOccupancyGrid()
   {
-    return msg_;
+    return *msg_;
+  }
+};
+
+class FakeMapServer : public nav2_lifecycle::LifecycleNode
+{
+public:
+  FakeMapServer()
+  : nav2_lifecycle::LifecycleNode("FakeMapServer") {}
+
+  nav2_lifecycle::CallbackReturn on_configure(const rclcpp_lifecycle::State &) override
+  {
+    return nav2_lifecycle::CallbackReturn::SUCCESS;
+  }
+  nav2_lifecycle::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override
+  {
+    return nav2_lifecycle::CallbackReturn::SUCCESS;
+  }
+  nav2_lifecycle::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override
+  {
+    return nav2_lifecycle::CallbackReturn::SUCCESS;
+  }
+  nav2_lifecycle::CallbackReturn on_cleanup(const rclcpp_lifecycle::State &) override
+  {
+    return nav2_lifecycle::CallbackReturn::SUCCESS;
   }
 };
 
@@ -73,35 +105,53 @@ public:
   MapLoaderTest()
   {
     // 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_);
+
+    std::string temp_name("/tmp/map_unit_test.yaml");
+    std::ofstream params_file;
+    params_file.open(temp_name, std::ofstream::out | std::ofstream::trunc);
+
+    params_file << "resolution: 0.1" << std::endl;
+    params_file << "origin: [2.0, 3.0, 1.0]" << std::endl;
+    params_file << "negate: 0" << std::endl;
+    params_file << "occupied_thresh: 0.65" << std::endl;
+    params_file << "free_thresh: 0.196" << std::endl;
+
+    params_file.close();
+
+    node_ = std::make_shared<FakeMapServer>();
+    map_loader_ = std::make_unique<TestMapLoader>(node_, temp_name);
   }
 
 protected:
-  rclcpp::Node::SharedPtr node_;
-  TestMapLoader * map_loader_;
-  YAML::Node doc_;
+  nav2_lifecycle::LifecycleNode::SharedPtr node_;
+  std::unique_ptr<TestMapLoader> map_loader_;
 };
 
-/* Try to load a valid PNG file.  Succeeds if no exception is thrown, and if
- * the loaded image matches the known dimensions and content of the file.
- *
- * This test can fail on OS X, due to an apparent limitation of the
- * underlying SDL_Image library. */
+// Try to load a valid PNG file.  Succeeds if no exception is thrown, and if
+// the loaded image matches the known dimensions and content of the file.
+//
+// This test can fail on OS X, due to an apparent limitation of the
+// underlying SDL_Image library.
 
 TEST_F(MapLoaderTest, loadValidPNG)
 {
   auto test_png = path(TEST_DIR) / path(g_valid_png_file);
 
-  ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_png.string()));
+  nav2_map_server::OccGridLoader::LoadParameters loadParameters;
+  loadParameters.resolution = g_valid_image_res;
+  loadParameters.origin[0] = 0;
+  loadParameters.origin[1] = 0;
+  loadParameters.origin[2] = 0;
+  loadParameters.free_thresh = 0.196;
+  loadParameters.occupied_thresh = 0.65;
+  loadParameters.mode = nav2_map_server::OccGridLoader::TRINARY;
+  loadParameters.negate = 0;
+
+  // In order to loadMapFromFile without going through the Configure and Activate states,
+  // the msg_ member must be initialized
+  map_loader_->msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+
+  ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_png.string(), &loadParameters));
   nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid();
 
   EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
@@ -112,15 +162,28 @@ TEST_F(MapLoaderTest, loadValidPNG)
   }
 }
 
-/* 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. */
+// 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(MapLoaderTest, loadValidBMP)
 {
   auto test_bmp = path(TEST_DIR) / path(g_valid_bmp_file);
 
-
-  ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_bmp.string()));
+  nav2_map_server::OccGridLoader::LoadParameters loadParameters;
+  loadParameters.resolution = g_valid_image_res;
+  loadParameters.origin[0] = 0;
+  loadParameters.origin[1] = 0;
+  loadParameters.origin[2] = 0;
+  loadParameters.free_thresh = 0.196;
+  loadParameters.occupied_thresh = 0.65;
+  loadParameters.mode = nav2_map_server::OccGridLoader::TRINARY;
+  loadParameters.negate = 0;
+
+  // In order to loadMapFromFile without going through the Configure and Activate states,
+  // the msg_ member must be initialized
+  map_loader_->msg_ = std::make_unique<nav_msgs::msg::OccupancyGrid>();
+
+  ASSERT_NO_THROW(map_loader_->loadMapFromFile(test_bmp.string(), &loadParameters));
   nav_msgs::msg::OccupancyGrid map_msg = map_loader_->getOccupancyGrid();
 
   EXPECT_FLOAT_EQ(map_msg.info.resolution, g_valid_image_res);
@@ -131,10 +194,22 @@ TEST_F(MapLoaderTest, loadValidBMP)
   }
 }
 
-/* Try to load an invalid file.  Succeeds if a std::runtime exception is thrown */
+// Try to load an invalid file.  Succeeds if a std::runtime exception is thrown
 
 TEST_F(MapLoaderTest, loadInvalidFile)
 {
   auto test_invalid = path(TEST_DIR) / path("foo");
-  ASSERT_THROW(map_loader_->loadMapFromFile(test_invalid.string()), std::runtime_error);
+
+  nav2_map_server::OccGridLoader::LoadParameters loadParameters;
+  loadParameters.resolution = g_valid_image_res;
+  loadParameters.origin[0] = 0;
+  loadParameters.origin[1] = 0;
+  loadParameters.origin[2] = 0;
+  loadParameters.free_thresh = 0.196;
+  loadParameters.occupied_thresh = 0.65;
+  loadParameters.mode = nav2_map_server::OccGridLoader::TRINARY;
+  loadParameters.negate = 0;
+
+  ASSERT_THROW(map_loader_->loadMapFromFile(
+      test_invalid.string(), &loadParameters), std::runtime_error);
 }