navigation2
navigation2 copied to clipboard
ros-navigation
Redesign Environmental Representation
Today we use a 2D costmap of independent cells. While this is a good and cheap way of saving some relevant information, it will bound us for the foreseeable future to 2.5D land.
If we want to support navigating on irregular surfaces (e.g. outdoor unstructured / natural), we need to make some steps towards that. A gradient map can still provide obstacle information similar but less regularly than a direct costmap. And to be fair, the costmaps are binned measurements so we're already a bit in that direction.
I propose a 2D gradient map being the default environmental model type. This can still be used in the same way as today's costmap to check for collisions based on gradients / absolute height in 2.5D space, but will also allow modelling for people to navigate permissibly high spaces in outdoor environments.
Gradient over 3D to allow for ramps or hills, representing downwards stairs or drops. I'm not certain for most cases what 3D collision checking really buys us if we’re only monitoring from ground to robot height. Gradient doesn't help if docking with objects under or alongside, but nor does 2d.
remove thinking if free or marked, and think now: can I traverse this
for 2D: invert that a bit, find if flat surface and then un-mark but then mark edges to not go off
Costmap representations also don't have to be same in both local and global. The local can be a gradient map but the global can be a much more coarse traversibiltiy estimate map
refs https://github.com/TixiaoShan/traversability_mapping https://github.com/leggedrobotics/traversability_estimation https://github.com/ANYbotics/elevation_mapping
Snippet of my thoughts on this from another conversation:
So because sensors are noisy, and especially sensors like the realsense cameras (ugh) we have to play some games. There's some usual filtering pipeline to throw out exaggerated outliers, but its hard to robustly and efficiently throw out outliers without running a probabilistic filter (like PCL's statistical outlier filter) which is really expensive and honestly just not practical for real-time applications on mobile robot computers. So we have to be using some data points in our environmental representation that are going to probably be trash. Raycasting is one way to get those out, if you have 1 point very close, the density of the output will let multiple pixels of the depth image cast through the cell and maybe clear it so the user never notices, if nothing else with speckling noise, the next update cycle will clear it.Anyhow, because of noise, I've found that "a" way to deal with it (note: not saying its the best way, really open for suggestions or other ideas) is to raise the minimum Z value of measurements we take in. We see alot of ground, and if ground is noisy, then we can't say that we take measurements exactly at Z=0 meters because then something that's 0.005 m from the ground will appear as an obstacle right in front of our robot. So we raise that up to like 10cm so that we don't deal with ground noise, but then you lose the ability to specifically identify smaller objects. Another way is to just detect the ground plane with ransac and then remove it, which is more or less what the thing below is doing, but only for the single camera feed that is used for ground-detections rather than running over all of them. A third way I've thought about is the mark threshold parameter, but I think that column-based "noise" removal is really not a good replacement for filtering and it is pretty hacky. Functionally there are lots of things that are flat and thin that then this would not mark in the costmap (tables, shelves, things you only see from the top, etc)
I've dealt with that then by implementing a single, probabilistic-derived filter to take in a single depth stream and compute normals to find smaller objects, as I've had usually 1 camera pointed down-ish and the others pointed up-ish. Running a filter like that on 1 stream at a low rate is practical for a mobile robotics application. That however, lacks generality and while it worked, it certainly wasn't perfect.
so taking in gradient info rather than voxels, my hope is with a bunch of measurements, even noisey, when we're estimating the gradient given some set of measurements over a set of poses, that we'll have a much closer look at what it is in reality and be able to detect more fine changes in gradient to find smaller obstacles. For 2D robots, we just set a very fine threshold for navigability on the gradient of the ground, and for outdoor robots, we set a sharper one. I think this would also not involve raycasting (maybe) to clear or maintain space, so just like my STVL work, we then have a bunch of CPU we can use to play games with statistics on the measurements we have for obstacles / gradients.
For redesign - we need to make an interface for the costmap_2d and our new thing to use so the models can be swapped out. This should include:
- type() (grid, gradient, etc)
- is_ready() (is tf actived and data flowing so its safe to use) On top of the usaual:
- GetCost()
- SetCost()
- etc
Re-evalutate openvdb given it has many similar elements as grid_Maps with iterators but also GPU optimization via the nvidia optimized version. At least in considering a raytracing 3D costmap method but even then an elevation mapping representation, though won't make use of the sparsity well.
Other random notes:
I think a 3D cost grid is an intuitive way to go from a 2D cost grid thing like costmap_2d, but elevation representations instead serve better in my opinion. You can still use raycasting if you want, or you can use more lightweight probablistic methods. Plus it scales N2 instead of N3. Plus you dont have to "guess" what the normals are from a voxel grid, you have them pretty much directly with a light convolution
I want the new ROS environment to work with 2D/3D cost grids as well as elevation mapping (pick you own representation or mix and match based on sensor modalities). and potentially have MoveIt and Autoware use it too, the one environmental model to rule them all, but would have to add some self-filters and such
http://ais.informatik.uni-freiburg.de/teaching/ss13/robotics/slides/17-3dmapping.pdf
https://www.digitalfish.com/portfolio-item/nasa-terrain-mapping/
https://nicolovaligi.com/occupancy-mapping-benchmarks.html --> more proof STVL's introduction of openVDB is good
https://developer.nvidia.com/gvdb-samples --> GPU version of openVDB
https://www.digitalfish.com/portfolio-item/nasa-terrain-mapping/
Really time to start designing a solution now that grid_maps is almost done. Main topics:
- Processing depth cameras
- Processing 2D lidar
- Processing 3D lidar
- Processing sonar / radar
- Elevation maps robust height computation (probablistic / filtered)
- dynamic obstacles in scene - height map
- dynamic obstacles - representation
- Vertical structures (going into caves, below some object like a table or cart, etc)
- Integrations with a 3D costmap
- Integrations with a 2D costmap
- Local rolling nature allowing RPY tilts so heights / gradients are relative to current pose and the full poses at each positions considered.
- Global - whether to incorporate RPY and/or handle orientation drift
- Plugin interfaces to replace costmap 2d with this or other environmental variants
- Places for openVDB vs grid_maps vs home grown
- CostmapFilter representations (keep out, speed zones, another 2D masks)
- Using 2D / 3D static maps, inflations
- Collision checking (footprint? mesh? URDF?)
Interesting thought for integrating both height maps and 3D costmaps;
Find if pts in a column have 2 modalities. If so lower is ground upper is voxel grid. The minimum difference in modality a parameter depending on environment type (flat ground can be lower, non-flat higher). That would differentiate:
- Floor-ceiling
- Floor-table
- Floor-extended human arm
Then voxel grid is collision checked against robot model & height map is against gradients able to traverse (traversibility estimation)
Best of both worlds! No 2d costmap info but 2d info can be represented by 3D voxel grid (openvdb/gvoxels)
Some more notes on this topic: integrated costmap (now called "risk map") and height map. The separation of ideas is the height map processes raw data (3D lidar, 2D lidar, depth from RGBD) for straight representation of the environment. Then the risk map is all of the association costs like inflation, detections that are treated uniquely with rules, etc. The risk map is then a "overlay" on the height map which restricts the height map's viable search locations based on an overlay of "rules" or annotations. This can include things like:
- Detect couch -> know people generally face forward for TV on couch. Add rules for weighing that region in front of the couch as higher risk than other areas.
- Speed / keep out zones
So no longer think of them as "cost" maps, but now as "risk" maps and the costs for actual measurements from the environment are in the height model. This helps integrate both 2D annotations / data with the height map in a coherent way.
I want to create something somewhat novel similar to height maps, but more focused on the gradients between the heights than the heights themselves. the gradient is the pertinent information for whether it's passable or not
Raw notes:
Using semantic segmentation and detection to identify objects that could be risky and add sp. rules to them. Not just obstacles, but what kind of obstacles & treating that vision more seriously & use it. Examples:
- don't opt to drive in front of the couch, narrow & people face that way don't like being blocked
- don't opt to drive behind chairs with people in them
- robots at docks generally don't move
- forklifts & uboats move in any direction & quickly
- people
- other robots
- random rubbish on ground or in way
- maybe there's less actual classes of things that matter for specific rules and it would be tractable (otherwise default to some profile like inflation that's a good general purpose estimator)
I suppose this could be tied into or thought of as motion or "use" models
You're always going to buffer in depth data in case there are missed detections / segmentations / class identification -- height map. But, we could largely reduce it to just edge cases and most normal cases are handled based on detections / segmentation / dynamic tracking. Do we need to correlate the height map obstacles and the risk map detections in the representation to prevent error-based aliasing?
The “costmap” really turns into a "risk map" of detections / segmentations overlayed on a height map to better represent smaller things and traversibility in more types of spaces. Risk based on this info is an important perception overlay to the general “is this even possible” traversibility height map. It provides the "secret sauce" for where to go when, while the height map just tells us where is even at all possible.
Think of a risk map based on models, detections, tracks, and semantic information about those things. That info is overlayed and used as a cost function on the traversibility map underlay, which is purely about what’s possible with no other knowledge about these rules (based on depth and probabilistic models). Risk overlay is all derived quantities like:
- dynamic track
- inflation base type if no other knowledge
- sp. rules for semantic obj detections (live inputs)
- keep out / manual map annotations (infrastructure inputs)
- speed
- temporal changes / variations
And naturally fits in the idea of having a merged height & cost maps.
Why height map?
- flexibility: robots do more, go outside, new applications, indoor & outdoor
- smaller obj & get over ramps, gaps, imperfections, leverage algorithms from other communities
- same memory efficiency
Why risk map?
- continue to have 2D annotations used for height maps to leverage costmap filters work
- Continue to have 2D grids to express special rules about objects w.r.t. planning and control
- More formally have a structure to separate data buffering (height map, current costmap) from rules (risk map) based on modern detection trends or other more complex rules.
http://ais.informatik.uni-freiburg.de/teaching/ss19/robotics/slides/17-3dmapping.pdf
https://wppmfr.isr.uc.pt/pdfs/WPPMFR_2020_paper_5.pdf
See #2229 for some discussion on this topic further. I believe height maps are the way to go with a layer built around 2D still in order to represent semantic information like keepouts. For the total outdoor environment case, this would only be used for the local trajectory planning, and for the planning we use a route planner that doesn't require an environmental model.
Notes from another conversation
My general plan was this:
- Start with abstracting away the specific environmental models via a plugin interface https://github.com/ros-planning/navigation2/issues/1517. That means that costmap_2d and all other environmental models (e.g. height_map or something) would themselves be plugins of the navigation servers to be used in planning an control. Doing this first lets us easily swap between multiple environmental models.
- Update the planner / controller APIs to be given these plugin objects instead of costmaps and update the algorithms to use them instead
- Start designing a new model with feedback from users to support 2.5D https://github.com/ros-planning/navigation2/issues/1278 potentially with support for multi-level structures. Fix any integration issues between the planners/controllers working in height maps vs risk maps
- After system is designed, create new sensor processing layers for radar/3d lidar/2d lidar/sonar/etc so there's analogous capabilities in this new model as costmap_2d
In terms of tech, I was thinking about using the grid_maps package from ETH-Zurich, but they haven't really been maintaining it for ROS2. I've taken the liberty of getting it ported over to ROS2 and doing a 'nights and weekends' maintenance of it to keep it up to date roughly speaking so its ready if/when Nav2 wants to use it. I'm obviously familiar with the codebase, but know there's some legwork to figure out all of what it provides and what we need to put on top of it for a navigation application. They also have elevation_mapping and some other work based on that, I was going to take a look at those but not use them, but take some lessons learned from them in creating a new one
It is probably wise at this point to also consider GPU / hardware accelerated options for those that have them available, since this kind of work is leaning in that direction
Pulling https://github.com/ros-planning/navigation2/issues/1517 into here: pluginize environmental models so we can use this new model as an option to a costmap representation