Vicon Position Tracking w/ ROS2

[bantno]

@agarlow and I are working on doing indoor flight tests with position feedback from a VICON motion capture system. We have been having trouble getting the VICON feedback onboard our Pixhawk 6X. We tried to follow the motion capture tutorial from the PX4 website (https://docs.px4.io/main/en/ros/external_position_estimation.html), but it seems to be somewhat outdated, seeing as it still refers to using the “EKF_AID_MASK” parameter, which appears to be deprecated.

We have tried to piece together parameter settings for the new [EV_CTL] parameter, but with no luck yet. The parameters we are currently trying are:

EKF2_EV_CTRL = 11 EKF2_EV_DELAY = 0.1ms EKF2_EV_NOISE_MD = EV noise parameters EKF2_HGT_REF = Vision EKF2_IMU_CTRL = 1 EKF2_MAG_DECL = -5.4deg EKF2_MULTI_IMU = 3 EKF2_MULTI_MAG = 3

We wrote a basic ROS2 node to take the VICON data and publish it to the desired PX4 micro_dds bridge topic (/fmu/in/vehicle_visual_odometry). We are able to see and echo ROS2 topics published by the Pixhawk6X, so we are confident our network is set up correctly. Our pub/sub node code is attached to this message. We updated the message definitions to match the new micro_dds format, but clearly something is still not configured correctly. For what it’s worth, we really only need the VICON position feedback, the orientation data isn’t critical to us, as the onboard IMU seems to be plenty sufficient. Is there a more fleshed out guide as to how to use mocap data in place of a GPS using ROS2? Are there any critical parameters that need to be set that we are missing? Thanks!

Library Versions in Use:

PX4-Autopilot: v1.14.0 commit 8ebf47edb17390ad7e2aa01c24fc45ec3df48a3a px4_msgs: commit 979b090a1407789a7c399cbf1aadcdac233aae17 px4_ros_com: commit 482da5cc4af3209a65f76d6d6c3affbcb73f9481

Pub/Sub ROS2 Node px4_vicon_pubsub.txt

[dagar]

As a start can you share the output of listener vehicle_visual_odometry?

[bantno]

@dagar thanks for getting back to me. Running "ros2 topic echo /fmu/in/vehicle_visual_odometry" yields the following:

---
timestamp: 0
timestamp_sample: 0
pose_frame: 2
position:
- 0.014547323808073997
- -0.00824128556996584
- -0.050785161554813385
q:
- 0.003695771098136902
- 0.0016035017324611545
- -0.9999895095825195
- 0.002166511956602335
velocity_frame: 2
velocity:
- .nan
- .nan
- .nan
angular_velocity:
- .nan
- .nan
- .nan
position_variance:
- 0.0
- 0.0
- 0.0
orientation_variance:
- 0.0
- 0.0
- 0.0
velocity_variance:
- 0.0
- 0.0
- 0.0
reset_counter: 0
quality: 0
---

[dagar]

@bantno the problem (or at least the first problem) is that the timestamp and timestamp_sample isn't set. EKF2 isn't going to take that data.

I would still say the correct solution is to populate those fields before publishing, but if you try PX4 main as of ~4 days ago you can get away with leaving them 0 (they'll be populated automatically by PX4 microdds_client).

https://github.com/PX4/PX4-Autopilot/pull/21222

[bantno]

@dagar So I went ahead and updated to the most recent PX4 main. It still appears that EKF2 is rejecting the data. I get these errors in qgroundcontrol:

Additionally, what is the best way to check if I'm receiving an adequate position lock? Right now I am using the odometry section of the mavlink inspector in qgroundcontrol.

I'm relatively new to PX4 so thanks for your help with this!

[dagar]

Can you share a ulg from boot? Ideally including sections with both the vehicle moving and stationary.

SDLOG_MODE 2

In SDLOG_PROFILE enable "Estimator replay" and "Computer Vision and Avoidance".

From that log we should be able to get a better idea what's going on.

[bantno]

I've attached a log with the vehicle stationary. I'm not able to upload a log of the vehicle in motion at the moment, but will do so when I have one available.

log_0_2023-3-6-16-19-46.zip

[dagar]

Uploaded to flight review. https://logs.px4.io/plot_app?log=2efb428d-c784-4c00-a27e-9a5e279c6ec3

[dagar]

According to your vehicle_visual_odometry publication the position data is POSE_FRAME_FRD, but the EKF is aligning yaw with mag because EKF2_EV_CTRL doesn't have yaw selected. Can you try again with EKF2_EV_CTRL 11 (horizontal position, vertical position, yaw).

[bantno]

Pulled a new log with yaw selected: https://logs.px4.io/plot_app?log=fbeb12d7-fc09-454b-9087-3a9c6f213fc2

It looks like the plots for local position (x,y,z) are all accurate. Thank you so much for your help!

[agarlow]

Just wanted to mention as an addendum that @bantno and I are working on getting our autonomous indoor flight totally ironed out with this help. As soon as we have it all working and documented we will drop a link to our project write-up here. Thanks again!

[ajordan5]

@agarlow have you had success using motion capture data for position or offboard modes? We are passing motion capture info as you described above, but once we try to fly in position or altitude modes the vehicle seems to be unstable and doesn't hold its position well.

[bantno]

@ajordan5 We were able to hold position and fly to waypoints using offboard control and feedback from a Vicon motion capture system. We initially observed some jittering during takeoff and landing, which we believe was because both the takeoff mission and our offboard mission were sending setpoints simultaneously. We resolved this issue by arming the drone in manual mode and taking off using our offboard control mission.

log_58_2023-4-3-14-41-00.zip

[mengchaoheng]

Hi, you mentioned earlier, "We wrote a basic ROS2 node to take the VICON data and publish it to the desired PX4 micro_dds bridge topic (/fmu/in/vehicle_visual_odometry). We are able to see and echo ROS2 topics published by the Pixhawk6X, so we are confident our network is set up correctly." Could you please provide some references for this, I'd really like to see how these are set up? @bantno

[bantno]

@mengchaoheng Hi, @agarlow and I are planning to compile our work and publish a preliminary write up to our lab github early next week.

[Jaeyoung-Lim]

@bantno It would also benefit others greatly if you can improve the documentation on the px4 userguide

[agarlow]

Hey, for anyone who has been in this thread. @bantno and myself have stood up the initial version of the documentation site for our lab. The site is at: https://areal-gt.github.io/documentation/_build/index.html

In particular, the "Hardware Standup Guide" and "Motion-Capture Experimental Instructions" pages will hopefully be of use. They lay out our setup and PX4 parameters. The documentation page also links to repositories of ROS 2 code we have written/use for the motion-capture feedback and general UAV control.

It is our first pass at putting together documentation, so apologies in advance for whatever information is missing or unclear. Please let us know if there are any questions and we would be happy to try and answer and fix any issues. Thanks!

@mengchaoheng @ajordan5 @Jaeyoung-Lim

[Jaeyoung-Lim]

@agarlow Is there any chance we can also add this to the official px4 documentation?

[agarlow]

Definitely. We don't have any experience in adding/editing the PX4 documentation, but would be happy to help support the effort however we can.