Skip to content

Commit

Permalink
Added new costmap buffer to resize map safely (ros-navigation#1837)
Browse files Browse the repository at this point in the history
* Added new costmap buffer to resize map safely

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Update map if the layer is not being processed.

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>

* Updated bool name and added buffer initialization

Signed-off-by: Aitor Miguel Blanco <aitormibl@gmail.com>
  • Loading branch information
gimait authored and ruffsl committed Jul 2, 2021
1 parent 79a2abd commit 993b5ac
Show file tree
Hide file tree
Showing 2 changed files with 26 additions and 3 deletions.
2 changes: 2 additions & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -111,6 +111,8 @@ class StaticLayer : public CostmapLayer
bool trinary_costmap_;
bool map_received_{false};
tf2::Duration transform_tolerance_;
std::atomic<bool> update_in_progress_;
nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_;
};

} // namespace nav2_costmap_2d
Expand Down
27 changes: 24 additions & 3 deletions nav2_costmap_2d/plugins/static_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -58,6 +58,7 @@ namespace nav2_costmap_2d
{

StaticLayer::StaticLayer()
: map_buffer_(nullptr)
{
}

Expand Down Expand Up @@ -140,6 +141,7 @@ StaticLayer::getParameters()
// Enforce bounds
lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
map_received_ = false;
update_in_progress_.store(false);

transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
}
Expand Down Expand Up @@ -193,6 +195,9 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)

unsigned int index = 0;

// we have a new map, update full size of map
std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());

// initialize the costmap with static data
for (unsigned int i = 0; i < size_y; ++i) {
for (unsigned int j = 0; j < size_x; ++j) {
Expand All @@ -204,8 +209,6 @@ StaticLayer::processMap(const nav_msgs::msg::OccupancyGrid & new_map)

map_frame_ = new_map.header.frame_id;

// we have a new map, update full size of map
std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
Expand Down Expand Up @@ -249,9 +252,15 @@ void
StaticLayer::incomingMap(const nav_msgs::msg::OccupancyGrid::SharedPtr new_map)
{
std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
processMap(*new_map);
if (!map_received_) {
map_received_ = true;
processMap(*new_map);
}
if (update_in_progress_.load()) {
map_buffer_ = new_map;
} else {
processMap(*new_map);
map_buffer_ = nullptr;
}
}

Expand Down Expand Up @@ -311,6 +320,14 @@ StaticLayer::updateBounds(
}

std::lock_guard<Costmap2D::mutex_t> guard(*getMutex());
update_in_progress_.store(true);

// If there is a new available map, load it.
if (map_buffer_) {
processMap(*map_buffer_);
map_buffer_ = nullptr;
}

if (!layered_costmap_->isRolling() ) {
if (!(has_updated_data_ || has_extra_bounds_)) {
return;
Expand Down Expand Up @@ -338,6 +355,7 @@ StaticLayer::updateCosts(
int min_i, int min_j, int max_i, int max_j)
{
if (!enabled_) {
update_in_progress_.store(false);
return;
}
if (!map_received_) {
Expand All @@ -347,6 +365,7 @@ StaticLayer::updateCosts(
RCLCPP_WARN(node_->get_logger(), "Can't update static costmap layer, no map received");
count = 0;
}
update_in_progress_.store(false);
return;
}

Expand All @@ -369,6 +388,7 @@ StaticLayer::updateCosts(
transform_tolerance_);
} catch (tf2::TransformException & ex) {
RCLCPP_ERROR(node_->get_logger(), "StaticLayer: %s", ex.what());
update_in_progress_.store(false);
return;
}
// Copy map data given proper transformations
Expand All @@ -393,6 +413,7 @@ StaticLayer::updateCosts(
}
}
}
update_in_progress_.store(false);
}

} // namespace nav2_costmap_2d

0 comments on commit 993b5ac

Please sign in to comment.