Time, Duration, Clock wrapping rcl

rclpy/package.xml

@@ -21,6 +21,7 @@
   <depend>rcl</depend>
 
   <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>builtin_interfaces</exec_depend>
 
   <test_depend>ament_cmake_pytest</test_depend>
   <test_depend>ament_lint_auto</test_depend>

rclpy/rclpy/clock.py

@@ -0,0 +1,55 @@
+# Copyright 2018 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.
+
+from enum import IntEnum
+
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+
+
+class ClockType(IntEnum):
+    """
+    Enum for clock type.
+
+    This enum matches the one defined in rcl/time.h
+    """
+
+    ROS_TIME = 1
+    SYSTEM_TIME = 2
+    STEADY_TIME = 3
+
+
+class Clock:
+
+    def __init__(self, *, clock_type=ClockType.SYSTEM_TIME):
+        if not isinstance(clock_type, ClockType):
+            raise TypeError('Clock type must be a ClockType enum')
+        if clock_type is ClockType.ROS_TIME:
+            raise NotImplementedError
+        self._clock_handle = _rclpy.rclpy_create_clock(clock_type)
+        self._clock_type = clock_type
+
+    @property
+    def clock_type(self):
+        return self._clock_type
+
+    def __repr__(self):
+        return 'Clock(clock_type={0})'.format(self.clock_type.name)
+
+    def now(self):
+        from rclpy.time import Time
+        time_handle = _rclpy.rclpy_clock_get_now(self._clock_handle)
+        # TODO(dhood): Return a python object from the C extension
+        return Time(
+            nanoseconds=_rclpy.rclpy_time_point_get_nanoseconds(time_handle),
+            clock_type=self.clock_type)

rclpy/rclpy/duration.py

@@ -0,0 +1,82 @@
+# Copyright 2018 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 builtin_interfaces
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+
+
+class Duration:
+
+    def __init__(self, *, seconds=0, nanoseconds=0):
+        if seconds < 0:
+            raise ValueError('Seconds value must not be negative')
+        if nanoseconds < 0:
+            raise ValueError('Nanoseconds value must not be negative')
+        total_nanoseconds = int(seconds * 1e9)
+        total_nanoseconds += int(nanoseconds)
+        try:
+            self._duration_handle = _rclpy.rclpy_create_duration(total_nanoseconds)
+        except OverflowError as e:
+            raise OverflowError(
+                'Total nanoseconds value is too large to store in C time point.') from e
+
+    @property
+    def nanoseconds(self):
+        return _rclpy.rclpy_duration_get_nanoseconds(self._duration_handle)
+
+    def __repr__(self):
+        return 'Duration(nanoseconds={0})'.format(self.nanoseconds)
+
+    def __eq__(self, other):
+        if isinstance(other, Duration):
+            return self.nanoseconds == other.nanoseconds
+        # Raise instead of returning NotImplemented to prevent comparison with invalid types,
+        # e.g. ints.
+        # Otherwise `Duration(nanoseconds=5) == 5` will return False instead of raising, and this
+        # could lead to hard-to-find bugs.
+        raise TypeError("Can't compare duration with object of type: ", type(other))
+
+    def __ne__(self, other):
+        return not self.__eq__(other)
+
+    def __lt__(self, other):
+        if isinstance(other, Duration):
+            return self.nanoseconds < other.nanoseconds
+        return NotImplemented
+
+    def __le__(self, other):
+        if isinstance(other, Duration):
+            return self.nanoseconds <= other.nanoseconds
+        return NotImplemented
+
+    def __gt__(self, other):
+        if isinstance(other, Duration):
+            return self.nanoseconds > other.nanoseconds
+        return NotImplemented
+
+    def __ge__(self, other):
+        if isinstance(other, Duration):
+            return self.nanoseconds >= other.nanoseconds
+        return NotImplemented
+
+    def to_msg(self):
+        seconds = int(self.nanoseconds * 1e-9)
+        nanoseconds = int(self.nanoseconds % 1e9)
+        return builtin_interfaces.msg.Duration(sec=seconds, nanosec=nanoseconds)
+
+    @classmethod
+    def from_msg(cls, msg):
+        if not isinstance(msg, builtin_interfaces.msg.Duration):
+            raise TypeError('Must pass a builtin_interfaces.msg.Duration object')
+        return cls(seconds=msg.sec, nanoseconds=msg.nanosec)

rclpy/rclpy/time.py

@@ -0,0 +1,134 @@
+# Copyright 2018 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 builtin_interfaces
+from rclpy.clock import ClockType
+from rclpy.duration import Duration
+from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy
+
+
+class Time:
+
+    def __init__(self, *, seconds=0, nanoseconds=0, clock_type=ClockType.SYSTEM_TIME):
+        if not isinstance(clock_type, ClockType):
+            raise TypeError('Clock type must be a ClockType enum')
+        if seconds < 0:
+            raise ValueError('Seconds value must not be negative')
+        if nanoseconds < 0:
+            raise ValueError('Nanoseconds value must not be negative')
+        total_nanoseconds = int(seconds * 1e9)
+        total_nanoseconds += int(nanoseconds)
+        try:
+            self._time_handle = _rclpy.rclpy_create_time_point(total_nanoseconds, clock_type)
+        except OverflowError as e:
+            raise OverflowError(
+                'Total nanoseconds value is too large to store in C time point.') from e
+        self._clock_type = clock_type
+
+    @property
+    def nanoseconds(self):
+        return _rclpy.rclpy_time_point_get_nanoseconds(self._time_handle)
+
+    @property
+    def clock_type(self):
+        return self._clock_type
+
+    def __repr__(self):
+        return 'Time(nanoseconds={0}, clock_type={1})'.format(
+            self.nanoseconds, self.clock_type.name)
+
+    def __add__(self, other):
+        if isinstance(other, Duration):
+            try:
+                return Time(
+                    nanoseconds=(self.nanoseconds + other.nanoseconds),
+                    clock_type=self.clock_type)
+            except OverflowError as e:
+                raise OverflowError('Addition leads to overflow in C storage.') from e
+        else:
+            return NotImplemented
+
+    def __radd__(self, other):
+        return self.__add__(other)
+
+    def __sub__(self, other):
+        if isinstance(other, Time):
+            if self.clock_type != other.clock_type:
+                raise TypeError("Can't subtract times with different clock types")
+            try:
+                return Duration(nanoseconds=(self.nanoseconds - other.nanoseconds))
+            except ValueError as e:
+                raise ValueError('Subtraction leads to negative duration.') from e
+        if isinstance(other, Duration):
+            try:
+                return Time(
+                    nanoseconds=(self.nanoseconds - other.nanoseconds),
+                    clock_type=self.clock_type)
+            except ValueError as e:
+                raise ValueError('Subtraction leads to negative time.') from e
+        else:
+            return NotImplemented
+
+    def __eq__(self, other):
+        if isinstance(other, Time):
+            if self.clock_type != other.clock_type:
+                raise TypeError("Can't compare times with different clock types")
+            return self.nanoseconds == other.nanoseconds
+        # Raise instead of returning NotImplemented to prevent comparison with invalid types,
+        # e.g. ints.
+        # Otherwise `Time(nanoseconds=5) == 5` will return False instead of raising, and this
+        # could lead to hard-to-find bugs.
+        raise TypeError("Can't compare time with object of type: ", type(other))
+
+    def __ne__(self, other):
+        return not self.__eq__(other)
+
+    def __lt__(self, other):
+        if isinstance(other, Time):
+            if self.clock_type != other.clock_type:
+                raise TypeError("Can't compare times with different clock types")
+            return self.nanoseconds < other.nanoseconds
+        return NotImplemented
+
+    def __le__(self, other):
+        if isinstance(other, Time):
+            if self.clock_type != other.clock_type:
+                raise TypeError("Can't compare times with different clock types")
+            return self.nanoseconds <= other.nanoseconds
+        return NotImplemented
+
+    def __gt__(self, other):
+        if isinstance(other, Time):
+            if self.clock_type != other.clock_type:
+                raise TypeError("Can't compare times with different clock types")
+            return self.nanoseconds > other.nanoseconds
+        return NotImplemented
+
+    def __ge__(self, other):
+        if isinstance(other, Time):
+            if self.clock_type != other.clock_type:
+                raise TypeError("Can't compare times with different clock types")
+            return self.nanoseconds >= other.nanoseconds
+        return NotImplemented
+
+    def to_msg(self):
+        seconds = int(self.nanoseconds * 1e-9)
+        nanoseconds = int(self.nanoseconds % 1e9)
+        return builtin_interfaces.msg.Time(sec=seconds, nanosec=nanoseconds)
+
+    @classmethod
+    def from_msg(cls, msg, clock_type=ClockType.ROS_TIME):
+        if not isinstance(msg, builtin_interfaces.msg.Time):
+            raise TypeError('Must pass a builtin_interfaces.msg.Time object')
+        return cls(seconds=msg.sec, nanoseconds=msg.nanosec, clock_type=clock_type)