ROS2 Planowanie trajektori ruchu drona z omijaniem przeszkód i CBF

Potrzebuję zrobić projekt na studia w ROS2, który będzie wysyłał sygnał planowania trajektorii ruchu z algorytmami omijania przeszkód do autopilota PX4 (przez mostek MicroXRCE-DDS) przez node. Dron prawdopodobnie x500_depth (lub podobny, jeśli byłoby lepiej). Algorytmy A* i RRT (lub podobne), następnie do obu dodać w osobnych węzłach, np. A*+CBF (Control Barrier Function) i RRT+CBF. Wreszcie jakieś wskaźniki porównawcze dla obu algorytmów, np. długość ścieżki, prędkość podróży, odległości do przeszkód.