Check hw is_ready_for_cmds (or similar) in controllers?

[gavanderhoorn]

First: I was not sure whether to post this here or on ros-controls/ros_control. Issues can be transferred now with GH, so is ros_control is more appropriate, please move the issue.

Second: this is more a discussion-type issue, not an actual problem that I'd like to see fixed or a bug report.

---

Without going into too much (implementation) detail for now: what are the thoughts of @ros-controls on whether controllers should be able to have more insight into the state of the hardware they are controlling?

As an example: the joint_trajectory_controller will only reject goals when it's not in the running state. It could be beneficial in some cases to also reject goals whenever the hardware_interface reports an error (example: hw for which some kind of system setup or initialisation is needed before attempting to execute any new trajectories).

By exposing the state of the hw (in some hw-agnostic and generic way) to the controller, it could reject goals with error messages such as:

Can't accept new commands. Hardware reports it is not ready for new commands.

(ideally with a new and more descriptive error_code value in control_msgs/FollowJointTrajectory than INVALID_GOAL)

For controllers that don't accept goals (such as those only subscribing to command topics), they could ignore incoming commands.

This could be done by implementing a virtual resource that controllers could claim which encapsulates hw status and they could check periodically. By making it an optional resource, hardware_interfaces that don't support the "hw status interface" would not get the benefit of the additional check.

[gavanderhoorn]

An alternative could be for the program wrapping/hosting the hardware_interface and controller_manager to monitor the state of the hardware_interface and then to stop/start a (preconfigured) set of controllers.

This could even be done using a stand-alone node (that essentially maps something like hw_state -> controller runtime state).

As at least the management of controller execution status is a coordination level type activity, that could make sense.

It would not allow the controller to send back more appropriate rejection messages though.

[bmagyar]

We had a similar experience when experimenting with mode switching motor modules. If the module takes more than one update loop to switch, it will currently get new commands which it won't be able to fulfill.

Indeed this feels like something that could be handled at the interface level. Should this be the responsibility of each controller or be directly enforced by the controller manager?

[gavanderhoorn]

I've implemented a poc of my first alternative (hardware_interface hosting node manages state of controllers based on hw state).

It works quite ok, although I've only tested it under rather artificial conditions so far (ie: stationary robot, forcing error states which may not occur 'naturally').

The controllers not returning a more appropriate rejection message, but simply "controller is not running", can get really confusing. I've had a few times I was really surprised nothing was happening and why the controller was not running, until I remembered that the controller was stopped by the robot being in an error state. As I did not add auto-restart and did not have any higher-level coordination (which could've restarted the controller for me), not seeing something like "hardware reports it's not ready" caused me to look at other things a few times.

Without auto-restart I also had to take care of making sure the hardware_interface did not resume sending old commanded positions after the runtime state of the hw had returned to normal -- at least with the hardware_interface I am working with that quickly became a problem. Controllers are supposed to sample the current state whenever they are started, but I could not find a similar convention/requirement for hardware_interfaces.

@bmagyar wrote:

We had a similar experience when experimenting with mode switching motor modules. If the module takes more than one update loop to switch, it will currently get new commands which it won't be able to fulfill.

Yes, that would be similar to what I have here: commands will either be ignored (in the best case), or lead to errors in the worst. Not calling write(..) whenever the underlying system is not in an OK state was what I ended up doing (as it's unclear at this point what the hw would do when enabled right in the middle of incoming commands).

Indeed this feels like something that could be handled at the interface level. Should this be the responsibility of each controller or be directly enforced by the controller manager?

I first started out adding an interface, handle and associated code for a HardwareRunState resource (or something like that, with all the names already taken it gets difficult to find something descriptive that is unused). Controllers would then have to claim that resource in addition to what they already claimed. That quickly became messy (and I don't believe it'd scale very well).

Extending the controller manager (with perhaps a "hw_runstate_callback") could be an option, although it would seem like it would violate separation of concerns: the controller manager -- responsible for managing controllers -- then also becomes responsible for checking the state of the hw.

As this is really a coordination type activity, I believe it would make more sense to keep it out of the rest of the infrastructure, and add something that specifically ties all this together. For my poc I ended up adding it to the "driver" node.

[gavanderhoorn]

The controller_stopper package/node part of the new UR driver would seem to be an example of the external coordination type approach I described above.