----------------- OptiTrack ---------------------------------

Must do with each optitrack activation:

1. make sure the the all the cameras are activated
2. position the quadrotor at [0,0,0] with heading x direction to align coordinate system
3. make sure Motive up direction is Z
4. make sure the quaternion is [1 0 0 0] and not [-1 0 0 0]
5. make sure local position topic is 0 0 0 heading 0
6. make sure the /fmu/in/vehicle_visual_odometry topic is published at 180hz: ros2 topic hz /fmu/in/vehicle_visual_odometry
7. on motive, make sure the transition time is ~6ms, if not, restart Motive.

start the communication:

Setup your optitrack configuration:

mocap4r2_ws/src/mocap4ros2_optitrack/mocap4r2_optitrack_driver/config/mocap4r2_optitrack_driver_params.yaml

Launch optitrack system:

cd optitrack_ws  
source install/setup.bash  
ros2 launch mocap4r2_optitrack_driver optitrack2.launch.py

Check that Optitrack configuration works fine and is connected. As the driver node is a lifecycle node, you should transition to activate:

cd optitrack_ws
source install/setup.bash
ros2 lifecycle set /mocap4r2_optitrack_driver_node activate

ros2 launch mocap_px4_bridge run.launch.py

must check:

1. make sure messege is pubshished on: ros2 topic echo /fmu/in/vehicle_visual_odometry

2. move the quadrotor and make sure the messege correspondes to FLU coordinate system, if not, here are possible reasons: (1) the Z is not set as up axis as in the streaming settings (2) the rigid body coordinate system is not set properly, see picture: