Switch from map service to topic (#885)

* Add parameter to disable map server

* Move parameter check to getParameters

* Uncrustify

* Don't mark static layer as current until we have a map

* Make sure static layer gets initialized

Make sure static layer gets initialized, even with an empty layer, so it doesn't crash if you disable the map service and enable the obstacle layer

* remove map service from static layer

Only use map topic instead
Subscribe to map topic with default transient local volatility
Add parameter "map_subscribe_transient_local" (default=true) for backwards compatibility with volatile publishers

* Change AMCL to use only the map topic, not map service

* remove now-unused MapServiceClient

* uncrustify

* only set current_=true when first map received

* Fix nav2_costmap inflation tests.

* Fixing collision tester test.

* Revert "uncrustify"

This reverts commit 82f88a7.
This uncrustify failure is enforced differently depending on whether
we are on the dashing branch or on the master branch. This particular
commit is only appropriate on the dashing branch, but is already present
there.

nav2_amcl/include/nav2_amcl/amcl_node.hpp

@@ -32,7 +32,6 @@
 #include "geometry_msgs/msg/pose_stamped.hpp"
 #include "message_filters/subscriber.h"
 #include "nav2_util/lifecycle_node.hpp"
-#include "nav2_util/map_service_client.hpp"
 #include "nav2_util/motion_model/motion_model.hpp"
 #include "nav2_util/sensors/laser/laser.hpp"
 #include "nav_msgs/srv/set_map.hpp"
@@ -80,15 +79,12 @@ class AmclNode : public nav2_util::LifecycleNode
   } amcl_hyp_t;
 
   // Map-related
-  void initMap();
   void mapReceived(const nav_msgs::msg::OccupancyGrid::SharedPtr msg);
   void handleMapMessage(const nav_msgs::msg::OccupancyGrid & msg);
   void createFreeSpaceVector();
   void freeMapDependentMemory();
-  nav2_util::MapServiceClient map_client_{"amcl"};
   map_t * map_{nullptr};
   map_t * convertMap(const nav_msgs::msg::OccupancyGrid & map_msg);
-  bool use_map_topic_{true};
   bool first_map_only_{true};
   bool first_map_received_{false};
   amcl_hyp_t * initial_pose_hyp_;

nav2_amcl/src/amcl_node.cpp

@@ -31,7 +31,6 @@
 #include "message_filters/subscriber.h"
 #include "nav2_util/angleutils.hpp"
 #include "nav2_util/duration_conversions.hpp"
-#include "nav2_util/map_service_client.hpp"
 #include "nav2_util/pf/pf.hpp"
 #include "nav2_util/string_utils.hpp"
 #include "nav2_util/sensors/laser/laser.hpp"
@@ -108,9 +107,6 @@ AmclNode::on_configure(const rclcpp_lifecycle::State & /*state*/)
   RCLCPP_INFO(get_logger(), "Configuring");
 
   initParameters();
-  if (!use_map_topic_) {
-    initMap();
-  }
   initTransforms();
   initMessageFilters();
   initPubSub();
@@ -917,7 +913,6 @@ AmclNode::initParameters()
   get_parameter("z_max", z_max_);
   get_parameter("z_rand", z_rand_);
   get_parameter("z_short", z_short_);
-  get_parameter("use_map_topic_", use_map_topic_);
   get_parameter("first_map_only_", first_map_only_);
 
   save_pose_period_ = tf2::durationFromSec(1.0 / save_pose_rate);
@@ -1031,37 +1026,6 @@ AmclNode::freeMapDependentMemory()
   lasers_.clear();
 }
 
-void
-AmclNode::initMap()
-{
-  RCLCPP_INFO(get_logger(), "Requesting map from the map service");
-
-  // Get the map from the map server
-  nav_msgs::msg::OccupancyGrid msg;
-  if (!map_client_.getMap(msg)) {
-    throw "Failed to get map from the map server";
-  }
-
-  RCLCPP_INFO(get_logger(), "Received a %dx%d map @ %.3f m/pix",
-    msg.info.width, msg.info.height, msg.info.resolution);
-
-  // Perform a check on the map's frame_id
-  if (msg.header.frame_id != global_frame_id_) {
-    RCLCPP_WARN(get_logger(), "Frame_id of map received:'%s' doesn't match global_frame_id:'%s'."
-      " This could cause issues with reading published topics",
-      msg.header.frame_id.c_str(), global_frame_id_.c_str());
-  }
-
-  first_map_received_ = true;
-
-  // Convert to our own local data structure
-  map_ = convertMap(msg);
-
-#if NEW_UNIFORM_SAMPLING
-  createFreeSpaceVector();
-#endif
-}
-
 // Convert an OccupancyGrid map message into the internal representation. This function
 // allocates a map_t and returns it.
 map_t *
@@ -1136,13 +1100,11 @@ AmclNode::initPubSub()
     "initialpose", rclcpp::SystemDefaultsQoS(),
     std::bind(&AmclNode::initialPoseReceived, this, std::placeholders::_1));
 
-  if (use_map_topic_) {
-    map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>("map",
-        rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
-        std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
+  map_sub_ = create_subscription<nav_msgs::msg::OccupancyGrid>(
+    "map", rclcpp::QoS(rclcpp::KeepLast(1)).transient_local().reliable(),
+    std::bind(&AmclNode::mapReceived, this, std::placeholders::_1));
 
-    RCLCPP_INFO(get_logger(), "Subscribed to map topic.");
-  }
+  RCLCPP_INFO(get_logger(), "Subscribed to map topic.");
 }
 
 void

nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp

@@ -74,7 +74,6 @@ class StaticLayer : public CostmapLayer
 
 private:
   void getParameters();
-  void getMap();
   void processMap(const nav_msgs::msg::OccupancyGrid & new_map);
 
   /**
@@ -102,8 +101,8 @@ class StaticLayer : public CostmapLayer
   rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::SharedPtr map_update_sub_;
 
   // Parameters
-  bool first_map_only_;      ///< @brief Only use the static map
   std::string map_topic_;
+  bool map_subscribe_transient_local_;
   bool subscribe_to_updates_;
   bool track_unknown_space_;
   bool use_maximum_;

nav2_costmap_2d/include/nav2_costmap_2d/testing_helper.hpp

@@ -62,13 +62,14 @@ unsigned int countValues(
   return count;
 }
 
-void addStaticLayer(
+nav2_costmap_2d::StaticLayer * addStaticLayer(
   nav2_costmap_2d::LayeredCostmap & layers,
   tf2_ros::Buffer & tf, nav2_util::LifecycleNode::SharedPtr node)
 {
   nav2_costmap_2d::StaticLayer * slayer = new nav2_costmap_2d::StaticLayer();
   layers.addPlugin(std::shared_ptr<nav2_costmap_2d::Layer>(slayer));
   slayer->initialize(&layers, "static", &tf, node, nullptr, nullptr /*TODO*/);
+  return slayer;
 }
 
 nav2_costmap_2d::ObstacleLayer * addObstacleLayer(

nav2_costmap_2d/plugins/static_layer.cpp

@@ -44,7 +44,6 @@
 #include <string>
 
 #include "nav2_costmap_2d/costmap_math.hpp"
-#include "nav2_util/map_service_client.hpp"
 #include "pluginlib/class_list_macros.hpp"
 #include "tf2/convert.h"
 #include "tf2_geometry_msgs/tf2_geometry_msgs.h"
@@ -72,24 +71,27 @@ StaticLayer::onInitialize()
   global_frame_ = layered_costmap_->getGlobalFrameID();
 
   getParameters();
-  getMap();
 
-  if (!first_map_only_) {
-    RCLCPP_DEBUG(node_->get_logger(), "Subscribing to the map topic (%s)", map_topic_.c_str());
-    map_sub_ = node_->create_subscription<nav_msgs::msg::OccupancyGrid>(map_topic_,
-        rclcpp::SystemDefaultsQoS(),
-        std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1));
+  rclcpp::QoS map_qos(1);
+  if (map_subscribe_transient_local_) {
+    map_qos.transient_local();
   }
 
+  RCLCPP_DEBUG(node_->get_logger(),
+    "Subscribing to the map topic (%s) with %s durability",
+    map_topic_.c_str(),
+    map_subscribe_transient_local_ ? "transient local" : "volatile");
+  map_sub_ = node_->create_subscription<nav_msgs::msg::OccupancyGrid>(
+    map_topic_, map_qos,
+    std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1));
+
   if (subscribe_to_updates_) {
     RCLCPP_INFO(node_->get_logger(), "Subscribing to updates");
     map_update_sub_ = node_->create_subscription<map_msgs::msg::OccupancyGridUpdate>(
       map_topic_ + "_updates",
       rclcpp::SystemDefaultsQoS(),
       std::bind(&StaticLayer::incomingUpdate, this, std::placeholders::_1));
   }
-
-  current_ = true;
 }
 
 void
@@ -118,13 +120,15 @@ StaticLayer::getParameters()
   int temp_lethal_threshold = 0;
 
   declareParameter("enabled", rclcpp::ParameterValue(true));
-  declareParameter("first_map_only", rclcpp::ParameterValue(false));
   declareParameter("subscribe_to_updates", rclcpp::ParameterValue(false));
+  declareParameter("map_subscribe_transient_local",
+    rclcpp::ParameterValue(true));
 
   node_->get_parameter(name_ + "." + "enabled", enabled_);
-  node_->get_parameter(name_ + "." + "first_map_only", first_map_only_);
   node_->get_parameter(name_ + "." + "subscribe_to_updates", subscribe_to_updates_);
   node_->get_parameter("map_topic", map_topic_);
+  node_->get_parameter(name_ + "." + "map_subscribe_transient_local",
+    map_subscribe_transient_local_);
   node_->get_parameter("track_unknown_space", track_unknown_space_);
   node_->get_parameter("use_maximum", use_maximum_);
   node_->get_parameter("lethal_cost_threshold", temp_lethal_threshold);
@@ -135,20 +139,6 @@ StaticLayer::getParameters()
   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
 }
 
-void
-StaticLayer::getMap()
-{
-  RCLCPP_INFO(node_->get_logger(), "StaticLayer: Requesting map from the map service");
-  auto map_client = std::make_unique<nav2_util::MapServiceClient>(client_node_);
-
-  nav_msgs::msg::OccupancyGrid map;
-  if (!map_client->getMap(map)) {
-    throw "StaticLayer: Failed to get map";
-  }
-
-  processMap(map);
-}
-
 void
 StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
 {
@@ -210,6 +200,8 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)
   width_ = size_x_;
   height_ = size_y_;
   has_updated_data_ = true;
+
+  current_ = true;
 }
 
 void
@@ -253,6 +245,27 @@ void
 StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr update)
 {
   std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
+  if (update->y < static_cast<int32_t>(y_) ||
+    y_ + height_ < update->y + update->height ||
+    update->x < static_cast<int32_t>(x_) ||
+    x_ + width_ < update->x + update->width)
+  {
+    RCLCPP_WARN(
+      node_->get_logger(),
+      "StaticLayer: Map update ignored. Exceeds bounds of static layer.\n"
+      "Static layer origin: %d, %d   bounds: %d X %d\n"
+      "Update origin: %d, %d   bounds: %d X %d",
+      x_, y_, width_, height_, update->x, update->y, update->width,
+      update->height);
+    return;
+  }
+
+  if (update->header.frame_id != map_frame_) {
+    RCLCPP_WARN(node_->get_logger(),
+      "StaticLayer: Map update ignored. Current map is in frame %s "
+      "but update was in frame %s",
+      map_frame_.c_str(), update->header.frame_id.c_str());
+  }
 
   unsigned int di = 0;
   for (unsigned int y = 0; y < update->height; y++) {

nav2_costmap_2d/test/integration/inflation_tests.cpp

@@ -77,6 +77,8 @@ class TestNode : public ::testing::Test
 
   void initNode(double inflation_radius);
 
+  void waitForMap(nav2_costmap_2d::StaticLayer * slayer);
+
 protected:
   nav2_util::LifecycleNode::SharedPtr node_;
 };
@@ -104,6 +106,13 @@ std::vector<Point> TestNode::setRadii(
   return polygon;
 }
 
+void TestNode::waitForMap(nav2_costmap_2d::StaticLayer * slayer)
+{
+  while (!slayer->isCurrent()) {
+    rclcpp::spin_some(node_->get_node_base_interface());
+  }
+}
+
 // Test that a single point gets inflated properly
 void TestNode::validatePointInflation(
   unsigned int mx, unsigned int my,
@@ -349,12 +358,13 @@ TEST_F(TestNode, testInflation)
   // circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 1, 1);
 
-  addStaticLayer(layers, tf, node_);
+  auto slayer = addStaticLayer(layers, tf, node_);
   nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
   addInflationLayer(layers, tf, node_);
   layers.setFootprint(polygon);
 
   nav2_costmap_2d::Costmap2D * costmap = layers.getCostmap();
+  waitForMap(slayer);
 
   layers.updateMap(0, 0, 0);
   // printMap(*costmap);
@@ -419,11 +429,13 @@ TEST_F(TestNode, testInflation2)
   // circumscribed radius = 3.1
   std::vector<Point> polygon = setRadii(layers, 1, 1);
 
-  addStaticLayer(layers, tf, node_);
+  auto slayer = addStaticLayer(layers, tf, node_);
   nav2_costmap_2d::ObstacleLayer * olayer = addObstacleLayer(layers, tf, node_);
   addInflationLayer(layers, tf, node_);
   layers.setFootprint(polygon);
 
+  waitForMap(slayer);
+
   // Creat a small L-Shape all at once
   addObservation(olayer, 1, 1, MAX_Z);
   addObservation(olayer, 2, 1, MAX_Z);

nav2_costmap_2d/test/integration/test_collision_checker.cpp

@@ -120,7 +120,10 @@ class TestCollisionChecker : public nav2_util::LifecycleNode
 
     layers_ = new nav2_costmap_2d::LayeredCostmap("frame", false, false);
     // Add Static Layer
-    addStaticLayer(*layers_, *tf_buffer_, shared_from_this());
+    auto slayer = addStaticLayer(*layers_, *tf_buffer_, shared_from_this());
+    while (!slayer->isCurrent()) {
+      rclcpp::spin_some(this->get_node_base_interface());
+    }
     // Add Inflation Layer
     addInflationLayer(*layers_, *tf_buffer_, shared_from_this());