Auto switching of control chains

[JayHerpin]

Is your feature request related to a problem? Please describe.

I work with surface vessels. In this domain, it is common for the platform to have multiple modes of control. Often times, different modes of control will use different sets of actuators based on what is desired.

For example, imagine a boat that has an engine, a rudder, and a bow thruster. While transiting between locations, the higher level system may wish to specify a particular heading and speed for the vessel, which should be achieved using the engine and rudder only.

Now imagine that this vessel is on station and wants to do some sort of dynamic positioning. The higher level system might specify a particular orientation and position, and this would be achieved by using a combination of the bow thruster, engine, and rudder.

This is just one example, but the idea being that the specific controller or control chain that is active can automatically switch based on something like which inputs are being received.

Presuming there is some sort of priority scheme for arbitrating which controller/chain is active, in addition to supporting arbitration between mutliple types of control inputs, this could also arbitrate between multiple instances of the same input type (eg. it could serve as a sort of builtin version of twist_mux).

Describe the solution you'd like

I haven't looked at the fallback control mechanism, but I can imagine there would be some overlap with that. I'll provide a straw man here, but there are some details I am iffy about, so please feel free to redirect me.

We define a state that is somewhere in between ACTIVE and INACTIVE; let's call it STANDBY. I am proposing it as an explicit separate state, but this could also be a substate of ACTIVE. A controller which is in STANDBY may be automatically activated by the control_manager.

Now, imagine that we have something called an InputController, which is, by definition, non-chainable (maybe this is just a Controller). These controllers provide a mechanism to say if they "want to be active". This could be as simple a method like "isReady" on the controller object.

Further, assume that the InputControllers also have an explicit priority associated with them, and have their dependencies listed (their control chain).

On each control loop, each ACTIVE or STANDBY input controller should be checked to see if it is "Ready". If the highest priority input controller is not one that is currently activated, then the control chains should be switched to reflect that.

Describe alternatives you've considered

I've actually prototyped this out previously as a wrapper around the controller_manager in a custom control node executable, however, the problem with this was that it didn't introduce a new state for the controllers, so it basically circumvented the ability to explicictly bring up/down controllers using the service API.

I've considered that this could be done as an external layer, where you have something which is receiving all the inputs, then based on that sending explicit commands to the controller manager to do the switching of chains and then forwarding the inputs to the control manager. However, that solution seems like it would add considerably more latency, and given that I expect the primary mechanism used to switch between control modes would be message timeouts, the added latency (particularly when activating a new chain failed) would be a non-starter.

Additional context

If this is a feature that seems desirable and you can provide me with some direction on how to implement it properly, I would be happy to work on this myself.

[christophfroehlich]

In one of my projects, I also have to switch controllers depending on the current situation. But anyways, I have a behavior execution engine on a higher level, and I just added the necessary service calls there. Could you highlight, why this is not feasible in your usecase, i.e., why the switching has to be done automatically?

[JayHerpin]

In one of my projects, I also have to switch controllers depending on the current situation. But anyways, I have a behavior execution engine on a higher level, and I just added the necessary service calls there. Could you highlight, why this is not feasible in your usecase, i.e., why the switching has to be done automatically?

Well, for one, it would require funneling all the potential input sources through a mediator in order to do that switching. One of the goals of this change would be to be able to leverage it to do control arbitration when switching between distinct external controllers.

For example, lets say I have some sort of mission system which drives the vessel, and then separately I have a reactive collision avoidance system. Doing arbitration externally would require funneling those systems through a single point which could then utilize the service API to reconfigure the control manager. Adding that additional hop would add additional latency by itself, but additionally, having to wait for the service calls to hit the bus, be handled, and then responded to would increase the latency of control switching even futher. This gets even worse if thse mode switches can happen somewhat frequently.

Another good example of this would be if we want to do a powered stop. For boats, these can be special maneuvers. For example, if you have two directional thrusters, pointing them each in opposite directions parallel to direction of travel will stop the boat faster than cutting thrust, and oviously, going full reverse would stop you faster still. These maneuvers are typically used in an emergency situation where we would not necessarily want to incure additional delays going through a mediator.

Another issues that I've had to deal with in some previous projects was dealing with discontinuities of control, particularly when there were human passengers on boat. That canonical example of this was if you had a behavior based mission system when switching between two behaviors. For example, you arrive at a final transit waypoint and then want to move into a search pattern or something like that. When switch between those behaviors, if there is a short period of time between the two when neither behavior is in control then this can manifest itself in throttle dipping before the second behavior takes over. Those small sudden fluctations tended to be unpleasant for riders and can make them sea sick.

I'd probably argue this behavior belongs close to the control loop for the same reasons that the fallback controller functionality belongs there.

[christophfroehlich]

Just a comment here: Switching of controllers should happen in the same cycle, and no discontinuity should happen (maybe considering --activate-as-group depending on your chains).

[JayHerpin]

The discontinuity is more from the external entity. For example, node A is sending on topic "foo", and then at some point it finishes whatever its doing and node B starts sending on topic "bar". Simulataneously we need to change controllers so that the "bar" topic can be the new input to the new chain. Its coordinating those things that leads to the discontinuity.

As an aside, when I prototyped this out before, I did do it as a wrapper around the controller manager but in process using the methods to change chains. This was a fine way to go about it, but I wasn't totally happy about there being the service interface still exposed and the fact that that would fight with my internal switching.

If the controller manager implementation was separated from its service interface, I'd probably be ok just leaving my thing as a wrapper around the contoller manager.