From de1c8fc6ec36ff992d65c3aae54a99e7be65162b Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 28 Nov 2017 15:34:35 -0800 Subject: [PATCH 1/2] Added timeout_sec_to_nsec utility Executor timeouts now block if None or Negative Executor is only non-blocking if timeout is zero --- rclpy/rclpy/executors.py | 31 +++++++++++++------------------ rclpy/rclpy/utilities.py | 23 +++++++++++++++++++++++ rclpy/test/test_utilities.py | 31 +++++++++++++++++++++++++++++++ 3 files changed, 67 insertions(+), 18 deletions(-) create mode 100644 rclpy/test/test_utilities.py diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index 7825ac8aa..bfcae0b41 100644 --- a/rclpy/rclpy/executors.py +++ b/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: diff --git a/rclpy/rclpy/utilities.py b/rclpy/rclpy/utilities.py index 58141481e..9a8dcfb37 100644 --- a/rclpy/rclpy/utilities.py +++ b/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) diff --git a/rclpy/test/test_utilities.py b/rclpy/test/test_utilities.py new file mode 100644 index 000000000..82cb9fc57 --- /dev/null +++ b/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() From c82a256b40fcb2d1e2828980deef6a588271746d Mon Sep 17 00:00:00 2001 From: Shane Loretz Date: Tue, 28 Nov 2017 16:16:30 -0800 Subject: [PATCH 2/2] Test timeout <1ns doesn't block --- rclpy/test/test_utilities.py | 1 + 1 file changed, 1 insertion(+) diff --git a/rclpy/test/test_utilities.py b/rclpy/test/test_utilities.py index 82cb9fc57..4817fea26 100644 --- a/rclpy/test/test_utilities.py +++ b/rclpy/test/test_utilities.py @@ -24,6 +24,7 @@ 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(0, rclpy.utilities.timeout_sec_to_nsec(0.5 / S_TO_NS)) self.assertEqual(int(1.5 * S_TO_NS), rclpy.utilities.timeout_sec_to_nsec(1.5))