Skip to content

Commit

Permalink
remove map service from static layer
Browse files Browse the repository at this point in the history 
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
  • Loading branch information
@rotu
rotu committed Jul 11, 2019
1 parent 427d742 commit 2953562
Show file tree
Hide file tree
Showing 2 changed files with 33 additions and 54 deletions.
4 changes: 1 addition & 3 deletions nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,6 @@ class StaticLayer : public CostmapLayer

private:
void getParameters();
void getMap();
void processMap(const nav_msgs::msg::OccupancyGrid & new_map);

/**
Expand Down Expand Up @@ -102,9 +101,8 @@ class StaticLayer : public CostmapLayer
rclcpp::Subscription<map_msgs::msg::OccupancyGridUpdate>::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_;
Expand Down
83 changes: 32 additions & 51 deletions nav2_costmap_2d/plugins/static_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down Expand Up @@ -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<nav_msgs::msg::OccupancyGrid>(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<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");
Expand Down Expand Up @@ -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<nav2_util::MapServiceClient>(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)
{
Expand Down Expand Up @@ -270,24 +250,25 @@ StaticLayer::incomingUpdate(map_msgs::msg::OccupancyGridUpdate::ConstSharedPtr u
{
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) {
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);
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;
Expand Down

0 comments on commit 2953562

Please sign in to comment.