Using Motion Capture Systems (Optitrack) for Position Estimation
-
Hello,
I have a VOXL m500, and I am planning to replace the Vision Pose data by the Motion Capture Pose Data as the pose feedback.
Therefore, I have used a ros package which is called "vrpn_client_ros" to receive the streaming data from the OptiTrack, and there is another ros package called "mavros_extras" to transfer the rostopic to the MAVLINK data and then send those data to the PX4 EKF2 estimator.
However, the estimation of the position will diverge after around 50 seconds. I put some relevant plots in here and hopefully it can provide some hints.
Any comments and suggestions are welcome.
-
We have not tried this, PX4 has a writeup though on tuning EKF for this https://docs.px4.io/v1.12/en/ros/external_position_estimation.html
-
@DarkKnightCH I am trying to do the exact same thing but the pose message being remapped to mavros/vision_pose/pose is not being relayed to the flight controller.
Did you run the MAVROS node in the companion computer of the voxl or the host computer? If you ran it in the companion computer, did you use the roskinetic docker image? If you ran it in the host computer, which port did you use to communicate between the fcu and the host computer?
Any help in the regard would be greatly appreciated.
-
Hey, I am really glad to have a discussion in here. Before I answer your question, how could you know mavros/vision_pose/pose is not being relayed to the flight controller?(I really doubt the issue that I mentioned before is caused by this reason, but I don't know how to check with that)
For your Q1, I tried both ways(companion computer and host computer[my laptop]). For Q2, Yes, and I used a roskinetic docker. For Q3, I am still working on this and I cannot give you a specific answer now, but I found a related post here udp port.
I have been stuck in this EKF2 divergence issue for a few weeks and I have found a post which describes a very very similar issueEKF2 divergence issue with me. Hopefully It can provide some hints.
I will update my progress and let me know if you have any questions！
-
@Insomnia_C For your Q3, I set fcu_url = "udp://:14551:@VOXL_IP:14556", and this is consistent with parameter "localhost_udp_port_number":14551 in VOXL setting file /etc/modalai/voxl-vision-px4.conf.
-
@DarkKnightCH Thanks for the response.
There are a couple of ways I checked the data relaying:
- There wasn't a message stream when I echoed the /mavlink/to topic.
- By setting the MAV_ODOM_LP parameter in QGC, the ODOMETRY message should echo the external pose message, but the pose there wasn't the same as the pose from the mocap. So the ODOMETRY message was giving out the pose from the VIO.
I haven't had any issues with the EKF2 divergence yet. It's probably because my mocap data isn't being relayed yet
-
@DarkKnightCH I don't have the mavros_extras package in the roskinetic docker and the normal way of getting the package doesn't work there so I am trying to run the mavros node on the host computer instead.
-
@IC Hey, have you ever get your EKF2 works?
-
@DarkKnightCH I still haven't had any issues with the EKF2 divergence yet. You're getting 0s for your x and y coordinates, which from my understanding indicates that the VIO is disabled. I was able to relay the MoCap data to the drone, using the port configuration on the host computer suggested on the post you linked.
However, now there are two external pose messages being sent to the vehicle_vision_odometry message: from the VIO and the MoCap. So the EKF2 output is not stable. I am currently looking for a way to just have the MoCap data publish to the message. I've tried changing the vio, qvio, and camera service configurations but these end up just disabling the VIO pipeline so the MoCap data doesn't get relayed to the EKF2 as well.
Were you able to somehow just get the MoCap data to publish to the vehicle_vision_odometry?
-
@IC Hi, indeed we disabled the VIO. The reason for that is we thought if we disable the VIO, then the only the Mocap external pose will be sent to the vehicle_vision_odometry.
For your question, I am not sure how to do that since previously I thought as long as I disable the VIO, then only Mocap data will be published to the vehicle_vision_odometry.
-
@IC @Chad-Sweet The Mocap_ModalAI_QGC_communication graph
Hi, the above graph is my understanding about how whole system is communicating, please feel free to point out if there is any mistake. Thanks
-
According to this post which is about checking the PX4 received external message, https://docs.px4.io/master/en/computer_vision/visual_inertial_odometry.html
I set the PX4 parameter MAV_ODOM_LP to 1. Then PX4 will stream back the received external pose as MAVLink ODOMETRY messages. As you can see in the above Figure, the left side is the Odometry which shows quadcopter's pose in NED frame, and the right side is the Mocap pose in the ENU frame, and they match pretty well. Therefore, it seems the EKF2 can receive the Mocap pose.
This is one of my logged data,
https://review.px4.io/plot_app?log=7f27f736-8ea9-4678-80cc-e97801608610
and according to the below report, it starts to show an error at 5 seconds: EV Data drop frame, and I doubt this is the reason that the EKF2 will diverge. Still need some time to figure out why the EV data will drop. Is there anyone can give me some suggestion?
Thanks
-
I tried another way to implement this. Currently my flight core is disconnected to the VOXL. I use my laptop as the companion computer, and then I add a ESP Wifi module to Flight Core TELEM1, so my laptop will get the motion capture pose by using vrpn_client_ros, the mavros will transfer the motion capture pose to mavlink data and then send to flight core.
In this case, my EKF2 will converge. The new issue is that the modalAI cannot hover at it's height on flight mode or hold mode, and the modalAI will drift. Also, the mavros terminal will show:"RTT too high for timesync: 78 ms". I am not sure if this is the reason that the modalAI will drifting in position mode.
Any suggestion?
