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.

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

Bug: OD message "There might be a bug here!" #2

Merged
merged 3 commits into from
Oct 23, 2020

Conversation

marcelino-pensa
Copy link

closes Pensasystems/pensa-ros#740

This is a workaround for what seems to be a bug in octomap. I have filed an issue in the octomap repository: OctoMap/octomap#307

I tested the current solution and it works in simulation.
We can reproduce the error by killing the lidar node, which I plan to test on our drone.

Test card: https://docs.google.com/spreadsheets/d/11nEIxDr3Xem8T7m9ldJpqJAyNbu205QsmVvI4Ka0qGE/edit#gid=486985852

Copy link

@gssammy gssammy left a comment

Choose a reason for hiding this comment

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

Seems pretty straight forward.

// not being deleted. If we have only the root node available (which we can
// capure by checking the depth of the iterator), then we can safely clear
// the tree, as it is not a use case we need
const uint depth = it.getDepth();
Copy link

Choose a reason for hiding this comment

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

seems like this could be a function on its own, called every time we do something with the tree. It appears twice in the code.

Copy link
Author

@marcelino-pensa marcelino-pensa Oct 15, 2020

Choose a reason for hiding this comment

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

are you talking about getting the depth?
It appears twice because it is in two different for loops. I think that a function to retrieve the depth from a leaf iterator would be overkill, as it is a short one-liner that is clear about its goal.

Copy link
Author

Choose a reason for hiding this comment

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

If you are talking about the whole chunk of code that is repeated, note that one of them clears tree_, and the other clears tree_inflated_.

Choose a reason for hiding this comment

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

I think you could write a function clearIfEmpty or something that took a reference to a tree, checked its depth, and emptied it. It's a small optimization but it is more DRY

@@ -520,7 +542,7 @@ void OctoClass::FadeMemory(const double &rate) { // rate at which this function

// tree nodes that are unknown
if (is_occ != tree_inflated_.isNodeOccupied(n)) { // if it was occupied then disoccupied, delete node
tree_inflated_.deleteNode(key, it.getDepth());
tree_inflated_.deleteNode(key, depth);

Choose a reason for hiding this comment

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

I think is better not to change it.getDepth() in this line, since you define depth as a const unint. on the other hand you could define, depth as a pointer to it.getDepth() , instead of making a copy of this iterator.

Copy link
Author

Choose a reason for hiding this comment

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

Sorry, I don't follow your comment... what is the problem of having depth as a const uint?
If you are worried about complexity, uint should take less RAM than a pointer, as uint has 4 bytes, while a pointer has 8 bytes (in a 64bit machine).

Choose a reason for hiding this comment

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

it's a trivial matter, you declare depth variable in line 527 and it's not changing anymore. but technically it.getDepth() iterator can check the most recent root nodes.

Copy link
Author

Choose a reason for hiding this comment

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

not really... the iterator can only point to a different node after if moves on in the for loop through the command ++it...

Choose a reason for hiding this comment

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

is this thread-safe? If not then potentially it could change?

Copy link
Author

Choose a reason for hiding this comment

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

This is the only place where FadeMemory is called:

        mutexes_.octomap.lock();
            if (globals_.octomap.memory_time_ > 0) {
                globals_.octomap.FadeMemory(fading_memory_update_rate_);
            }
        mutexes_.octomap.unlock();

So yeah, it is thread-safe

Copy link

@adam-pensa adam-pensa left a comment

Choose a reason for hiding this comment

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

LGTM, interested to see test results

@@ -520,7 +542,7 @@ void OctoClass::FadeMemory(const double &rate) { // rate at which this function

// tree nodes that are unknown
if (is_occ != tree_inflated_.isNodeOccupied(n)) { // if it was occupied then disoccupied, delete node
tree_inflated_.deleteNode(key, it.getDepth());
tree_inflated_.deleteNode(key, depth);

Choose a reason for hiding this comment

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

is this thread-safe? If not then potentially it could change?

// not being deleted. If we have only the root node available (which we can
// capure by checking the depth of the iterator), then we can safely clear
// the tree, as it is not a use case we need
const uint depth = it.getDepth();

Choose a reason for hiding this comment

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

I think you could write a function clearIfEmpty or something that took a reference to a tree, checked its depth, and emptied it. It's a small optimization but it is more DRY

@marcelino-pensa
Copy link
Author

I fully rewrote the solution to this issue.
The new solution does not have to check the depth for all nodes in the iterator anymore.

Copy link

@MajidTaheri MajidTaheri left a comment

Choose a reason for hiding this comment

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

Looks good to me!

Copy link

@MajidTaheri MajidTaheri left a comment

Choose a reason for hiding this comment

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

Looks good, sorry for delay!

@marcelino-pensa marcelino-pensa merged commit 3b0f97c into master Oct 23, 2020
@marcelino-pensa marcelino-pensa deleted the bug/ma-there-might-be-a-bug-here-740 branch October 23, 2020 17:31
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

4 participants