Allow option to initialise robot in any starting state

[justagist]

Currently, the plugin initialising joint commands using values from the initial_value parameter of state_interface in the ros2_control tag for each joint. This means the robot has to always start at the same configuration, which can be a problem in some cases.
This PR introduces option to initialise the robot at any joint state, by initialising the commands with the first joint state value received via joint_states_topic (if available while controller is loading).

Introduces the <param name="use_initial_states_as_initial_commands">true</param> tag to the plugin (defaults to false).

[SFhmichael]

This is what I need, thanks for your contribution @justagist.

[justagist]

Hi @JafarAbdi, can you review this please?

[JafarAbdi]

Currently, the plugin initialising joint commands using values from the initial_value parameter of state_interface in the ros2_control tag for each joint. This means the robot has to always start at the same configuration, which can be a problem in some cases.

Should we use the initial_value from command_interface then?

Maybe a better option is to completely remove https://github.com/PickNikRobotics/topic_based_ros2_control/blob/main/src/topic_based_system.cpp#L80-L98 and set both state/command interfaces to nan

Do you mind sharing an example I can test? Thanks!

[JafarAbdi]

#28 should fix this