3D path planning in an underwater environment with obstacles

edit

Hello,

I was hoping to ask for some guidance from the ROS community on my current project. I am tasked with creating a controller capable of navigating AUVs (autonomous underwater vehicles) and submarines through a simulated underwater environment. The idea would be for the controller to be able to guide these vehicles through a set of 3D waypoints. The environment will contain obstacles like the ocean floor and ice flows that must be avoided. I will likely use UUV simulator, which is built on ROS and Gazebo, to visualize and simulate the environment.

The simulator has simple built in controllers that should be able to implement any feasible path generated for a specific vehicle. However, I cannot find anything capable of generating three-dimensional paths while avoiding obstacles and (preferably) adhering to the kinematic constraints of the vehicle.

The navigation package is only applicable to 2D problems and the consensus for applying MoveIt to path planning for a base instead of joints seems to be that it cannot not be done for non-holonomic vehicles. I understand that OMPL, the motion planning library that MoveIt uses, has path planners capable of 3D path planning subject to constraints, but as it does not have consideration of obstacles integrated like MoveIt does (among other things I would have to implement), it seems I would be reinventing the wheel to accomplish what is supposed to be a small part of a simulator. That being the case, I would love to hear if anyone has any advice on how best to accomplish my task, whether through pointing me towards resources or suggesting alternatives. Thank you!

One note that may be important is that the vehicle does not have to sense the obstacles through simulated sensors for my application. I can provide data about the obstacles in whatever way works. I just need the vehicle to be able to navigate the environment.