Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Added new costmap buffer to resize map safely #1837

Merged
merged 4 commits into from
Jul 23, 2020
Merged

Added new costmap buffer to resize map safely #1837

Show file tree
Hide file tree
Changes from 2 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
7c63041
Added new costmap buffer to resize map safely
gimait Jul 1, 2020
7e926b8
Update map if the layer is not being processed.
gimait Jul 21, 2020
a241308
Updated bool name and added buffer initialization
gimait Jul 23, 2020
51acf47
Merge branch 'master' of https://github.com/ros-planning/navigation2 …
gimait Jul 23, 2020
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions nav2_costmap_2d/include/nav2_costmap_2d/static_layer.hpp
View file
Open in desktop
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> processing_layer_;
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
nav_msgs::msg::OccupancyGrid::SharedPtr map_buffer_;
};

} // namespace nav2_costmap_2d
Expand Down
26 changes: 23 additions & 3 deletions nav2_costmap_2d/plugins/static_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -140,6 +140,7 @@ StaticLayer::getParameters()
// Enforce bounds
lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
map_received_ = false;
processing_layer_.store(false);

transform_tolerance_ = tf2::durationFromSec(temp_tf_tol);
}
Expand Down Expand Up @@ -193,6 +194,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 +208,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 +251,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 (processing_layer_.load()) {
map_buffer_ = new_map;
} else {
processMap(*new_map);
map_buffer_ = nullptr;
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You don't need to set to nullptr here. If you set it, it will be reset below in the update function.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think it is necessary. If the incomingMap is called while updating the layer, and then again after the update is finished, the last call should pe processed, and the buffer emptied. Otherwise in this case the new map would be processed twice, once here, another time in the update function.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If map_buffer_ is set to nullptr to begin with (you should do that in constructor's :), then it is only not nullptr when 259 sets it to something else. In the next function, you process the map and then reset it to nullptr. At that point, I don't think there's any case when this is not nullptr where setting it here is necessary. You have a mutex locked above this so only this callback or one of the functions can be in use at the same time, effectively locking this variable -- unless I'm missing something.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yeah, so both functions can call the processMap, therefore if we don't clear the map_buffer_ here, when we get to the update we see that efectively there is something in the buffer and we update it again. I think. The fact of having the lock doesn't change that: When we are here, this function has the lock, and processes the map. Then the update is waiting, and when the lock gets free, it would check the buffer, and call processMap again if it's not empty. I think you are seeing the practical case, but in theory, it would be possible to call incomingMap several times between calls to updateBounds, including during the execution of updateBounds.

But I see how this can be confusing, and tricky to follow. Maybe it's best to simply get the layered_costmap mutex at the beginning of processMap and leave the rest as it was..

I'll initialize the buffer to nullptr also, I missed that one.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess let me game this out without the 262 map_buffer_ = nullptr:

  • I start up (so map_buffer_ is nullptr) have a new map, not in the middle of a process cycle so it just directly updates. Do this 10 times, still never set to anything other than nullptr
  • I start up (so map_buffer_ is nullptr) have a new map, not in the middle of a process cycle so it just directly updates. Then I update bounds 10 times, so still nullptr so never gets called in the update function
  • I start up (so map_buffer_ is nullptr) have a new map, its now in the middle of an update cycle so it stores the map in map_buffer_ between the 2 functions after the mutex is released and then in the updateCosts it uses the old map since there's no processMap() call in updateCosts. Then we wait for the next iteration of the costmap to add / process the map
  • I start up (so map_buffer_ is nullptr) have a new map, its now in the middle of an update cycle so it stores the map in map_buffer_ between the 2 functions after the mutex is released and then in the updateCosts it uses the old map since there's no processMap() call in updateCosts. Then we have a second new map before the updateBounds can be called again. If we're not in an update cycle, then that new map will be directly processed but the old map will still be stored and processed next round.

You are correct, we need to set to reset in case there's something there. Thanks!

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Just make the other changes and we should be GTG

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay I'll push the update during the day.
Just to clarify why I placed the processMap at the beginning of updateBounds and not at the end of updateCosts:
The line 168 of layered_costmap.cpp can prevent the updateCosts call from being done, which would lock the costmap if the reason for the error comes from the static_layer's map (if that would be the case, the logic would prevent the static map from being updated). By placing the processMap call at the beginning of updateBounds, we ensure that the map would be updated every time there is a new map, without any real changes on the performance of the layered costmap.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay, I'll fix the branch and the other changes and push during the day.

Just to clarify why I placed the call to processMap at the beginning of updateBounds and not at the end of updateCosts:

The line 168 of layered_costmap.cpp can prevent the call of updateCosts. This is problematic when the reason why we end up returning there is related to the static layer, because in that case, the incomingMap call would be prevented from updating the map (the 'update_in_progress' flag is set to true until the end of updateCosts).
It might seem like a lot of conditions should be met for that to happen, but it could happen. By moving the processMap to the beginning of updateBounds, we prevent this logic lock from happening without any performance penalty.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Beginning makes sense, no issue

}
}

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

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

// If there is a new available map, load it.
if (map_buffer_ != nullptr) {
SteveMacenski marked this conversation as resolved.
Show resolved Hide resolved
processMap(*map_buffer_);
map_buffer_ = nullptr;
}

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

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

} // namespace nav2_costmap_2d