BioscaraPanel: Arm state not updated when stalled

[sbstorz]

To reproduce
Activate the hardware and trajectory controller, execute a trajectory and during execution stall the arm.

The hardware state goes to UNCONFIGURED but this change is not updated in the GUI.

Output of ros2 control list_hardware_components :

Hardware Component 1
        name: bioscara_gripper_128
        type: system
        plugin name: bioscara_hardware_interfaces/BioscaraGripperHardwareInterface
        state: id=3 label=active
        read/write rate: 50 Hz
        is_async: False
        command interfaces
                gripper/position [available] [claimed]
Hardware Component 2
        name: bioscara_arm
        type: system
        plugin name: bioscara_hardware_interfaces/BioscaraArmHardwareInterface
        state: id=1 label=unconfigured
        read/write rate: 50 Hz
        is_async: False
        command interfaces
                j4/home [unavailable] [unclaimed]
                j1/home [unavailable] [unclaimed]
                j1/velocity [unavailable] [unclaimed]
                j3/position [unavailable] [unclaimed]
                j2/position [unavailable] [unclaimed]
                j1/position [unavailable] [unclaimed]
                j3/home [unavailable] [unclaimed]
                j2/home [unavailable] [unclaimed]
                j2/velocity [unavailable] [unclaimed]
                j3/velocity [unavailable] [unclaimed]
                j4/position [unavailable] [unclaimed]
                j4/velocity [unavailable] [unclaimed]

The GUI panel:

[sbstorz]

The problem is that the controller manager does not send a new message and hence the GUI is not updated.
The controller states are also not updated.

This is the controller managers log:

[ros2_control_node-1] [INFO] [1765540114.485331005] [velocity_joint_trajectory_controller]: Accepted new action goal
[ros2_control_node-1] [ERROR] [1765540118.060414741] [controller_manager.hardware_component.system.bioscara_arm]: Failed to set j1/velocity of joint 'j1'. Reason: stalled
[ros2_control_node-1] [INFO] [1765540118.066492224] [controller_manager.hardware_component.system.bioscara_arm]: Previous State: active
[ros2_control_node-1] [INFO] [1765540118.066648519] [controller_manager.hardware_component.system.bioscara_arm]: Deactivating ...please wait...
[ros2_control_node-1] [INFO] [1765540118.077264720] [controller_manager.hardware_component.system.bioscara_arm]: Successfully deactivated!
[ros2_control_node-1] [INFO] [1765540118.077432904] [controller_manager.hardware_component.system.bioscara_arm]: Cleaning up ...please wait...
[ros2_control_node-1] [INFO] [1765540118.079266758] [controller_manager.hardware_component.system.bioscara_arm]: Successfully cleaned up!
[ros2_control_node-1] [ERROR] [1765540118.083311203] [controller_manager]: Deactivating following hardware components as their write cycle resulted in an error: [ bioscara_arm ]
[ros2_control_node-1] [ERROR] [1765540118.083488776] [controller_manager]: Deactivating following controllers as their hardware components write cycle resulted in an error: [ joint_state_broadcaster homing_controller velocity_joint_trajectory_controller ]
[ros2_control_node-1] [ERROR] [1765540118.083707626] [controller_manager]: Not acceptable command interfaces combination: 
[ros2_control_node-1] Start interfaces: 
[ros2_control_node-1] [
[ros2_control_node-1] ]
[ros2_control_node-1] Stop interfaces: 
[ros2_control_node-1] [
[ros2_control_node-1]   j1/home
[ros2_control_node-1]   j2/home
[ros2_control_node-1]   j3/home
[ros2_control_node-1]   j4/home
[ros2_control_node-1]   j1/velocity
[ros2_control_node-1]   j2/velocity
[ros2_control_node-1]   j3/velocity
[ros2_control_node-1]   j4/velocity
[ros2_control_node-1] ]
[ros2_control_node-1] Not available: 
[ros2_control_node-1] [
[ros2_control_node-1]  j1/home
[ros2_control_node-1]  j2/home
[ros2_control_node-1]  j3/home
[ros2_control_node-1]  j4/home
[ros2_control_node-1]  j1/velocity
[ros2_control_node-1]  j2/velocity
[ros2_control_node-1]  j3/velocity
[ros2_control_node-1]  j4/velocity
[ros2_control_node-1] ]
[ros2_control_node-1] 
[ros2_control_node-1] [ERROR] [1765540118.083862698] [controller_manager]: Error while attempting mode switch when deactivating controllers in write cycle!

[sbstorz]

There seems to be an issue in ros2_control controller manager that if ERROR is returned from a read() or write() call, the controller manager tries to stop command interfaces which are no longer available (see previous comment). Also then any controller or hardware state change are not published.

A temporary fix is to return DEACTIVATE on any error in write(). Then the controller manager only deactivates the trajectory controller and hardware component. The on_error() method is not called:

[ros2_control_node-1] [INFO] [1765545876.232235774] [velocity_joint_trajectory_controller]: Accepted new action goal
[ros2_control_node-1] [ERROR] [1765545879.782599809] [controller_manager.hardware_component.system.bioscara_arm]: Failed to set j1/velocity of joint 'j1'. Reason: stalled
[ros2_control_node-1] [INFO] [1765545879.782946101] [resource_manager]: 'deactivate' hardware 'bioscara_arm' 
[ros2_control_node-1] [INFO] [1765545879.783035488] [controller_manager.hardware_component.system.bioscara_arm]: Deactivating ...please wait...
[ros2_control_node-1] [INFO] [1765545879.793628350] [controller_manager.hardware_component.system.bioscara_arm]: Successfully deactivated!
[ros2_control_node-1] [INFO] [1765545879.793845180] [resource_manager]: Successful 'deactivate' of hardware 'bioscara_arm'
[ros2_control_node-1] [ERROR] [1765545879.794645910] [controller_manager]: Deactivating controllers [velocity_joint_trajectory_controller ] as their command interfaces are tied to DEACTIVATEing hardware components

The controller switch does not return an error and the change is published.

For errors during read() the controller manager prints:

[ros2_control_node-1] [WARN] [1765546184.857469038] [controller_manager]: DEACTIVATE returned from read cycle is treated the same as ERROR.

Hence the on_error() method is called and the original issues occur