Problems when trying to fuse VIO data with uXRCE-DDS Bridge

[ripdk12]

I've written a ROS2 node that sends VIO data to the /fmu/in/vehicle_visual_odometry topic using the uXRCE-DDS bridge. To test that the fusion is handled correctly I've created a stationary flight log, and I noticed by reading the /fmu/out/vehicle_odometry topic (vehicle_odometry uORB message), that the estimator jumps between what is perceived by the VIO and the internal (GPS+IMU) odometry.

Here is an example of a message that is sent to PX4:

timestamp: 1884348743
timestamp_sample: 1884348745
pose_frame: 2
position:
- 0.3625916540622711
- 0.2121472805738449
- 0.06812606006860733
q:
- 0.9994385838508606
- -2.914839751610998e-05
- 0.03003314882516861
- -0.014851653948426247
velocity_frame: 2
velocity:
- -0.0002751341671682894
- -0.00013385475904215127
- -0.0017852116143330932
angular_velocity:
- -7.331899541895837e-05
- -1.2296965905989055e-05
- 8.007127325981855e-05
position_variance:
- 2.4092826692023728e-11
- 1.7808475180625116e-11
- 2.919307684479122e-09
orientation_variance:
- 5.31228099703851e-12
- 1.589840048889582e-13
- 5.67606378565344e-12
velocity_variance:
- 6.862764507786778e-07
- 3.4139620019857375e-09
- 2.808655779062974e-07
reset_counter: 0
quality: 0

I also noticed that there seems to be a Visual Odometry Latency issue, something I've noticed is a recurring issue here and here, though I see no explanation of how to fix this timestamp issue, or even if it makes any difference.

Should it be unix time? I simply copied the timestamp from the advertiser example from the px4_ros_com package: odometry_msg.timestamp = std::chrono::time_point_cast<std::chrono::microseconds>(std::chrono::steady_clock::now()).time_since_epoch().count();

In the following image you can see exactly when the estimator stops fusing the yaw data of the VIO, followed by the position estimation drifting, and then suddenly snaps back:

I obviously don't dare to do any real flight tests with the system this unstable.

@bresch wrote this response to a similar issue #21504. However, I am actually sending in the FRD MAV_FRAME, so the solution that is suggested doesn't work for me.

Any help would be greatly appreciated, thanks!

[mzahana]

Hi @ripdk12

I am also trying to relay an output of ROS 2 VIO system that is published as nav_msgs/msg/Odometry to PX4 on the topic fmu/in/vehicle_visual_odometry

My node is written here

My issue is that I am not sure how to populate the uint64 timestamp and uint64 timestamp_sample fields.

Have you figured this out?

[ripdk12]

Hi @ripdk12

I am also trying to relay an output of ROS 2 VIO system that is published as nav_msgs/msg/Odometry to PX4 on the topic fmu/in/vehicle_visual_odometry

My node is written here

My issue is that I am not sure how to populate the uint64 timestamp and uint64 timestamp_sample fields.

Have you figured this out?

I just ended up forwarding the timestamp from my VIO source, it made no difference compared to leaving them blank, however, now the Visual Odometry Latency is 0 ms in the flight review.

[ripdk12]

Update

So I've been doing some more flight tests and have found the best results by setting: EKF2_EV_CTRL = 11 for yaw estimate fusion EKF2_MAG_TYPE = 5 (no magnetometer fusion)

As my EV data is FRD and not ENU this setting means I don't know the global heading unless I manually start the drone facing north each time, which obviously isn't optimal!

However, I STILL have issues with the yaw under flight! The latest flight review shows how the estimator suddenly stops fusing yaw estimation from the VIO source around the 33 second mark (not sure why it says 50 seconds in Flight Review, but in Plotjuggler the time is correct):

This is also the time that the cs_yaw_alignflag goes high, and the cs_ev_yaw flag goes low. According to this page, the cs_yaw_align flag goes high when "the filter yaw alignment is complete", Why does it take so long to complete? Are my VIO variances too high?

A couple of screenshots from my recorded rosbag also shows exactly when the position estimate freaks out:

Before freakout:

After freakout:

Sorry for the missing labels, The leftmost pose is the ground truth (recorded by MOCAP), and the rightmost pose is the ROS2 Topic /fmu/out/vehicle_odometry, which corresponds to the uORB message vehicle_odometry. Sadly I didn't include the VIO estimate in this rosbag, but looking at the .ulog file this estimate seems to be just fine, meaning something unknown (to me) is causing the EKF2 to discard the VIO estimate completely.

[beniaminopozzan]

Hi @ripdk12 You said you are using MOCAP, so I assume you are flying indoor, am I right? If so, do you actually have a GPS fix? How are you providing true North if you are providing external odometry in RFD, and you disabled the MAG, what is providing true North alignment?

In my lab, as we do not have GPS and we only fly indoor, we use pos_frame=LOCAL_FRAME_NED even if the Vicon "global" frame is not North aligned. Did you try that? Keep the EKF2 parameters unchanged and just change the pos_frame in the odometry message.

In your screenshots I see ENU and FLU frames, those are just for display, right? Is the actual data sent to PX4 in NED / FRD?

[ripdk12]

Hi @ripdk12 You said you are using MOCAP, so I assume you are flying indoor, am I right? If so, do you actually have a GPS fix?

Yes, I have GPS fix, this problem persists when flying outdoors as well.

In my lab, as we do not have GPS and we only fly indoor, we use pos_frame=LOCAL_FRAME_NED even if the Vicon "global" frame is not North aligned. Did you try that? Keep the EKF2 parameters unchanged and just change the pos_frame in the odometry message.

I just tried that, and the problem persists. I have a flight log showing the result of an outdoor flight here

A screenshot of the estimator flags:

You can also see the message at the point where the cs_ev_yaw flag goes low (about 42 seconds after takeoff) here:

In your screenshots I see ENU and FLU frames, those are just for display, right? Is the actual data sent to PX4 in NED / FRD?

Yeah you're correct, I translate the odometry from PX4 to ROS frame standard for visualization purposes only.

Because you are mentioning that you don't have GPS in your lab I assume this to be related, as the problem occurs AFTER the GNSS data fusion starts.

[beniaminopozzan]

Thanks @ripdk12 You are probably one of the very first trying to use EV and GPS together :sweat_smile:

PX4 stops fusing the EV data as soon as you get GPS fix. Here are the lines that do that: https://github.com/PX4/PX4-Autopilot/blob/1707805ed273e90313f533823d1ace928682b03f/src/modules/ekf2/EKF/gps_control.cpp#L205-L214

starting_conditions_passing is controlled by: https://github.com/PX4/PX4-Autopilot/blob/1707805ed273e90313f533823d1ace928682b03f/src/modules/ekf2/EKF/gps_control.cpp#L147-L153

Therefore, you can't use EV yaw alongside the GPS (note the _inhibit_ev_yaw_use = true)

At the same time I see that, on ev_yaw_contol.cpp: https://github.com/PX4/PX4-Autopilot/blob/1707805ed273e90313f533823d1ace928682b03f/src/modules/ekf2/EKF/ev_yaw_control.cpp#L53-L68

• EV yaw fusion is disabled (continuing_conditions_passing=false) if GPS is OK ( _inhibit_ev_yaw_use=true )
• EV yaw fusion should be allowed if GPS is enabled and EV is in NED

I'm confused here now. Can EV and GPS be used together for yaw estimation? Let me ping @dagar for this matter.

Anyway.... @ripdk12, you started the topic with a bigger issue, so what do you want to do exactly? Fly indoor and then go outdoor?

[ripdk12]

Thanks for the quick response @beniaminopozzan

PX4 stops fusing the EV data as soon as you get GPS fix.

Thanks for clarifying this, I was quite confused by these lines in the code.

Anyway.... @ripdk12, you started the topic with a bigger issue, so what do you want to do exactly? Fly indoor and then go outdoor?

I'm doing my thesis in the use of VIO to improve general UAV flight. I am primarily interested in outdoor flight, keeping in mind that VIO is probably mostly useful indoors. The indoor flight tests I have done has mostly been for using MOCAP as my ground truth, for evaluating improvements in flight stability and odometry estimation.

My goal has been to do flight tests with and without VIO and do some sort of data analysis of the results (captured by various ROS topics exposed by the uXRCE bridge).

I suppose I could just completely shut off all GPS fusion while flying with VIO (and hope that I don't lose tracking) However, originally I just assumed the EKF2 simply fused VIO data as long as the covariances were below a certain threshold, which I guess is what I would want it to do.

[bresch]

You can fuse VIO position and GPS lat/lon at the same time if the VIO data is correctly "north" oriented. The when fusing both position sources, EKF2 will estimate a "VIO position bias" to keep the 2 measurements consistent: since GPS is absolute and VIO relative, you can think of the measurement models as: position = GPS = VIO - bias so the bias will accumulate all the errors to maintain the innovation as small as possible.

You can find the bias in estimator_ev_pos_bias (https://github.com/PX4/PX4-Autopilot/blob/main/msg/EstimatorBias3d.msg)

[DronecodeBot]

This issue has been mentioned on Discussion Forum for PX4, Pixhawk, QGroundControl, MAVSDK, MAVLink. There might be relevant details there:

https://discuss.px4.io/t/px4-community-q-a-june-28-2023/32873/8

[Amiraqaie]

Hi currently we are developing same project. in our work we are using RtabMap odometry and calculating the 6DOF pose from rtabmap odometry in NED frame and the velocity in the FRD frame and publishing the message to "/fmu/in/vehicle_visual_odometry" topic. note that the angular orientation of message is aligned with true North direction. we are taking off the ground and we can get perfect position hold for about a minute but after the autopilot suddenly switchs to altitude flight mode. when i open my simulation log the local vision position status is rejected and then offboard signal lost flag is setting 1. i dont know the exact reason for this problem but i was prevously working on the micro_rtps_client for the exact same mission and the same code for calculating pose and velocity to pass it to px4 and i did not have any issue with that. the problem started when i migrated my code and system from "ubuntu 20.04 + ros2 foxy + micro_rtps (px4 1.13)" to the "ubuntu 22.04 + ros2 humble + uxrce_dds (px4_main)". note that in px4 1.13 i fuse vision pose and yaw and velocity with EKF2_AID_MASK pluse i turned off the GPS fusion; and in px4_main i did turn off GPS with EKF2_GPS_CTRL and made the EKF2_HGT_REF to vision.

[beniaminopozzan]

@Amiraqaie , could you share a log?

[ViniciusAbrao]

Hi, please, is there any tutorial related to the step by step on how to fly using the VIO using ROS2 and uxrce_dds? In the official PX4 documentation I only can see the setup based on MAVROS.

[mzahana]

@ViniciusAbrao I just used mavros (works with ros2 as well) as it has everything you need to fuse VIO. Using xrce topics requires conversion between px4 and ros frames, which is a daunting task that mavros already does.

[ViniciusAbrao]

@mzahana tks for your answer. I understand that, despite that you can launch it using ROS2, the protocol is still Mavlink, so it is not a communication directly via dds based on the micro client and agent. In my understanding it only makes some kind of bridge. I thought maybe there is a ROS2 node that you can publish directly in the /fmu/in/vehicle_visual_odometry. Actually I am not sure what is the disadvantage of using the Mavlink protocol to send the messages instead of dds for this specific work, maybe you or @beniaminopozzan could clarify if I am wrong.

[fp018]

Hi! I am currently trying to feed visual (mocap) feedback through the uxrce by publishing on the topic _/fmu/in/vehicle_visualodometry. It works with a cube orange flight controller, but I cannot switch in position (local position estimation not valid) with a Pixhawk 6c. Both boards are flashed with the latest release. I am wondering if it could be related to the CONFIG_EKF2_EXTERNAL_VISION not being set to true in the bootloader. Any suggestions? Someone made it work?

Thanks!

[XXLiu-HNU]

@mzahana tks for your answer. I understand that, despite that you can launch it using ROS2, the protocol is still Mavlink, so it is not a communication directly via dds based on the micro client and agent. In my understanding it only makes some kind of bridge. I thought maybe there is a ROS2 node that you can publish directly in the /fmu/in/vehicle_visual_odometry. Actually I am not sure what is the disadvantage of using the Mavlink protocol to send the messages instead of dds for this specific work, maybe you or @beniaminopozzan could clarify if I am wrong.

In PX4 v1.15 : The PX4 ROS 2 Interface Library navigation interface enables developers to send their position measurements to PX4 directly from ROS 2 applications, such as a VIO system or a map matching system. The interface provides a layer of abstraction from PX4 and the uORB messaging framework, and introduces a few sanity checks on the requested state estimation updates sent via the interface. These measurements are then fused into the EKF just as though they were internal PX4 measurements.