The correct setup to use Optitrack and uXRCE DDS

ROS / MAVROS / MicroDDS
1

Hello, I asked this question in the community Q&A June 28 2023, thanks @rroche for hosting! Just a recap, I am trying to achieve indoor flight with an Optitrack setup using uXRCE dds bridge and ROS2. However, it seems that the optitrack data is not fusing into the ekf2 estimator.

My setup is as follows:

1.) Flash PX4 (master branch firmware) into Pixhawk 6X using QGC
2.) Change UXRCE_DDS_CFG parameter to Telem2
3.) Change EKF2_EV_CTRL parameter to use external vision for everything
4.) Run ROS2 script to publish optitrack data to /fmu/in/vehicle_visual_odometry topic
5.) Run microros agent on the companion computer (Ubuntu 22.04 with ros2 Humble) to start the uXRCE dds agent. The uXRCE dds bridge is connected from FCU (Pixhawk 6x) to the on board computer through telem2 serial.

I can echo out the topics published by the uXRCE dds bridge and I have checked that the optitrack data is indeed being streamed in to the FCU by checking the vehicle_visual_odometry uORB topic in the nsh. EKF2 does not seem to be using the optitrack pose because vehicle_local_position uORB message isn’t converging with the optitrack pose.

Flight log: https://review.px4.io/plot_app?log=ea97afb4-9435-4a3b-89e9-5c5159b87a2e

@dagar did I miss anything out? Do let me know.

Thanks!

2

Hi @eightfingers ,

From what I can see, your parameters are OK. I cannot see the vehicle_visual_odometry on the logs though. It is probably just not logged.
However, you have a lot of

[timesync] RTT too high for timesync: 18 ms

which means that something is wrong with your serial connection. 18ms of RTT is quite a huge value.
First of all, how is the connection made? tty2tty, tty2USB, is there some other hardware in the middle (say… a telemetry radio because the companion computer is not on the drone)?

Then, try the following:

  1. Increase the baudrate as much as you can. If the warnings persist, then
  2. Edit dds_topics.yaml removing every unneeded topics.

If everything starts working after the warnings disappear, perfect :slight_smile:
Next time, log with more information SDLOG_PROFILE with Computer Vision and Avoidance bit set to true.

3

Hello, we managed to get it to fuse by changing some of the input fields of the Optitrack data (VehicleOdometry.msg):

  1. Set the timestamp sample = 0 (For some reason this needs to be zero otherwise it won’t fuse)
  2. Instead of POSE_FRAME_NED = 1 field we used pose_frame = 2. So its something like
# Before
msg.POSE_FRAME_NED = 1
# After, we chaged the frame to FRD too
msg.pose_frame = 2

My connection setup is through tty2USB via an ftdi adapter, I will try out your suggestions to see if the warnings persist.

Btw, in VehicleOdometry.msg,

...
uint8 POSE_FRAME_UNKNOWN = 0
uint8 POSE_FRAME_NED     = 1 # NED earth-fixed frame
uint8 POSE_FRAME_FRD     = 2 # FRD world-fixed frame, arbitrary heading reference
...
uint8 VELOCITY_FRAME_UNKNOWN  = 0
uint8 VELOCITY_FRAME_NED      = 1 # NED earth-fixed frame
uint8 VELOCITY_FRAME_FRD      = 2 # FRD world-fixed frame, arbitrary heading reference
uint8 VELOCITY_FRAME_BODY_FRD = 3 # FRD body-fixed frame
...

Are these message fields actual inputs? Cause they seem to be more of comments rather than actual inputs which confused us.

4

We did more tests and flew the drone around (around a 5x5m square). It seems to fuse however, there is occasional spike up in the vehicle_local_position (x shoots up to 11m and z shoots up to 30m). We are confident that the optitrack did not lose tracking during flight, we are theorizing that EKF2 might be occasionally rejecting the incoming optitrack data which causes the spikes. The logs are below, is there something that we missed?

https://review.px4.io/plot_app?log=ee071078-9485-4d8a-b779-b73e3c6319c7

5

Those are constant fields, they are used to give meaning to the values in other fields.