Mode Switch timeout is too short for some drives

[aatb-ch]

I have successfully setup canopen_motor_node on a Festo CMMP-AS drive, took me a while to get all parameters corrects by following various Q&As and issues, but the last nagging thing turned out to be a vendor-specific behavior:

When switching to Profile Position mode, no problem, it instantly switches mode, but I need Interpolated Position with a Joint Trajectory Controller, and it seemingly has been starting or crashing randomly. I read through all the Festo docs and once all params are set correctly and mode switch 7 is asked the drive returns mode -1 while it goes through a synchronization phase, after which it will return mode 7.

It just so happens that this phase takes between 3 to 6 seconds, randomly varying at each mode switch. The timeout for mode switching is hardcoded at 5s, I recompiled from source by setting it to 20s and the drive will switch to mode 7 every single time.

ros_canopen/canopen_402/src/motor.cpp

Line 333 in e10f490

time_point abstime = get_abs_time(boost::chrono::seconds(5));

Maybe would be good to have this as a YAML parameter for the motor layer? I would happily try to get a PR going but my C++ skills are not the sharpest.. I could give it a try though.

[aatb-ch]

I have read further the sources to check something else, and fell on this line:

ros_canopen/canopen_402/include/canopen_402/motor.h

Line 299 in e10f490

state_switch_timeout_(settings.get_optional<unsigned int>("state_switch_timeout", 5))

This is specific to state switching, I should be able to use this template to add a second timeout parameter for mode switching.