Fixed uninitialized Time usage in rosservice call

[peci1]

When launching my application with sim time, I sometimes get a fail from a rosservice call --wait /node/set_logger_level ... which is launched from the launch file together with the node it is configuring. The fail is not easily reproducible in a simple MWE. In my application, it happens in ~20% launches.

The error I get is:

[Errno 111] Connection refused
Traceback (most recent call last):
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 152, in wait_for_service
    uri = contact_service(resolved_name)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 113, in contact_service
    s.connect(addr)
ConnectionRefusedError: [Errno 111] Connection refused

During handling of the above exception, another exception occurred:

Traceback (most recent call last):
  File "/opt/ros/noetic/lib/rosservice/rosservice", line 35, in <module>
    rosservice.rosservicemain()
  File "/opt/ros/noetic/lib/python3/dist-packages/rosservice/__init__.py", line 768, in rosservicemain
    _rosservice_cmd_call(argv)
  File "/opt/ros/noetic/lib/python3/dist-packages/rosservice/__init__.py", line 611, in _rosservice_cmd_call
    rospy.wait_for_service(service_name)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/impl/tcpros_service.py", line 164, in wait_for_service
    rospy.core.logwarn_throttle(10, "wait_for_service(%s): failed to contact, will keep trying"%resolved_name)
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/core.py", line 243, in logwarn_throttle
    _base_logger(msg, args, kwargs, throttle=period, level='warn')
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/core.py", line 180, in _base_logger
    if _logging_throttle(caller_id, throttle):
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/core.py", line 218, in __call__
    now = rospy.Time.now()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/rostime.py", line 155, in now
    return get_rostime()
  File "/opt/ros/noetic/lib/python3/dist-packages/rospy/rostime.py", line 190, in get_rostime
    raise rospy.exceptions.ROSInitException("time is not initialized. Have you called init_node()?")
rospy.exceptions.ROSInitException: time is not initialized. Have you called init_node()?

The explanation is simple: In case contact_service() ends with Connection refused (this is the hard-to-reproduce part, but it happens on node startup), it calls:

ros_comm/clients/rospy/src/rospy/impl/tcpros_service.py

Line 163 in 845f746

rospy.core.logwarn_throttle(10, "wait_for_service(%s): failed to contact, will keep trying"%resolved_name)

However, for the throttle to work, ROS time has to be initialized. However, rosservice does init_node() only later in the code, so at time of the throttled logging, ROS time is not yet initialized.

This fix uses the same approach as rostopic: explicitly initialize time prior to any possible logging:

ros_comm/tools/rostopic/src/rostopic/__init__.py

Lines 1013 to 1014 in 845f746

	# initialize rospy time due to potential timestamp printing
	rospy.rostime.set_rostime_initialized(True)

I tried to write an MWE, but I could not reproduce it with it. Probably, the called node has to have a bit more complicated startup...

<launch>
  <param name="use_sim_time" value="true" />
  <node pkg="rosservice" type="rosservice" name="test_call" args="call --wait /test/set_logger_level 'rosout' 'debug'" />
  <node pkg="rostopic" type="rostopic" name="test" args="pub a std_msgs/Bool 'data: false'" />
</launch>

With this fix, I can see the throttled message being correctly written to the terminal in the 20% failing cases, and the rosservice call finishes successfully in the contact_service() call.

[sloretz]

Thank you for the PR!