Mode switch timed out.

[Vidicon]

Describe the bug

We were trying to get a ENGEL HBI3760 working with the 402 driver, but we kept hanging on the
[canopen_402_driver]: Mode switch timed out.

After looking through the logs, we found that the halt bit is set when it tries to change modes of operation, this does not work as that's not allowed in this motor:

In the datasheet on page 49:

Notice:
Changing the mode of operation is not allowed, when the HALT-bit (Bit 8 of Controlword 6040h, chapter 5.3.1) is set!

as a quick test, when we change:

ros2_canopen/canopen_402_driver/src/motor.cpp

Line 272 in 7687f32

if (okay)

to
if (okay || true)

the driver starts working,

is there a way to do fix this without changing the code, or what would be a better solution for this.

Setup:

• Device: Dell Laptop with peak can-interface.
• OS: Ubuntu 22.04
• ROS-Distro: Humble
• Branch/Commit: Humble