Roadmap for "full time" ROV control stack

[amarburg]

I'm considering the question of next steps for a "runs all the time" control stack for the Blue+BlueROV. "Runs all the time" in this case means there is always a control strategy / loop active, rather than a tear-down-and-restart model. This also means rapid switching from more-complex to less-complex control strategies for safety (e.g. as an e-stop). Luckily with a neutral ROV, the safest control strategy, stopping the motors, is almost always available.

As a simplification, I propose four levels of control.

1. "Sophisticated" ROS2-based control. This could be e.g. move2 / nav2-compatible trajectory following, a novel whole-body controller, etc. The interesting stuff
2. Setpoint-based control on each axis independently (e.g. XY position hold, depth hold, heading hold) with an interface for teleoperation e.g. one click of joystick moves heading setpoint 5 deg to CW, heading hold loop reacts and ROV turns, then stops and continues to hold that new heading)
3. Body frame velocity control. Interface for teleoperation where e.g. joystick commands vehicle velocities in different axes
4. Body frame thrust control. "raw" interface where body-frame commands e.g. "100% forward thrust" is allocated to thrusters via TAM

Of those, 4 is the only one that does not require a feedback loop. 4 is essentially "raw ROV control", while 3 / 2 are reasonable expectations for a mature human-in-the-loop ROV controller. There may be mixing of 2 and 3 e.g. velocity control in X-Y while holding heading and depth.

The goal would be to switch between these modes seamlessly in realtime. e.g.:

1. launch vehicle, drive to desired location in 3
2. then enable holds per 2. Vehicle holds station.
3. "nudge" vehicle into desired location with teleop
4. "launch" mission (behavior tree?) which switches into 4
5. notice issue, press e-stop. vehicle drops to 3 and stabilizes
6. (etc)

So the question is how to apportion functions 2-4 between the ros2_control space and ardupilot. It can be done entirely in ros2_control; I think this ros2 control gsoc proposal speaks to both the need and path forward -- essentially a "conductor" which switches various controller sets on and off as appropriate.

However I suspect ardupilot contains code to do some part of it as well. Should we leverage this (the current OverrideRC approach bypasses it entirely) or work to actively move away from ardupilot / ardusub functionality?

[evan-palmer]

This is a nice proposal, and I would be interested in helping push this forward. Your question regarding ros2_control and ArduPilot is more difficult to answer, so I'm glad that you've made this discussion.

The first main point that I would like to raise is that I have been moving Blue towards a more generalized, hardware-agnostic platform so that it's easier to integrate custom hardware and to transfer work across systems. In a perfect world, this would allow us to use some of the same code on the BlueROV as you might on the Raven. This doesn't answer your question, but I think provides some context for the vision that I have.

Regarding moving away from ArduPilot, the issues that I run into with using ArduPilot stem from the fact that ArduPilot was built to support aerial hobbyists and other folks that need a flight stack--not a research platform. This leads to a scenario where the baseline functionality that ArduPilot provides is very good, but lacks the flexibility that some researchers (certainly not all) need to develop and deploy new algorithms, hardware, etc. For instance, the EKF that ArduPilot provides out-of-the-box is excellent, but integrating multiple new sensor modalities is far from trivial. Additionally, everything that we do has to go through MAVROS or MAVSDK, which adds complexity.

Of course, we can often replace pieces that don't work for us with a custom solution. In my previous research with aerial platforms, I found that this was an acceptable and preferred solution. However, in my current work with with underwater systems, I have found that I am replacing much more with custom solutions than I was previously. With Blue Robotics releasing navigator-lib, I have been considering the switch away from ArduSub to a ROS 2 platform more seriously. However, I'd like to note that Rhys has been putting a lot of effort into improving ROS 2 support in ArduPilot (not ROS 1). This work is far from done, but there is progress. You can read more about what's presently available in the AP_DDS library.

There are plenty of downsides to moving away from ArduSub. Beyond the development time, here are a couple limitations that I've thought about:

• The flight mode interface made available in ArduPilot is very responsive and might degrade when using ros2_control (not sure, haven't tried controller switching yet)
• If you aren't using thruster-level control, you would need to re-implement some of the safety features that ArduPilot implements, such as pre-arm checks.
• You wouldn't be able to use QGC or Mission Planner
• ArduPilot has a good flight logging implementation and tools to evaluate those flight logs
• We lose some of the calibration features that ArduPilot has
• etc.

Altogether, here is what your proposal would look like if you were to leverage ArduPilot:

1. There is some support for Nav2 in ArduPilot. A 2023 GSoC student spent last summer working to integrate the two. The write-up is available in the ArduPilot forums here and the code is here. Implementing your own custom controller, however, would look something more attune to the OverrideRC approach currently used.
2. This would probably involve using GUIDED mode and writing a ros2_control controller on top of it. There are also some position hold modes in ArduPilot that do something similar (and might actually do what you've described). The downside here is that the ArduPilot position hold modes require a position lock to switch into them, which involves getting the EKF to play nicely with your additional sensors.
3. If you want teleoperation, body-velocity control is implemented in MANUAL mode. Body-velocity setpoint control is also partially supported in GUIDED mode (supports linear velocity and yaw rate control).
4. I don't know of any support for this in ArduPilot.

If we were to do a ros2_control only approach here is what that would look like:

1. We would integrate Nav2 on top of auv_controllers. MoveIt2 support could also be built on top of this once I move the whole-body controllers in Angler to that package.
2. Similar to adding a ros2_control controller described above
3. Technically this exists with the ISMC. I would like to implement a model-free controller like PID to provide a simpler default, though.
4. Exists in auv_controllers and the ardusub_driver

Switching between these would probably be involve a state machine or behavior tree implementation like you've described.

This post has grown quite long now, so I'll finish with saying that I don't plan to completely eliminate ArduSub support in Blue, because my goal for Blue is still to remain hardware agnostic. What I would likely recommend for us moving forward is that we start by keeping things simple and iterate. If there is a feature that you need that already exists in ArduPilot, I think it makes sense to leverage that. However, I would also ask you what you hope to get out of using Blue? If you see this as primarily a tool for integrating with the BlueROV, then I would again say that it makes sense to take advantage of ArduPilot where you can. However, if you want to be able to leverage the work that you do here across multiple platforms, then we may want to consider spending the time to move towards the ROS 2 implementation.

[amarburg]

Thanks for the detailed response. Per your last question, our current project focuses primarily on mission-level scripting/autonomy, rather than control. So I do want to bring in move2/nav2 compatibility (pytrees, etc). However I'm agnostic as whether the middle of the stack (the PID loops, as it were), are provided by ros2 control or ardupilot. I can certainly see ros2 as a better environment for developing more sophisticated controllers (e.g. whole body control), that needs to remain an option.

The other part is realizing ROS2-control may be a small fraction of the duration any given dive, with the rest spent with hardware checkout, maneuvering into position, etc. Cutting ardupilot out would mean recreating many of the GUI elements (QGC or Cockpit, leak detector warning, low battery warnings, etc), and the most basic/reliable teleoperation modes.

Re Raven, it's an interesting comparison. I would like to chart a path where we can leverage Blue on Raven (if nothing else, the Raven stack is currently ROS1 so I need to be thinking about a migration path to ROS2). Raven uses Greensea's software. From an end user perspective, we wanted a similar experience as described above. Greensea's GUI and nav/EKF run at all times; the system then has a "big mode switch" which allows switching between ROS- and Greensea-in-control modes. One key difference is that our current ROS1 controller doesn't leverage any part of Greensea's control stack --- instead we re-implemented the hardware thruster driver with a big "am I listening to ROS or Greensea" switch. This required building a wholely independent, parallel implementation of the PID loops, luckily it was just PID and easy to reimplement. There were reasons.

All that said, I think my proposed solution would be as follows:

• a "big switch" which allows rapid online switching between ROS and QGC/Cockpit control. This would involve:
  • A switch on who Ardupilot is listening to (could be as simple as getting mavlink to not send RcOverrideIn?)
  • A switch on where joystick input is routed (this is optional)
  • A service (in ROS2) for controlling these switches
• Implement desired control modes in ROS2_control assuming mode switching in ros2_control is responsive

This is not too dissimilar to the relationship between ROS1 and Greensea on Raven.

All things being equal, re-implementing a e.g. heading hold PID loop in ROS should not be difficult, I think it's worth doing and does not require engaging with the spaghetti of modes provided by Ardupilot. But this would allow seamless switching between BlueROV-as-we-know-it control and ROS control.

[evan-palmer]

The other part is realizing ROS2-control may be a small fraction of the duration any given dive, with the rest spent with hardware checkout, maneuvering into position, etc. Cutting ardupilot out would mean recreating many of the GUI elements (QGC or Cockpit, leak detector warning, low battery warnings, etc), and the most basic/reliable teleoperation modes.

This is a good point. Cockpit seems to be quite flexible with its extension, so it might be possible to replace the ArduPilot endpoints with ROS 2 endpoints, but I don't know that for certain. Regardless, I agree with you--we would be throwing away a lot of well-tested & supported work.

All that said, I think my proposed solution would be as follows:

• a "big switch" which allows rapid online switching between ROS and QGC/Cockpit control. This would involve:
  • A switch on who Ardupilot is listening to (could be as simple as getting mavlink to not send RcOverrideIn?)
  • A switch on where joystick input is routed (this is optional)
  • A service (in ROS2) for controlling these switches
• Implement desired control modes in ROS2_control assuming mode switching in ros2_control is responsive

This makes sense to me. I think the first step would be to do some tests with ros2_control to see how fast controller switching is. The MoveIt2 Controller Manager might be an easy way to explore this.

Ironically, the first point in your proposed solution is quite similar to some functionality that I have since gotten rid of. The implementation worked fine, but wasn't as clean of a solution as I would have liked.

One other limitation that I forgot to mention above regarding sending RC override messages for control is that it's hard to test how responsive the thrusters are to ros2_control commands (i.e., how much latency does using ArduPilot + MAVROS add). I don't think that we need to focus on getting real-time control rates in the scope of this proposal, but we should keep an eye on this. For my own work, I'm planning to add a real-time (ideally ~500 Hz - 1 kHz) joint-level impedance controller to track whole-body control commands. If the ArduSub latency is significant, I may still be forced to look into using navigator-lib with an RTOS.

FYI, Daniel has mostly stopped developing pytrees, so I have been looking into switching to BehaviorTree.CPP as a replacement.