Inconsistency with the controller node clock in simulation

[saikishor]

Describe the bug
We have been facing an issue for several months that the JTC doesn't publish the state or the controller_state topic after starting TIAGo simulation. After taking a close look today, I found out that the issue is coming from the controller_interface itself when the controllers are loaded and launched in the simulation, their node clock tends to stay in the RCL_SYSTEM_TIME` for some cycle time instead of switching to the clock of the gazebo.

It seems that the JTC is entering into this condition and never publishing the state topic
https://github.com/ros-controls/ros2_controllers/blob/e08dcb91123a511fbf8233161d4f8d5ffad854be/joint_trajectory_controller/src/joint_trajectory_controller.cpp#L1136-L1138

I've added some print statements and found out the following

last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 1.7055e+09
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 1.7055e+09
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 1.7055e+09
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 1.7055e+09
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 1.7055e+09
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 1.7055e+09
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.672
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.684
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.695
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.705
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.717
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.727

As shown above, this is creating a jump and then that particular condition is always true and the topic is never published. It seems to be the fact that the clock is trying to subscribe to the parameter event of use_sim_time, and this is delaying the resetting of the clock to the clock provided by the gazebo.

I tried to do add some logic in the init and configure methods to see, if I can find the clock type and wait for it to be good one, and unfortunately this trick doesn't work in this case.

Solution:
The only way I could get the proper close inside the controllers is by parsing the use_sim_time as an argument to the NodeOptions at the time of LifeCycleNode creation and this did help to fix the issue by correctly initializing the clock from the beginning

rclcpp::NodeOptions()
        .allow_undeclared_parameters(true)
        .automatically_declare_parameters_from_overrides(true).arguments(
      {"--ros-args", "-p", "use_sim_time:=true", "--params-file", "/tmp/foo.yaml"})

For this to be properly done, we might need to use a part of this PR : #1293

Expected behavior

last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.672
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.684
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.695
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.705
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.717
last_state_publish_time_: 1.7055e+09, state_publisher_period_ : 0.02 and get_node()->now() : 0.727

Screenshots
BEFORE THE FIX

AFTER APPLYING THE ABOVE SOLUTION

Environment (please complete the following information):

• Version Humble