We have a BlueROV2 with a Water Linked DVL A50 that is behaving very strangely in guided mode.
For example: Experiment: We place a goal right in front of the ROV and put it in guided mode via QGroundControl. Result: Sometimes the ROV rotates a seemingly random (and often large ) number of degrees before moving straight in the “wrong direction”.
In QGroundControl the ROV strafes a bit and reaches the goal. In reality the ROV moves straight after rotating, (without strafing) and ends up in the wrong location.
Other times (especially after a reset of both ardusub and the dvl) it seems to work as it should for a while.
Hi @Sam -
Are you operating the system in a tank or pool? This environment can cause issues with multipath reflections that can cause drift or strange behavior.
Have you tried a simple waypoint mission with Auto mode?
We have observed the same navigation behaviour in a smaller round tank, in a reasonably large and pretty deep swimming pool and outdoors in a bay. POSITION_HOLD mode has always worked fine as far as I remember. If it was the environment, you would expect issues in that mode right?
Something we did see was that the yaw estimate was drifting quite a bit when the ROV was standing still in POSITION_HOLD mode without compass enabled. Don’t know if that is relevant for the main navigation issue that I’m trying to solve.
We tried waypoint missions using Auto mode, and that has worked without issues so far.
In the orca4 based gazebo simulation we use, we don’t have the issue with guided mode. We just put ground truth position from gazebo into the ardusub filter instead of using delta positions like the real dvl though.
It seems like we can pretty reliably recreate the odd behavior by starting mavros and using the mavros/set_mode service to put the ROV in guided mode.
The odd behavior is then the same when we try to use guided mode through mavros and QGroundControl after that.
A reset of ardusub seems to fix it. Restoring parameters to what they were before we started mavros does not fix the issue.
Another thing to note is that going to guided mode in QGroundControl normally rotates the ROV to facing what it thinks is north. When we have the odd behavior it doesn’t rotate and just stays at the current angle.
Do you happen to know if anyone has gotten some kind of navigation to work with a real BlueROV2 through MAVROS @clyde?
We have been trying to get something to work based on orca4 but are having some issues, as described above.
I suspect that the local (relative, odom) frame is rotated wrt the global (absolute, map) frame.
In ROS the two frames (odom, map) are explicitly managed and there is a transform between them.
I am still learning ArduSub, but AFAICT there is really only 1 global frame. I am pretty sure that QGC is sending targets in the global NED frame. Check the tlog to verify this.
In your first case, if you have a DVL w/o a compass, then I suspect that the yaw error has built up and the sub is rotated relative to the global frame, so QGC targets will cause problems.
For the case involving mavros, there is probably something similar going on, although exactly how this works might mean digging into the mavros code to see how they map from ROS frames to MAVLink frames.
If you can get the compass(es) to work in your environment that should help. Or I suppose you could add April tags to the environment to provide some sort of yaw measurement.
The global and local frames being rotated wrt each other seems like plausible reason to me, I’ll check it out.
I don’t understand why this would start to happen both in ROS2 and QgroundControl after just running: ros2 service call /mavros/set_mode "custom_mode: 'GUIDED when everything seems to work like it should if I only use QGroundControl.
Is this a simulation? I assume that QGC is on the left, is that Gazebo on the right? What are the coordinate frames?
In simulation the compass should provide good readings, and so the EKF should have a good heading. It should be possible to trace through the logs (BIN, tlog) to isolate the bug. If you post logs I will take a look.
Real life is trickier. I’ve found that the compasses are basically useless in commercial buildings with reinforced concrete floors. Perhaps the same is affecting the pool tests? The logs might provide some clues.
Edit: double-check the coordinate frames… IIRC, in ROS yaw=0 means east (ENU), while in ArduSub yaw=0 means north (NED).
Yesterday I found out that deactivating the sys_satus mavros plugin consistently makes the bad behavior go away in simulation. This makes me think that this is not a straight forward coordinate system bug.
We are using Ardusub release 4.1.2 for the sitl and we don’t seem to have this problem while using the master branch sitl.
I see that you are using just the DVL for yaw, no compass. The EFK should be happy estimating yaw from the DVL and gyros, but this will be relative to the initial yaw value – which seems to be 0 (north).
It looks like the estimated yaw (ATTITUDE.yaw) is ~180 degrees off from the ground truth (SIMSTATE.yaw):
In this state a global “go here” command (a MISSION_ITEM MAVLink message with frame==3) will be interpreted incorrectly.
I don’t know why this rotation occurred. I did some tests myself in SITL with a Python script to inject VISION_POSITION_DELTA messages, and I can’t repro this. Can you generate a BIN file with LOG_DISARMED=1? That might provide a few more clues.
One “fix” is to enable the SITL compass and use it in the EKF. This should work great in SITL, and it may help you flush out other bugs.
I had some issues trying to get a BIN file out of QGroundControl. (No logs are showing up when I click on Refresh under Log Download)
Would another .tlog file with LOG_DISARMED=1 help any?
I noticed that the visualization of the yaw angle is different in QGC from UAV Log Viewer for the same .tlog file. The yaw of the ROV in UAV Log Viewer seems to correspond to how the robot is actually moving in gazebo/real life, but the yaw in QGC is off by quite a lot.
My issue at this point is not really that ROV moves to the wrong long/lat position because there is an offset in yaw from ground truth. The problem is that if I place a “go here” target right in front of it, it turns and moves forward in a different direction. This happens both for local and global “go here” targets when the sys_status and the custom dvl mavros plugins are loaded.
In SITL, the dataflash (BIN) logs are written to the ‘logs’ directory. The exact location will depend on how you are starting ardusub. E.g., if you are in the ‘ardupilot’ directory and run SITL using ./Tools/autotest/sim_vehicle.py then the dataflash logs will be in ardupilot/logs. You can open dataflash logs in UAV Log Viewer, but you’ll see dataflash tables, not MAVLink messages.
Interesting. It looks like UAV Log Viewer is reading a different message than QGC. Perhaps the UAV Log Viewer is reading the GPS_RAW_INT message? (You are simulating a GPS sensor, but not using the GPS sensor in the EKF.)
I agree, this is the key problem. It appears that the EKF origin is set while the sub is pointing south, not north. The dataflash log will provide the answers.
