Send Motion Capture System pose data to PX4

LucasW

Hi,

I am trying to send Motion Capture System (MCS) data to the FCU to fuse with or replace the VIO estimate. I have been reading some posts about this topic as I am not the first one tackling this, but there does not seem to be an accepted way of doing it.

I run a node onboard the kinetic docker image onboard voxl that subscribes to our MCS and generates a PoseStamped ROS message. I compiled the vision_pose_estimate plugin from mavros_extras as an independent node to relay the pose messages over mavlink, and configured it as follows (according to the way mavros node is launched):

int 		system_id 	= 1; // env PX4_SYS_ID
int 		component_id	= mavconn::MAV_COMP_ID_UDP_BRIDGE; // 240 
int 		tgt_system_id	= 1; // env PX4_SYS_ID
int 		tgt_component_id= 1; // env PX4_COMP_ID
std::string fcu_protcol		= "v2.0";
bool		respawn_mavros	= false;

Since the file /etc/modalai/voxl-vision-px4.conf is set with:

"qgc_udp_port_number": 14550,
"localhost_udp_port_number": 14551,

I let the vision_pose_estimate node send data over UDP via port 14552 (sending to 14551 blocks any other communication). The node seems to be relaying pose data fine (at least no error when sending the data). However, I am not sure whether PX4 is actually seeing any of it... I tried looking at the mavlink shell provided by voxl-px4-shell and ran the command listener vehicle_visual_odometry, but only the VIO data seems to be received.
How can I check whether the MCS pose is reaching PX4?

@Chad-Sweet mentioned that modifying the voxl-vision-hub at this location would allow relaying MCS pose instead of VIO pose.
Is it indeed the recommended way?

Thanks in advance for any help!

The voxl-version command returns the following, running of VOXL-Flight:

voxl-version 
cat: /etc/modalai/voxl-software-bundle-version.txt: No such file or directory
--------------------------------------------------------------------------------
system-image:    ModalAI 3.3.0 BUILDER: ekatzfey BUILD_TIME: 2021-06-06_19:28
kernel:          #1 SMP PREEMPT Sun Jun 6 19:41:01 UTC 2021 3.18.71-perf
factory-bundle:  1.0.1 (Yocto installation)
--------------------------------------------------------------------------------
architecture:    aarch64
processor:       apq8096
os:              GNU/Linux
--------------------------------------------------------------------------------
voxl-suite:
Package: voxl-suite
Version: 0.5.0
Depends: libjpeg_turbo (>= 9.0.4), libmodal_exposure (>= 0.0.2), libmodal_json (>= 0.3.6), libmodal_pipe (>= 2.1.1), librc_math (>= 1.1.5), libvoxl_cutils (>= 0.0.2), libvoxl_io (>= 0.5.4), mavlink-camera-manager (>= 0.0.2), mongoose (>= 7.3.0), opencv (>= 4.5.2-2), openmp (>= 10.0.2), voxl-camera-calibration (>= 0.1.1), voxl-camera-server (>= 0.8.1), voxl-cpu-monitor (>= 0.2.0), voxl-dfs-server (>= 0.2.2), voxl-docker-support (>= 1.1.3), voxl-gphoto2 (>= 0.0.5), voxl-imu-server (>= 0.9.1), voxl-mavlink (>= 0.0.2), voxl-modem (>= 0.12.0), voxl-mpa-tools (>= 0.3.6), voxl-nodes (>= 0.2.0), voxl-portal (>= 0.1.2), voxl-qvio-server (>= 0.3.4), voxl-streamer (>= 0.2.6), voxl-tag-detector (>= 0.0.2), voxl-tflite (>= 2.2.3), voxl-tflite-server (>= 0.1.5), voxl-utils (>= 0.8.4), voxl-vision-px4 (>= 0.9.5), voxl-vpn (>= 0.0.3)
Status: install user installed
Section: base
Architecture: all
Maintainer: james@modalai.com
MD5Sum: f55ec020942e92ff12bc137c2aa8d2a5
Size: 1956
Filename: voxl-suite_0.5.0.ipk
Description: meta-package for voxl-suite software collection
Installed-Time: 1642294777

Package: voxl-suite
Version: 0.2.0
Depends: docker, imu_app (= 0.0.6), libvoxl_io (= 0.5.2), voxl-cam-manager (= 0.2.2), voxl-docker-support (= 1.1.1), voxl-hal3-tof-cam-ros (= 0.0.2), voxl-modem (= 0.10.0), voxl-nodes (= 0.0.8), voxl-rtsp (= 1.0.2), voxl-utils (= 0.5.2), voxl-vision-px4 (= 0.6.8), voxl_imu (= 0.0.4), voxl-time-sync (= 0.0.1), voxl-vpn (= 0.0.2), librc_math (= 1.1.2), libmodal_pipe (= 1.2.2), modalai-vl (= 0.1.3)
Status: unknown ok not-installed
Section: base
Architecture: armv7a
Maintainer: james@modalai.com
MD5Sum: af706cd3c1ea59f274f2ed9b93141f1d
Size: 870
Filename: voxl-suite_0.2.0.ipk
Description: meta-package to install all of the voxl-suite

Package: voxl-suite
Version: 0.4.6
Depends: libmodal_exposure (>= 0.0.2), libmodal_json (>= 0.3.5), librc_math (>= 1.1.5), libvoxl_cutils (>= 0.0.2), libvoxl_io (>= 0.5.4), opencv (>= 4.5.2-1), voxl-mavlink (>= 0.0.2), libmodal_pipe (>= 2.0.7), voxl-camera-server (>= 0.7.1), voxl-cpu-monitor (>= 0.1.7), voxl-dfs-server (>= 0.2.0), voxl-imu-server (>= 0.8.1), voxl-mpa-tools (>= 0.2.6), voxl-qvio-server (>= 0.3.1), voxl-tag-detector (>= 0.0.2), voxl-tflite-server (>= 0.1.0), voxl-vision-px4 (>= 0.9.2), openmp (>= 10.0.2), voxl-tflite (>= 0.0.1), voxl-nodes (>= 0.1.6), mavlink-camera-manager (>= 0.0.2), voxl-docker-support (>= 1.1.3), voxl-gphoto2 (>= 0.0.5), voxl-modem (>= 0.12.0), voxl-streamer (>= 0.2.3), voxl-utils (>= 0.7.1), voxl-vpn (>= 0.0.3)
Replaces: voxl-suite-dev
Status: unknown ok not-installed
Section: base
Architecture: all
Maintainer: james@modalai.com
MD5Sum: a800fd5f7c98cfa4e6ed1638cf25d909
Size: 1902
Filename: voxl-suite_0.4.6.ipk
Description: meta-package for voxl-suite software collection

--------------------------------------------------------------------------------

LucasW

What about publishing MCS pose messages of type vio_data_t on the qvio pipe to leverage the MPA?

James Strawson

Hi Lucas,

To do this through mavros:

make sure voxl-qvio-server is disabled
disable the vio thread in voxl-vision-hub by setting en_vio = false in /etc/modalai/voxl-vision-hub.conf (used to be called /etc/modalai/voxl-vision-px4.conf on SDK 0.9 and older)
Point mavros to connect to port 14551 which is opened by voxl-vision-hub for talking to mavros/mavsdk. This is the port I use to send in target_local_position_ned commands to do a figure-eight demo with mavros.
Make sure that you configure your MCS node and mavros to send in the odometry messages as accurately timestamped to the linux monotonic clock as possible so PX4's EKF2 can account for the pose estimation and transmission delay.

In the case of an external flight controller connected to VOXL, voxl-vision-px4 (SDK0.9 and older) or voxl-mavlink-server (SDK1.0 and newer) respond to PX4's timesync requests so that PX4 has a good estimate of the clock delta between PX4 and Linux Clock Monotonic.
In the case of PX4 running onboard a VOXL2, PX4 (and therefore EKF2) natively uses the linux monotonic clock so they are perfectly in sync.
I have seen people's ROS nodes send out monotonic time until VOXL can ping an NTP server, then ROS suddenly sends timestamps in clock-realtime. This jump is unacceptable, all sensor data packets in the entire system need to share one consistent monotonic clock with no discontinuities.

PX4 will report its own odometry message as well as a local_position_ned message with mostly similar data. If your MCS data is going through then PX4's reported odometry should track yours closely.

You can also do this by publishing a pipe. You don't have to copy the qvio pipe name, in SDK0.9 you can edit the "vio_pipe" field in /etc/modalai/voxl-vision-px4.conf to point to your own pipe.

I've seen others struggle with MAVROS messing with their timestamps. Port 14551 is a straight shot through from MAVROS to PX4, the only messages that are touched are local position setpoint messages and that's only when using the apriltag relocalization feature in voxl-vision-hub (formerly voxl-vision-px4). Otherwise whatever MAVROS sends to that port goes straight to PX4 unaltered. I would suggest starting there and if PX4 still rejects the MAVROS messages then try publishing into MPA with the vio_data_t type

Good Luck,
James

LucasW

Hi @James-Strawson,

Thanks a lot for the very detailed answer, much appreciated!

I finally managed to have PX4 receive Motion Capture System pose data and fuse it. Here is what I did (following your guidelines):

I disabled the qvio server with: systemctl disable voxl-qvio-server
I installed from source the mavros_extras package (v1.0.0) and enabled the vision_pose_estimate plugin by publishing on the topic /mavros/vision_pose/pose (of type geometry_msgs::PoseStamped).
A node listening for the MCS poses runs onboard VOXL and stamps the poses with the monotonic clock.

When I visualize the MCS pose and the /mavros/local_position/pose (PX4 state estimate) in RVIZ while manually flying the drone, the estimate lags behind the MCS pose. It sometimes loses track of the MCS pose and jumps back to realign with it. Also, QGroundControl sometimes relays that the sensor calibration should be checked.

Any suggestion on where this behavior might come from?

LucasW

I just realized the MCS pose publishing rate is likely too high, which may cause the lag. I am investigating...

LucasW

Alright, solved it. I adjusted the MCS pose rate to be at 30 Hz (should be fine up to 50 Hz according to PX4 documentation), although this does not seem to have any significant impact.

The problem was that the EKF2_AID_MASK must be set to 24 (only position and yaw fusion) for motion capture, instead of 280 (default with VIO, which sends odometry messages including velocities). Hence the following fields must be set as follows:

...

vision position fusion

vision yaw fusion

...

vision velocity fusion <-- Disable for Motion Capture (enabled by default for VIO)

I did not have to modify any other parameters.

@James-Strawson, thanks again for the help!