StarlingV2: ROS2 offboard control using voxl-microdds-agent

smilon

Hi!

I have recently embarked on the wonderful journey of offboard control using VIO, ROS2, voxl-microdds-agent and voxl_mpa_to_ros2.

My goal is to be able to set TrajectorySetpoints relative to a fixed frame, and let PX4 do all the control between setpoints, using its own position PID controllers.

I understand that a "fixed frame" cannot be obtained by pure VIO since this just creates a local frame where the VIO initialized and that odometry is sent to PX4 in the form of a VehicleLocalPosition (UORB message) and that local frame is prone to drift and/or jumps depending on VIO quality.

In order to solve this I have followed the AprilTag Relocalization guide and set a series of AprilTags on the floor so that I can always have a position relative to a fixed frame. This looks just fine on VIO tab of the web-portal (which is a great debugging tool by the way) as well as on the voxl_mpa_to_ros2 topic -> /vvhub_body_wrt_fixed.

This is the setup, now to the problem...

Before even attempting to pass the visual odometry wrt to the fixed frame to PX4 (instead of the local), I have been trying to get the following offboard example to work. I am not really sure how to debug the TrajectorySetpoint coordinates but whenever I run it the drone is very jerky and flies off hugging the ground in the wrong direction. Eventually it sort of spirals towards the moon but I have not been able to witness the complete behaviour because I test it on a 2x3x2.5m enclosure for safety purposes. The script is just telling it to move upwards (in the NED frame) by 1m.

Initially I thought I will tweak the position PID of PX4 but that did nothing and a coordinate frame inconsistency is much more reasonable given the behavior.

Now, I am pretty much a PX4 / VOXL2 noob but I understand that the odometry obtained from VIO in the local frame is passed into PX4 somewhere in the VOXL stack and that also accounts for the frame conventions. I have struggled a great big deal the past few days debugging this behaviour and am wondering the following:

Where in the VOXL stack is VIO passed onto PX4. Also, is it possible to pass the "fixed frame" odometry without tampering with the source code?
Any idea why I am seeing this behaviour and how I can solve this problem? Could it be a frame inconsistency? I always ensure VIO and position mode works well before I attempt to run the example but the motion in ROS offboard mode is very very jerky.

Any help is much appreciated!
Cheers!

smilon

Ok, in a stroke of epipahny I realised that when switched to offboard mode through ROS2, the drone attempts to do both the modalAI "figure eight" that was configured in /etc/modalai/voxl-vision-hub.conf and satisfy the TrajectorySetpoint given to it by ROS2, resulting in this very jerky motion. Switching offboard_mode parameter to "off" solved this issue!! Works flawlessly now.

Any ideas how I can switch off passing the local odometry to PX4 and pass /vvhub_body_wrt_fixed instead? If I just publish to /fmu/in/visual_vehicle_odometry does it only take input from the publisher or will it publish from both sources similar to the offboard mode example? :)))