How to visualize PX4 SITL instances in RViz2

I am using ROS2 foxy, Gazebo 11 and PX4 SITL to simulate collision avoidance maneuvers between multiple copters and I would like to visualize the path of each vehicle.
I can see a 2D path in QGC of the lateral path but I can not see the vertical path. Also, you can only display the path of one vehicle at a time in QGC.
The question is how can I achieve that in order to have a better view of what each drone did. Is there a way to integrate RViz with the Gazebo simulation so that I can draw the path for each vehicle using its VehicleLocalPositionSetpoint message for example?

1 Like