NOTE: I am cross-posting this from my thread on the ArduSub forums per a suggestion to elicit some from the ArduPilot forums. Furthermore, as indicated above, I am not using ArduCopter, but it seemed like the community most likely to have experienced the problems that I am observing would be the ArduCopter community (there also isn’t an ArduSub tag).
I have recently been attempting to integrate a Qualisys motion capture (MoCap) system as an external navigation source using MAVROS and the BlueROV2; however, I am running into some difficulties getting the fused sensor data (LOCAL_POSITION_NED) from the EKF. The problem that I am observing is that, when I launch my localization framework while QGC is open, I do not receive any LOCAL_POSITION_NED messages. I was initially under the impression that QGC was adjusting the publishing rates when it gets loaded, but I integrate a solution applied by the Orca4 project here in which the message intervals are periodically set should a second GCS like GQC be launched. When I launch my framework without QGC open, I start to get these messages (albeit with the log IMU0 stopped aiding in QGC and MAVROS). Below are the current specifications of my configuration:
- ArduSub 4.1.0 Stable is running inside a Docker container on a Rasberry Pi 4 with Raspberry Pi OS Bullseye
- The BlueROV2 is connected to a network switch on the surface (~3-7ms latency) which facilitates connection between my laptop and the Qualisys MoCap system
- I am using ROS 2 with MAVROS as the primary GCS and QGC as the secondary. I prefer this setup because it makes it easier for my to operate the vehicle in manual mode should I need to take over quickly or reposition the vehicle during some tests after currents have moved it
- I am testing in a pool-like environment
- My localization interface is sending pose estimates to the FCU using the
/mavros/vision_pose/posetopic. I have also tried the/mavros/vision_pose/pose_covtopic (I describe this a bit more below).
Here are some of the debugging steps that I have already tried and their outcomes:
- Everything that I have implemented works quite well in my ROS 2 Gazebo SITL environment; however, problems start to pop up in the hardware integration (sim2real issues are basically a fact of life
). - When I launch MAVROS, I am able to successfully connect to the BlueROV and stream messages from the Qualisys interface. I am able to establish a connection both from the topside (my laptop) and from a separate Docker container on the BlueROV RPi itself. MAVROS is also able to establish a connection with QGC in both scenarios. When using the topside interface, I use the same connection string that previous projects have applied successfully here.
- I have verified that the
VISION_POSITION_ESTIMATEmessage is being received using the BlueOS MAVLink inspector - I tried using the DVL BlueOS extension with our DVL (without my localization framework launched) which sends velocity and position estimate messages to the EKF using
VISION_POSITION_ESTIMATEandVISION_SPEED_ESTIMATEmessages. This works, and I am able to both receive theLOCAL_POSITION_NEDmessages over MAVROS and set the message interval, as previously mentioned, all with QGC open. The reason that I am not using this instead of the MoCap systems is that, while this provides good velocity estimates, the position estimates are prone to some serious drift. - I have verified that my external state interface is working properly. I have applied an LWMA filter to the MoCap data and also filter out any invalid NaN data. This was in an attempt to handle the
IMU0 stopped aidingmessage and to account for the possibility that the EKF is rejecting my measurements. I also checked theLOCAL_POSITION_NEDmessages that are published when my framework is running with QGC closed and have confirmed that these messages are accurate. - I set the EKF origin using the
global_positionMAVROS plugin. I have confirmed that the location is set when I open QGC and see the ROV icon at the specified location. - I set the ArduSub parameter values to the same values as those applied by the DVL driver in BlueOS which can be seen here. I don’t have the parameters file on hand at the moment but can retrieve that on Monday if it is useful.
- I was originally publishing the MoCap data at a rate of 100 Hz, but observed some slowdown in the system response at this frequency. I originally hypothesized that the CPU was operating at its limits, but saw that the CPU usage was only at about ~50%. I dropped the MoCap streaming rate down to about 50 Hz and the response rate improved, but the main issue persisted.
- I am using ArduSub 4.1.0 Stable. I was originally using the Dev version, but observed some compatibility issues with the latest version of QGC
- Finally, I tried integrating an estimated covariance measurement (I just set it to a low 0.1 value because the MoCap has ~2mm accuracy at the time of calibration)
Given these things, I believe that the issue is on my end; however, I am a bit unsure where to go from here and was hoping to look for some feedback regarding some additional debugging steps that might be useful to apply or to see if there are solutions that folks have already found. Below are some of the relevant links within my implementation:
- Here is where I process the published MoCap data and send it to the FCU
- Here is my interface for the MoCap system
- This is where I set the message interval rate
- This is where I set the EKF origin
- Here is my MAVROS configuration file. The current connection string is the simulation connection string.
EDIT: I forgot to mention that I don’t have the logs with me at the moment, but I will retrieve those and share them here on Monday when I can access my lab.