Add environment representation

[elsayedelsheikh]

Use OctoMap to represent the environment and pass it to the planner to take it into consideration

[elsayedelsheikh]

NVIDIA nvblox can be used as well for scene reconstruction to output Distance fields.

• There's a ROS2 service in ISAAC ROS project that ingests depth images)