search

ros-planning / navigation

ROS Navigation stack. Code for finding where the robot is and how it can get somewhere else.
2.34k stars 1.8k forks source link

Problem with Voxel plugin: clearing logic seems to be wrong #662

Open facontidavide opened 6 years ago

facontidavide commented 6 years ago

Hi, something pretty weird happened to me when I try to use the Voxel Layer. To give you an idea, please take a look to this video:

https://vimeo.com/255893590

As you can see, a moving object (in this case, two cars) are moving in front of the robot and, as expected, they are detected and the local costmap is updated accordingly.

What is "wrong" is that the voxel grid is NOT cleaned up when the cars disappear. Needless to say that this prevents the local/global planner to work correctly.

After some debugging and testing I am pretty sure that I found the problem. Let me illustrate it using the following scientific representation:

image

The max_obstacle_height is set to 2.0 meters. All the points above this threshold are discarded. A moving obstacle is detected below the threshold, but once the obstacle disappears, the ray will hit a wall at a Z coordinate that is above the threshold. The ray is discarded and is not used for cleaning the VoxelGrid.

Note that the same is true for min_obstacle_height. We don't want to discard clearing rays that hit the ground!

In short: the min/max obstacle layers should be applied only to marking, but we should use ALL the rays for clearing, no matter their height.

facontidavide commented 6 years ago

For your information. using a value of z_voxels smaller than max_obstacle_height / z_resolution makes this problem even worse. I am trying to find the reason in the code, may be you can figure it out faster than me ;)

UPDATE: In other words, if (z_voxels < max_obstacle_height / z_resolution), it is more probable that some voxels are marked but NOT cleared when they could.

The problems seems to be related to these two checks that fails:

https://github.com/ros-planning/navigation/blob/kinetic-devel/costmap_2d/plugins/voxel_layer.cpp#L353 https://github.com/ros-planning/navigation/blob/kinetic-devel/voxel_grid/src/voxel_grid.cpp#L133

mikeferguson commented 6 years ago

The extensive discussion in https://github.com/ros-planning/navigation/issues/267 might also be of interest to you -- it's a slightly different issue, but given your type of sensor, might be something you'll see.

facontidavide commented 6 years ago

It is for sure an interesting discussion, but not really related to the issue I am describing, i.e. the fact that valid rays are not raytraced because the Z height is outside the given range.

If you have any argument against keeping these rays during the cleanup, let me know.

I just realized I didn't share with you my forked version... https://github.com/facontidavide/navigation/tree/voxel_cleaning

A little bit messy, I admit, the actual PR will be cleaner and easier to review

nickvaras commented 6 years ago

You have stated the issue very clearly and accurately IMO, @facontidavide . A crude workaround strategy I've tried was adding the same observation source twice, but with different parameters, where one performs a clearing function, whereas the other does the marking. Hope this gets resolved!

mikeferguson commented 6 years ago

Ok, I've now spent quite a bit of time looking at this, and your pull request.

First, let's disambiguate a few parameters:

The values for the observations are per-observation. You might have multiple observations each with different min/max. For instance, the PR2 has 4 observations, including a PCL-processed ground plane. They further make some observations marking-only, and others clearing-only, depending on the preprocessing that has been done.

In your current example, I believe you can get the desired effect, by making the max_obstacle_height of your observation very large -- which is still semantically correct because this value represents the max height "of the sensor", not the voxel grid you are writing into. When that data gets to the voxel layer, it will not be marked and then during raytracing it will be shortened to the max_obstacle_height of the voxel grid.

It should be noted, if you are also trying to go "downwards", the raytracing in the voxel grid will appropriately limit to origin_z, however, the marking will simply project points to origin_z. Since the navigation stack is 2d-only, dips in the ground plane really aren't supported, and so this case should not be an issue.

facontidavide commented 6 years ago

Hi @mikeferguson ,

everything you said is technically 100% correct. There are workarounds as you mentioned to obtain the same effect, but IMO 90% of the users of this plugin are not aware of this implicit "contract" and they break the principle of "minimum surprise".

In other words, I don't see any negative impact in implementing these changes, no "surprise" from the viewpoint of the user. Quite the opposite: suddenly we are clearing voxels which, given the information we have, were supposed to be cleared. I hardly believe that we will break anyone's code, since we are clearing voxels which are indeed empty!

For example. Let's suppose that we increase the value of the max_obstacle_height, as you suggested, as a workaround of the clearing "bug": this means that we also increase the height of the marking, that is not really what we wanted.

Rephrasing again: I understand that we might be able to find some combination of parameters that work as desired, but which are specifically the issues which you see in PR #667 ?

Cheers

nickvaras commented 6 years ago

I just want to add, mostly for the record, and since I was unable to find any online reference to the following whatsoever, that in order to get any clearing at all, it seems to be necessary that the sensor is within the vertical range defined by the voxel stack.

From what I could see, the raytracing in a voxel layer that starts from a sensor above or below the origin_z height or top of the highest voxel respectively, will not clear any observations.

facontidavide commented 6 years ago

For the records, I think that many users will be bitten by this issue and I believe the PR I proposed was "correct" from a technical and semantic point of view.

IMHO, discarding a clearing ray because of the max Z value is conceptually wrong, because within the Z limits we still have a valid clearing segment that is basically ignored.

Cheers :)

VorpalBlade commented 5 years ago

I ran into this problem as well, and fixed it (against kinetic). See commit c64079ca9c477a14743eadc6fc39072d8a1f49f8.

Presumably this need to be rebased against a newer version and a pull request could be done.

facontidavide commented 5 years ago

Good to know someone fixed this ;)

SteveMacenski commented 5 years ago

@VorpalBlade can't streamline this through Navigation myself, but I'm going to move that change into Navigation2's voxel layer (which I do have control over) so whenever you move to ROS2 it'll be waiting for you, longingly (come join us in ROS2, the water's fine!).

Biggest comment I'll make is that you still want to enforce the std::max

VorpalBlade commented 5 years ago

Biggest comment I'll make is that you still want to enforce the std::max

So I looked at that and couldn't figure out why it was needed to be honest (when would it even happen?). Also, it is not used for x or y.

SteveMacenski commented 5 years ago

Its mostly for safety - I look at it from the viewpoint that its not hurting anything and its been there long enough I don't want to break it

SteveMacenski commented 4 years ago

Oh whoops. The new github auto-linking is a mess.

mikeferguson commented 4 years ago

Oh whoops. The new github auto-linking is a mess.

Yep. that appears to be the case

zentala commented 4 years ago

I am developing a robot where the RGBD camera is localized 80cm above ground and I also had this issue. I solved it by using NonPersistentVoxelLayer instead of VoxelLayer. Package has Kinetic, Melodic, Dashing and Eloquent versions. You can read more in ROS documentation. This package is some kind of modification of the spatio_temporal_voxel_layer so in case of lack of documentation, look there.

SteveMacenski commented 4 years ago

NPVL has no relation to STVL. Its just a stripped down version of VL without maintaining data.

zentala commented 4 years ago

I see. It also seems that NPVL is useful only for robots that have 360* FOV. So it's not a long-term solution in my case anyway.

So my Voxel Layer is cleared when I am using NPVL, but I have blobs with STVL and regular VL as well. This issue occurs on ROS Melodic. I noticed @SteveMacenski prepared a fix for ROS2, and @facontidavide prepared a fix for ROS1 but this pull request was rejected. I noticed that this issue was fixed for Kinetic.

I would prefer to stay with Melodic. After reading all those posts I still don't understand how can I overcome this issue. Could you explain to me how to configure my costmap to avoid those blobs?

germal commented 4 years ago

@zentala Hello I have the same exact issue of obstacle clearing with a d435 RGBD camera at 70cm with my voxel_layer on Melodic.Did you manage to resolve it ? Thanks

facontidavide commented 4 years ago

As I said before, the logic of the PR is correct in my opinion and I don't agree with the reason provided for rejecting it ...

But you house, your rules.

@germal Arguably for a dense sensor such as RGBD, you probaly need to consider this (already mentioned earlier)

http://wiki.ros.org/spatio_temporal_voxel_layer

@SteveMacenski sounds familiar?

germal commented 4 years ago

@facontidavide Thank you very much , I will try soon !

SteveMacenski commented 4 years ago

Awesome, also, given we use costmap 2d in ROS2, if a PR is submitted over there, I can do something about it and get it moving forward.