Added timeout_sec_to_nsec utility

rclpy/rclpy/executors.py

@@ -17,10 +17,10 @@
 from threading import Condition as _Condition
 from threading import Lock as _Lock
 
-from rclpy.constants import S_TO_NS
 from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
 from rclpy.timer import WallTimer as _WallTimer
 from rclpy.utilities import ok
+from rclpy.utilities import timeout_sec_to_nsec
 
 
 class _WaitSet:
@@ -57,16 +57,17 @@ def wait(self, timeout_sec=None):
         """
         Wait until all work completes.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :rtype: bool True if all work completed
         """
+        if timeout_sec is not None and timeout_sec < 0:
+            timeout_sec = None
         # Wait for all work to complete
-        if timeout_sec is None or timeout_sec >= 0:
-            with self._work_condition:
-                if not self._work_condition.wait_for(
-                        lambda: self._num_work_executing == 0, timeout_sec):
-                    return False
+        with self._work_condition:
+            if not self._work_condition.wait_for(
+                    lambda: self._num_work_executing == 0, timeout_sec):
+                return False
         return True
 
 
@@ -100,7 +101,7 @@ def shutdown(self, timeout_sec=None):
 
         Return true if all outstanding callbacks finished executing.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :rtype: bool
         """
@@ -152,7 +153,7 @@ def spin_once(self, timeout_sec=None):
 
         A custom executor should use :func:`Executor.wait_for_ready_callbacks` to get work.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :rtype: None
         """
@@ -260,21 +261,15 @@ def wait_for_ready_callbacks(self, timeout_sec=None, nodes=None):
         """
         Yield callbacks that are ready to be performed.
 
-        :param timeout_sec: Seconds to wait. Block forever if None. Don't wait if <= 0
+        :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if 0
         :type timeout_sec: float or None
         :param nodes: A list of nodes to wait on. Wait on all nodes if None.
         :type nodes: list or None
         :rtype: Generator[(callable, entity, :class:`rclpy.node.Node`)]
         """
         timeout_timer = None
-        # Get timeout in nanoseconds. 0 = don't wait. < 0 means block forever
-        timeout_nsec = None
-        if timeout_sec is None:
-            timeout_nsec = -1
-        elif timeout_sec <= 0:
-            timeout_nsec = 0
-        else:
-            timeout_nsec = int(float(timeout_sec) * S_TO_NS)
+        timeout_nsec = timeout_sec_to_nsec(timeout_sec)
+        if timeout_nsec > 0:
             timeout_timer = _WallTimer(None, None, timeout_nsec)
 
         if nodes is None:

rclpy/rclpy/utilities.py

@@ -14,6 +14,9 @@
 
 import threading
 
+from rclpy.constants import S_TO_NS
+
+
 g_shutdown_lock = threading.Lock()
 
 
@@ -44,3 +47,23 @@ def get_rmw_implementation_identifier():
     # imported locally to avoid loading extensions on module import
     from rclpy.impl.implementation_singleton import rclpy_implementation
     return rclpy_implementation.rclpy_get_rmw_implementation_identifier()
+
+
+def timeout_sec_to_nsec(timeout_sec):
+    """
+    Convert timeout in seconds to rcl compatible timeout in nanoseconds.
+
+    Python tends to use floating point numbers in seconds for timeouts. This utility converts a
+    python-style timeout to an integer in nanoseconds that can be used by rcl_wait.
+
+    :param timeout_sec: Seconds to wait. Block forever if None or negative. Don't wait if < 1ns
+    :type timeout_sec: float or None
+    :rtype: int
+    :returns: rcl_wait compatible timeout in nanoseconds
+    """
+    if timeout_sec is None or timeout_sec < 0:
+        # Block forever
+        return -1
+    else:
+        # wait for given time
+        return int(float(timeout_sec) * S_TO_NS)

rclpy/test/test_utilities.py

@@ -0,0 +1,31 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import unittest
+
+from rclpy.constants import S_TO_NS
+import rclpy.utilities
+
+
+class TestUtilities(unittest.TestCase):
+
+    def test_timeout_sec_to_nsec(self):
+        self.assertGreater(0, rclpy.utilities.timeout_sec_to_nsec(None))
+        self.assertGreater(0, rclpy.utilities.timeout_sec_to_nsec(-1))
+        self.assertEqual(0, rclpy.utilities.timeout_sec_to_nsec(0))
+        self.assertEqual(int(1.5 * S_TO_NS), rclpy.utilities.timeout_sec_to_nsec(1.5))
+
+
+if __name__ == '__main__':
+    unittest.main()