iron out move_base issues

[bfalacerda]

As we saw during the preparation for the marathon, our continuous navigation system still has a couple of loose ends that need to be taken care of. We've been adding some issues here and there, and discussing it around, so I'd like to put them all together in a single issue where we can start ticking boxes. I would appreciate if we can split this work on a couple of people, and have the proposed changes discussed before merging, so we can get a basic system that everyone's happy with.

1. polish cost map layer definitions, including trying to get no-go map included in the local costmap - see Stuff being added to global costmap? strands_navigation#164 and check costmaps configuration #24. Also find if we can get lingering obstacles that aren’t being seen by the robot cleared from the global costmap, (see point 4)
2. check cost map loading, as it looks like its not working properly, and also it might be making move_base dies once in a while - see Global costmap being cleared strands_navigation#152 and move_base died after door pass strands_navigation#155
3. check dwa planner params to have the robot rotate more in place when executing plans, and stop throwing itself against obstacles - see Not turning on the spot. strands_navigation#127 and Karl going towards obstacles strands_navigation#163. In particular, during the g4s deployment, we found that bob was better off with the default params here:https://github.com/strands-project/strands_movebase/blob/hydro-devel/strands_movebase/strands_movebase_params/dwa_planner_ros.yaml#L30
4. check if the lingering obstacles issue is solved by [strands_movebase] Don't add obstacles at edge of laser's FOV #26. @lucasb-eyer said “ think the current costmap approach of ROS is fundamentally flawed and we'll never completely get rid of lingering points”. Care to elaborate? We should at least have a way of getting rid of lingering obstacles on the global costmap, when the robot is not seeing an area where those obstacles are (see point 1)

All the help with this is welcome. I think we should tackle these issues already in indigo, to be sure that the migration doesn’t undo our work. We are in the process of updating Bob at the moment. There aren’t too many strands packages released for indigo yet, but hopefully we’ll have a full indigo strands system soon.

[bfalacerda]

i'll be looking at the costmap layers in the next couple of days

[bfalacerda]

please add stuff that i missed

[lucasb-eyer]

I'll try to elaborate, but it'd be easier drawing on a whiteboard. There's the voxel grid in which single voxels get set when there is "something" and unset when there's nothing, then the voxel grid gets projected down to a 2d grid on the groundplane by "adding" up marked voxels. I think the problem is twofold:

1. (Un)Setting voxels in an axis-aligned grid using a pointcloud which is not aligned the same way. Due to aliasing, I'm pretty sure I can always come up with a configuration (pointcloud location+orientation) which will miss one of the voxels in the grid even though it shouldn't if it was continuous. The probability is reduced a lot as the resolution of the pointcloud increases relatively to the voxelgrid's resolution. @nilsbore's "unsetting but not setting border" trick also helps reducing the probability.
2. A voxel will never be cleared unless it's unset. I've just tried to explain why I think there'll always be some voxels you miss.

The extreme noise of the kinect when there's reflecting surfaces in the FOV (not of NaNs but of incorrect "flying" points!) worsens the problem, but is not a fundamental problem.

I think a possible solution would be:

Marking+unmarking together with a voxel "lifetime" which decays would work much better. If there's an obstacle at a location you haven't seen for a long time, you likely don't care and will see it again (or not) when it's time to care again. The lifetime should increase with more marking and decrease slowly over time, decreasing much faster in the event of unmarking. This would automatically filter-out high-frequency noise - I've never seen high-frequency obstacles IRL - and would avoid any kind of lingering.

Hope this was somehow understandable, it's hard to describe with words. Who knows, maybe it's a paper idea ;-)

[nilsbore]

Good initiative, thanks for writing this up. I have some thoughts on the different problems:

1. I think we should keep the laser in the global costmap but remove the chest camera obstacles from there. We might as well just use a 2d obstacle_layer instead of the voxel map in the global costmap then and the 2d obstacle_layer is a bit more stable in my experience. I think keeping the laser in the global costmap makes sense since it avoids us going straight up against the door before discovering that it is closed etc.
2. Maybe we'll have to fix a bug in move_base here.
3. Tough one, we tried lots of parameters already. We should investigate what changed in move_base, maybe specifically in dwa_local_planner. The performance was notably better a couple of months ago, with the exact same parameters.
4. I think this mostly fixes the problem for when the robot is static. One thing is when the robot moves and previously observed obstacles moves out of sight. I think in some instances a problem might be the update rate of the costmap. The robot observes voxels as free but then it moves past them before a new set of points is observed and the costmap is updated with those instead. So if we remove the chest camera from the global costmap, we should try higher update rates for the local one.

[bfalacerda]

thanks for replying @nilsbore @lucasb-eyer , you two were pretty much my hope for getting some help on this, I'm happy to see you step forward :)

I spent the day sending bob in and out our lab door, and figured out some things. I'll try to keep them organized on points 1-4, though there's a bit of an overlap between them.

1. The main thing that needs to be changed in our costmaps definition is the inflation_layer: ever since we moved to hydro, we've been mixing the robot footprint with the inflation of obstacles. To summarize, dwa planner uses the footprint to generate local plans. However, we are also adding the inflation layer to the local costmap. This pretty much duplicates the footprint: dwa planner takes it into account by design, and then we're adding an extra inflation to the obstacles. This is why we have the robot being extremely conservative in narrow spaces. The inflation layer needs to be taken into account in the global costmap, otherwise global plans will go very close to obstacles, but we should leave it off the local costmap definition. I tested it, and the footprint (without any extra inflation) is more than enough to have the robot not crashing into stuff. Regarding having the no-go map included on the local costmap, we can't really do it until Local Costmap Cannot Use Static Layer ros-planning/navigation#197 is merged upstream.
2. As we were already assuming, there's no way out of this problem without changing the static layer definition of move_base itself. As it is, static layers simply overwrite each other, so the only one we'll have is the last one to be defined. My suggestion to cope with this, is keep having a /map and /no_go_map parameters, but have /map used by amcl, and no_go_map used as the static layer for the global costmap. Of course this requires having our no-go-maps as an overlay of the no go areas on the original map, so the no_go_map you wanted to use wont work @lucasb-eyer
3. Taking out the inflation layer really did the trick. That, along with some other small fine tunes of the params really improved the overall performance on our lab door. I'll make a PR tomorrow
4. I was hoping that there was some kind of decay param that we could set @lucasb-eyer , unfortunately there isn't. @nilsbore your idea of having simplified, laser only, plugins for the obstacles in the global costmap might work. The problem might be that the global planner is the one in charge of getting the robot to the goal. The local planner just tries to follow the global plan as closely as possible. If we have the local and global costmaps defined in a different way, we might get into situations where the global planner keeps generating global plans that can't be followed locally, instead of planning around an obstacle that is only seen by the chest camera for example. I'll check it tomorrow

A final remark, all my tests were done directly on scitos_2d_navigation, as I couldnt use the chest camera in indigo, some openni_wrapper issue. I'll create a proper issue for it tomorrow

[lucasb-eyer]

Ha, that inflation thing makes sense and even reading the whole documentation it didn't occur to me, nice find! In case it can give you ideas or I forgot to mention any of the nav tweaks I did for the marathon, I pushed my full marathon system today.

Also, cheers for tackling this daunting task.

[nilsbore]

That is indeed a good find. It is strange that passing through quite narrow doors here worked fine for quite a while though, I guess they corrected a bug in dwa_local_planner that allowed us to do that with the inflation layer on.

1. 👍 Also, there is an option in the dwa_local_planner to make the footprint a bit bigger if we need it be a bit more conservative.
2. This makes sense as a stopgap solution.
3. Cool, let me know as soon as you have something that I can test.
4. This is something that we will just have to try out. In fact, for the head, I am only adding it to the local costmap already and it seems to mostly be able to plan around the head obstacles. So I guess we'll simply have to investigate for the chest. I can try that out today.

@bfalacerda You should be able to use openni_launch or openni2_launch, shouldn't make any difference.

[nilsbore]

I did a bit of testing with removing the inflation layer from the local costmap. It seems to improve things but the planner still has a difficulty passing through doors; I noticed that the problem with turning towards obstacles is still there. So I think there is something more at play here. I will get an old move_base and see what happens now.

[bfalacerda]

@nilsbore can you test the params in #30?

[nilsbore]

Yes, sure. Btw, I checked out branch 1.11.4 of https://github.com/ros-planning/navigation and it makes a huge difference! We need to figure out what's changed here. Navigation is so much smoother with that old version! Rosie gracefully passes through doors that she has had real problems with and more carefully turns around corners.

[nilsbore]

We had a problem with starting up multiple map servers before recently updating the packages (we could not use the no-go maps that everyone else could). This has not been a problem recently. I think it was because we were using an older branch of move_base before whereas everyone else were using a newer one (we did e.g. during the review). This older branch seems to have performed better with the parameters that we have. Thing is, I get the map server problem again with 1.11.4 so I had to disable the no-go map.

[bfalacerda]

well, we've seen that the two static layers dont work anyway, so having the ability to run 2 map servers is pretty much useless. If 1.11.4 is much better, we might think to revert to it for deployments, if the current version at the time sucks

[nilsbore]

Please try it out and see what you think. It's useless to optimize parameters for the new version if we're gonna use the old one. I would like to stay with the trunk version, but that also means that we might have to search for a new parameter set now and then. I will see if I can identify what's changed, haven't found any reasons for this in dwa_local_planner yet.

[nilsbore]

I tried 1.11.5 now also and it's as bad as trunk so it seems the change must have happened somewhere in between.

[bfalacerda]

very weird. at least now we know where to revert if we really need to. can you check if the params in #30 with the trunk version are acceptable?

[nilsbore]

With that resolution of the local costmap, it's not acceptable because of lingering obstacles.

[bfalacerda]

yeah, with it back to 0.05

[lucasb-eyer]

I noticed that the problem with turning towards obstacles is still there

This one, I don't know, but it feels so much like someone, somewhere, put a - where there should be a + or the other way around. But it'd be strange for such a bug to be unnoticed in ROS.

[nilsbore]

Me and @bfalacerda tried out what's changed from 1.11.4 and it's something that changed in the dwa_local_planner or base_local_planner before 1.11.5. It makes the robot less careful, which means it passes doors more easily but is also prone to crashing into the sides of the doors. Increasing the footprint_padding seemed to yield performance similar to later versions.

[bfalacerda]

I'm pretty much done with this, I don't think there's much more we can do given move_base's own limitations. My final remarks on what can still be an issue:

• The current version of move_base doesnt allow us to load more than 1 static layer, it just overwrites the old static layer when we define a new one. This means that, for the time being, our no_go_maps need to be a copy of the original map, plus no go areas painted on top of it
• The current version of move_base doesnt allow us to add the no_go_map to the local costmap. This means that the local planner might generate plans that go to no go areas if it's trying to avoid obstacles. It looks like there's a PR for this but it's not merged yet: Static layer works with rolling window now ros-planning/navigation#246
• Lingering ghostly obstacles seem to be unavoidable in general. However now we have the costmap clearance back in properly, so they should be much less of an issue. Also @nilsbore is looking at the observation_persistence parameter to see if it helps avoiding them even more. He's also experimenting with having a simpler, laser only, obstacle layer on the global costmap. If we go for this, it might be good to get its width and height a bit bigger so we can replan further away from obstacles
• While the new params seem to allow the robot to navigate better, it still has issues in narrow spaces and can't really go through doors. Navigation 1.11.4 is much better on that, but then it's also much better at crashing head on into doors... The only move_base based solution I see is, when in narrow areas:
  • reconfigure the local costmap to be only laser based, and with very high resolution (default resolution is 5cm)
  • reduce allowed speed
  • This narrow areas move base could then label the more problematic edges in the topological map

[bfalacerda]

closing this as the discussion on this is now being done on #62

Also linking this issue there, so we keep the conclusions made here in mind