Extend costmap2D to costmap3D, while still focusing on 2D navigation

[alanxuefei]

Feature request

Extend costmap2D to costmap3D by incorporating a Z dimension, while still focusing on 2D navigation

Feature description

A mobile robot with a larger arm navigates from a corridor to a lab table, matching the robot base's height.

In the corridor, its footprint includes the arm to avoid wall collisions. Near the lab table, the footprint is adjusted to exclude the arm, allowing closer access.

By utilizing a 3D costmap and 3D footprints, collision checks iterate through the costmap [i] and associated footprints. This enables robots to navigate narrow areas with obstacles of varying heights.

(Generated by ChatGPT)

Implementation considerations

• In the short term, we begin with two dimensions at Z. The Path Planner can then plan a feasible path in narrow lab environments for mobile manipulators.
• After that, the accuracy of AMCL can be also improved due to having more cell information to adjust particle weights.
• Collision checks with costmap2D[i] can be processed using GPUs, as this is suited to parallel processing.
• Eventually, the 3D aspect can also integrate with 3D deep learning, although this is a long-term benefit.

[alanxuefei]

Should I start implementing this experimental feature? If there are any considerations, please feel free to let me know.

[SteveMacenski]

We have a more generalized ticket about redesigned environmental models #1278 - lets keep discussions in one place, but yes, I think there are several dozen requirements for an effort like this to be sufficiently general and meet all of the modern needs of mobile robotics users.

I would never stop someone from wanting to build a new model for their applications and open-sourcing it - and perhaps that would be a useful starting point for a Nav2 redesign, but when we get to touching an environmental remodeling, I want a complete break from the costmap thought process to be able to leverage new technologies in perception and represent a broader set of modern surface applications. Designing up front all of the important features based on requirements is necessary for something like this. Its going to be unquestionably complicated so making sure we don't design ourselves into a corner by making short cut assumptions when we start