Test missing Ethercat device

[tpoignonec]

It could be interesting to handle missing devices (e.g., not connected, broken cable, not powered, etc.) directly at the driver level.
For instance, to fill data with NaN or even trigger a fatal error and stop the controller manager.

@yguel as we discussed, here is the result with the f/t sensor using generic slave config.

1) Device not connected

Description

The sensor is not connected to the master PC when the controller manager is launched.

Results

No error or warning in the main terminal.

[ros2_control_node-3] [INFO] [1715688462.539997660] [EthercatDriver]: sensors
[ros2_control_node-3] [INFO] [1715688462.541507963] [EthercatDriver]: Got 1 modules
[ros2_control_node-3] [INFO] [1715688462.541519987] [resource_manager]: Successful initialization of hardware 'ft'
[ros2_control_node-3] [INFO] [1715688462.541610446] [resource_manager]: 'configure' hardware 'ft' 
[ros2_control_node-3] [INFO] [1715688462.541614335] [resource_manager]: Successful 'configure' of hardware 'ft'
[ros2_control_node-3] [INFO] [1715688462.541617077] [resource_manager]: 'activate' hardware 'ft' 
[ros2_control_node-3] [INFO] [1715688462.541619311] [EthercatDriver]: Starting ...please wait...
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x7010, 0x1}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x7010, 0x2}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6000, 0x1}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6000, 0x2}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6000, 0x3}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6000, 0x4}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6000, 0x5}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6000, 0x6}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6010, 0x0}
[ros2_control_node-3] {0, 0, 0x732, 0x26483052, 0x6020, 0x0}
[ros2_control_node-3] [INFO] [1715688462.541928756] [EthercatDriver]: Activated EcMaster!
[ros2_control_node-3] [INFO] [1715688463.542115778] [EthercatDriver]: updated!
[ros2_control_node-3] [INFO] [1715688463.542150385] [EthercatDriver]: System Successfully started!

But there are some errors logged by the OS :

$ sudo dmesg -c

[18059.135727] EtherCAT 0: Releasing master...
[18059.135808] EtherCAT 0: Master thread exited.
[18059.135812] EtherCAT 0: Starting EtherCAT-IDLE thread.
[18059.135989] EtherCAT 0: Released.
[18059.437111] EtherCAT WARNING: Datagram 00000000321c0157 (master-fsm) was SKIPPED 1 time.
[18061.009446] EtherCAT: Requesting master 0...
[18061.009450] EtherCAT: Successfully requested master 0.
[18061.011460] EtherCAT 0: Domain0: Logical address 0x00000000, 40 byte, expected working counter 3.
[18061.011463] EtherCAT 0:   Datagram domain0-0-main: Logical offset 0x00000000, 40 byte, type LRW.
[18061.011475] EtherCAT 0: Master thread exited.
[18061.011476] EtherCAT 0: Starting EtherCAT-OP thread.

Force / Torques equal to offset (based value is null, plus scaling and offsets):

Expected result

Configured failed.

Warning

Among the 3 described scenario, this is the most dangerous!
If the offsets are large, it might cause large robot motions at startup (e.g., under force or admittance control).

2) Device disconnected at runtime

Description

The sensor is correctly connected to the master PC when the controller manager is launched.
At runtime, the device is disconnected (cable suddenly unplugged).

Results

No error or warning in the main terminal.

But there are some errors logged by the OS :

$ sudo dmesg -c

[18872.054777] EtherCAT: Requesting master 0...
[18872.054781] EtherCAT: Successfully requested master 0.
[18872.056826] EtherCAT 0: Domain0: Logical address 0x00000000, 40 byte, expected working counter 3.
[18872.056828] EtherCAT 0:   Datagram domain0-0-main: Logical offset 0x00000000, 40 byte, type LRW.
[18872.056840] EtherCAT 0: Master thread exited.
[18872.056841] EtherCAT 0: Starting EtherCAT-OP thread.
[18872.056884] EtherCAT WARNING 0: 1 datagram UNMATCHED!
[18875.330265] EtherCAT 0: Domain 0: Working counter changed to 3/3.
[18875.358560] EtherCAT 0: Slave states on main device: OP.
[18888.858139] EtherCAT 0: Domain 0: Working counter changed to 0/3.
[18888.862718] e1000e 0000:00:1f.6 enp0s31f6: NIC Link is Down
[18888.866123] EtherCAT 0: Link state of ecm0 changed to DOWN.
[18888.882423] EtherCAT 0: 0 slave(s) responding on main device.
[18889.230361] EtherCAT WARNING 0: 1 datagram TIMED OUT!
[18889.306068] EtherCAT WARNING: Datagram 0000000076fc4abc (domain0-0-main) was SKIPPED 2 times.
[18894.275755] EtherCAT 0: Releasing master...
[18894.275900] EtherCAT 0: Master thread exited.
[18894.275905] EtherCAT 0: Starting EtherCAT-IDLE thread.
[18894.276118] EtherCAT 0: Released.

However, the last sensor value is maintained !

Expected result

AT least a big warning in the terminal.
Set all concerned data to NaN.
Optionally, trigger controller manager ERROR / FATAL ERROR.

3) Ethercat driver and controller out of sync

Description

All OK hardware-wise, but bad config.

For instance:

Device update rate is 200Hz in the ros2-control config (xacro file)

    <hardware>
            <plugin>ethercat_driver/EthercatDriver</plugin>
            <param name="master_id">0</param>
            <param name="control_frequency">200</param>
    </hardware>

But controller manager has a 125 Hz update rate.

Results

Terminal :

...
[ros2_control_node-3] Domain: State 0.
[ros2_control_node-3] Domain: WC 3.
[ros2_control_node-3] Domain: State 2.
[ros2_control_node-3] Domain: WC 0.
[ros2_control_node-3] Domain: State 0.
[ros2_control_node-3] Domain: WC 3.
[ros2_control_node-3] Domain: State 2.
[ros2_control_node-3] Domain: WC 0.
[ros2_control_node-3] Domain: State 0.
[ros2_control_node-3] Domain: WC 3.
[ros2_control_node-3] Domain: State 2.
[ros2_control_node-3] Domain: WC 0.
[ros2_control_node-3] Domain: State 0.
...

Dmeg :

...
[19581.793386] EtherCAT 0: Domain 0: 4 working counter changes - now 3/3.
[19581.834848] EtherCAT WARNING 0: 2 datagrams UNMATCHED!
[19581.921398] EtherCAT WARNING: Datagram 000000005d798117 (domain0-0-main) was SKIPPED 2 times.
[19582.801203] EtherCAT 0: Domain 0: 6 working counter changes - now 3/3.
[19582.838558] EtherCAT WARNING 0: 3 datagrams UNMATCHED!
[19582.929179] EtherCAT WARNING: Datagram 000000005d798117 (domain0-0-main) was SKIPPED 3 times.
[19583.809061] EtherCAT 0: Domain 0: 6 working counter changes - now 3/3.
[19583.838412] EtherCAT WARNING 0: 4 datagrams UNMATCHED!
[19583.937003] EtherCAT WARNING: Datagram 000000005d798117 (domain0-0-main) was SKIPPED 5 times.
[19584.816848] EtherCAT 0: Domain 0: 2 working counter changes - now 3/3.
[19584.838277] EtherCAT WARNING 0: 1 datagram UNMATCHED!
[19585.824620] EtherCAT 0: Domain 0: 4 working counter changes - now 3/3.
[19585.838031] EtherCAT WARNING 0: 3 datagrams UNMATCHED!
...

Sensor data:

Mostly OK, but sometime it freezes (stops updating) for a few seconds.
NB: very hard to reproduce this last one, maybe there are multiple causes???

Expected result

AT least a big warning in the terminal.
Optionally, trigger controller manager ERROR / FATAL ERROR.