Redesign Environmental Representation

[SteveMacenski]

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

[SteveMacenski]

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

[SteveMacenski]

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.

[SteveMacenski]

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

[SteveMacenski]

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

[SteveMacenski]

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?)

[SteveMacenski]

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)

[SteveMacenski]

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

[SteveMacenski]

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.

[SteveMacenski]

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

[SteveMacenski]

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

[vinnnyr]

@SteveMacenski I am just curious, in your view, what's the latest on this topic? Have your above thoughts and notes changed in anyway since last year?

[SteveMacenski]

Not much has changed here. It was on my plans for next year but you know how that looks at this point...

[SteveMacenski]

+1 to add height map & 3D map version of planner/control servers to support the multiple environmental types of requests

[joewong00]

Hi. I'm very interested in this topic especially since I'm trying to localize my robot in both outdoor and indoor environment. What is your view on using my own 3d localization method using a 3d lidar pointcloud (such as scan matching) to replace with the current AMCL? Is it possible to achieve that?

[SteveMacenski]

Consider: have getCost APIs be rather than point-costs be cost-to-go. these could be still point-costs, but could have different costs based on the transition. So have last pose and proposed pose as the API fields. That would help handle situations like traveling out of keepouts (once in them) differently or other more advanced traversal functions based on gradient directions in height maps potentially

Also worth potentially having an (1) environmental grid model, (2) semantics model including things like what we have as costmap filters now, and (3) vectorized AI models that can be treated with that information separately that are combined on value-request rather than squashing each iteration. That way, grid/height can say X, AI detection model can say Y, and semantics can override for behaviors in context. Also, not quanitizing Ai information into a grid and can take into account transition functions with timestamps potentially for dynamic models

[SteveMacenski]

Some really high level jotted thoughts for @DLu https://docs.google.com/document/d/1obaQQ_lNVa0vRhjJK1yKaVOIT8VdjAGlR5-61_wsAOI/edit?usp=sharing. I can't seem to find my design documents from 2021 when I was thinking about this more seriously unfortunately :/ I had some reasonably thought out solutions to a few of the problems we'll have to redo

[alanxuefei]

Following the discussion at https://github.com/ros-planning/navigation2/issues/4186

I will begin extending Costmap2D to Costmap3D as a starting point this week.

Costmap2D offers comprehensive APIs and layering concepts, ensuring compatibility with the rest of Navigation2.

Indeed, Costmap2D is a voxel map. Voxels should continue to play an important role in the future due to their intuitiveness. Voxel maps can be converted into more advanced formats, such as OpenVDB, deep learning models, or 3D Gaussian, to enhance efficiency. However, the APIs and layering concepts should remain consistent.

[Ryanf55]

Is there any plan or in progress work to support vector based costmaps? The real world application here is a sea rescue drone that is only allowed to fly over water otherwise the company loses their permits.

Rather than using a 2D raster grid to represent a map, these data structures are vector based. I would like to implement a global planner using NAV2 to find a route between a start and goal state against this coastline. I would like to avoid discretizing the area into a large 2D raster - that would not be as accurate as the vector, and consume far more memory. This is related to the coastline paradox.

You can download vector-based coastline maps for Europe that are .shp files. I opened this up in QGIS.

NAV2's GlobalPlanner's configure function currently takes a nav2_costmap_2d::Costmap2DROS object.

Is it in scope of this issue to support vector based 2D maps? One idea I had is to add a new virtual interface to the GlobalPlanner that takes in a generic costmap type through dependency injection. This would allow users of NAV2 to bring their own costmap representations.

[SteveMacenski]

Hi, no there has not been any progress of note on this topic. Other areas have been of recent attention. This sounds like a neat application though and I added a note on this in my notes / requirements document on this subject. That is not currently a public document since its my personal set of notes and thoughts about this subject and isn't written for public consumption, I'm happy to share it with you if you'd get value from random, incoherent bullet point thoughts that I'm collecting for whenever I or some other organization has the resources/time to start this in earnest.

Admittedly vector versions of 2D maps wasn't on my radar as I thought about sharding occupancy maps to minimize the amount of grid-maps on disk for massive spaces, but that does seem quite sensible. Aerial constraints like what you're working on are a bit different in their needs however. For example, you probably don't need inflation around obstacles as much as massive total restriction areas around obstacles and certain areas -- so your massive freespace planning problems are structured less grid-y and potential-field-y by nature (and are thus subject to different planning techniques & constraints as well) and shouldn't be represented in such ways. I'm not sure how much it makes sense to try to fit vector 2D maps into a grid-map like structure whether cost-maps or height-maps or what have you, but I'm open to your thoughts based on your application knowledge. Perhaps its easier than I think. I'm thinking though if you have some massive open space with sparse keep-far-away obstacles, the nature of the data and the types of planning algorithms you want to perform on it is better suited by non-grid-based planning algorithms anyway. And in that case, there's not much point in trying to fit it into the Planner Server which has primary grid-based algorithms at play, even with an abstracted environmental model.

Abstracting out the representation so that one such implementation is costmap 2D is not a bad starting point. Its where I would start with this as well -- surveying the uses of the costmap APIs and creating an abstract interface with abstract APIs which costmap 2D is only one such implementation that others can replace to continue to use the same Task Server, Algorithm Plugins, and other pieces of software with different models.

---

A slightly different direction would be to have a new nav2_3d-esk repository that has a Planning Server specifically for Aerial / Submersible / sparse 3D needs. That way it has its own environmental model with algorithms sensible for that type of data structure and application needs. It could also be the start of a Nav2 branch off for more planning, control, systems, etc for Aerial / Submersible robotics autonomy moving forward. It could still leverage Nav2 core (utils, BT, navigators, etc) but replace segments that are meant for surface robotics for aerial 3d applications.

[Ryanf55]

Yea, feel free to share. I sent you my email in slack.

Regarding inflation, we really just need to account for the uncertainty in the UAV position + uncertainty in the coastline estimate. If inflation is the right technique, so be it. For vector based solutions, I've used distance to object compared to uncertainty as a criteria.

I would like to figure out a way to reduce the assumption of "grid-y" data structures. Perhaps I could at least propose a PR to switch to using dependency injection for the costmap and/or an abstract class that gives more flexibility? Even if we don't have an idea of the exact nature of a vector based class, it would at least allow nav2 users to bring their own costmap structures.

If you do not think that the planner server is suitable for extensibility to non-grid algorithms, do you think it's still worth abstracting the environment away from costmap 2D? Perhaps I could spend time looking at how the planner server can be modified to suit non-grid algorithms?

[SteveMacenski]

I'd be open to that! Its Step 0 for any potential replacement for Costmap2D I think. Surveying the APIs used in the Task Server and the algorithms that leverage the costmaps (planner, controller, smoother, collision checker, etc) and abstracting them with other possibly useful APIs is a good first step of that. Then proposing a set of abstract APIs based on those that generalize to more implementation types

If you do not think that the planner server is suitable for extensibility to non-grid algorithms, do you think it's still worth abstracting the environment away from costmap 2D?

Yes, because there are other non-costmap "grid-y" data structures, like height maps or even 3D occupancy grids for more complex collision checking of robot dynamic geometry. And Costmap2D needs a refactor/replacement even if using 2D cost-map like things for performance, testing, and cleanilness reasons, so having the abstraction would allow users to switch over without performing surgery for each new environmental type.

[Ryanf55]

Where should this generic Costmap interface live?

• nav2_core::GlobalPlanner depends on nav2_costmap_2d::Costmap2DROS
• nav2_core::Controller depends on nav2_costmap_2d::Costmap2DROS
• nav2_costmap_2d does not depend on any interface packages

I would like to make nav2_costmap_2d inherit from a common interface, but if that interface went in nav2_core, there would be circular dependency. What about a new package called nav_interfaces?

Side note: * nav2_map_server::MapServer depends on the nav_msgs::srv::GetMap which uses a 2D occupancy grid - This is another spot that could use dependency injection for the map message type (which is application specific).

[SteveMacenski]

I'd say EnvironmentModel or WorldModel or something non-costmap specific, but that's just naming.

Maybe I'm just jetlagged, but if you made it in nav2_core and adapted GlobalPlanner and Controller to use that EnvironmentModel (which I think is the point of the exercise), that would remove nav2_costmap_2d from anywhere in the nav2_core dependency tree. nav2_costmap_2d would become an implementation of nav2_core::EnvironmentModel. I don't see the circular dependence.