diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp index 119d077ea2..d5612f2f33 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp +++ b/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,9 +101,8 @@ class StaticLayer : public CostmapLayer rclcpp::Subscription::SharedPtr map_update_sub_; // Parameters - bool first_map_from_service_; - 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_; diff --git a/nav2_costmap_2d/plugins/static_layer.cpp b/nav2_costmap_2d/plugins/static_layer.cpp index b65ccf8c5a..09b9dbc071 100644 --- a/nav2_costmap_2d/plugins/static_layer.cpp +++ b/nav2_costmap_2d/plugins/static_layer.cpp @@ -44,7 +44,6 @@ #include #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" @@ -73,22 +72,23 @@ StaticLayer::onInitialize() getParameters(); - if (first_map_from_service_) { + nav_msgs::msg::OccupancyGrid empty_map; + // processMap updates the size of other costmap layers, so we have to call + // it, even if the map is 0x0 + processMap(empty_map); - getMap(); - } else { - nav_msgs::msg::OccupancyGrid empty_map; - // processMap updates the size of other costmap layers, so we have to call - // it, even if the map is 0x0 - processMap(empty_map); + rclcpp::QoS map_qos(1); + if (map_subscribe_transient_local_) { + map_qos.transient_local(); } - if (!first_map_only_) { - RCLCPP_DEBUG(node_->get_logger(), "Subscribing to the map topic (%s)", map_topic_.c_str()); - map_sub_ = node_->create_subscription(map_topic_, - rclcpp::SystemDefaultsQoS(), - std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1)); - } + 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( + map_topic_, map_qos, + std::bind(&StaticLayer::incomingMap, this, std::placeholders::_1)); if (subscribe_to_updates_) { RCLCPP_INFO(node_->get_logger(), "Subscribing to updates"); @@ -126,45 +126,25 @@ StaticLayer::getParameters() int temp_lethal_threshold = 0; declareParameter("enabled", rclcpp::ParameterValue(true)); - declareParameter("first_map_from_service", 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_from_service", first_map_from_service_); - 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); node_->get_parameter("unknown_cost_value", unknown_cost_value_); node_->get_parameter("trinary_costmap", trinary_costmap_); - if (!first_map_from_service_ && first_map_only_) { - throw std::runtime_error( - "Static layer does not have a map. Either set parameter " - "first_map_from_service=true or set parameter first_map_only=false"); - } - // Enforce bounds 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(client_node_); - - nav_msgs::msg::OccupancyGrid map; - if (!map_client->getMap(map)) { - throw std::runtime_error("StaticLayer: Failed to get map"); - } - - processMap(map); -} - void StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map) { @@ -270,24 +250,25 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u { std::lock_guard guard(*getMutex()); if (update->y < static_cast(y_) || - y_ + height_ < update->y + update->height || - update->x < static_cast(x_) || - x_ + width_ < update->x + update->width) { + y_ + height_ < update->y + update->height || + update->x < static_cast(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); + 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()); + "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;