From eaee1d2b75d39072ea532517d39c6fa5e53452b1 Mon Sep 17 00:00:00 2001 From: Michael Carlstrom <36806982+InvincibleRMC@users.noreply.github.com> Date: Thu, 7 Mar 2024 21:22:00 -0500 Subject: [PATCH] Creates Enum wrapper for ClockType and ClockChange (#1235) * Testing out Enum wrapper for ClockType * convert to rcl_clock_type_t * Update create_time_point Signed-off-by: Michael Carlstrom --- rclpy/rclpy/clock.py | 17 +++++++++++------ rclpy/rclpy/clock_type.py | 30 ++++++++++++++++++++++++++++++ rclpy/rclpy/executors.py | 2 +- rclpy/rclpy/time.py | 9 +++++---- rclpy/rclpy/time_source.py | 2 +- rclpy/src/rclpy/clock.cpp | 7 ++++--- rclpy/src/rclpy/clock.hpp | 2 +- rclpy/src/rclpy/time_point.cpp | 4 ++-- rclpy/test/test_clock.py | 2 +- rclpy/test/test_node.py | 2 +- rclpy/test/test_time.py | 2 +- rclpy/test/test_time_source.py | 2 +- rclpy/test/test_waitable.py | 2 +- 13 files changed, 60 insertions(+), 23 deletions(-) create mode 100644 rclpy/rclpy/clock_type.py diff --git a/rclpy/rclpy/clock.py b/rclpy/rclpy/clock.py index c99ff6d5d..c43f3f09b 100644 --- a/rclpy/rclpy/clock.py +++ b/rclpy/rclpy/clock.py @@ -12,10 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. +from enum import IntEnum from typing import Optional from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from .clock_type import ClockType from .context import Context from .duration import Duration from .exceptions import NotInitializedException @@ -23,8 +25,11 @@ from .utilities import get_default_context -ClockType = _rclpy.ClockType -ClockChange = _rclpy.ClockChange +class ClockChange(IntEnum): + ROS_TIME_NO_CHANGE = _rclpy.ClockChange.ROS_TIME_NO_CHANGE + ROS_TIME_ACTIVATED = _rclpy.ClockChange.ROS_TIME_ACTIVATED + ROS_TIME_DEACTIVATED = _rclpy.ClockChange.ROS_TIME_DEACTIVATED + SYSTEM_TIME_NO_CHANGE = _rclpy.ClockChange.SYSTEM_TIME_NO_CHANGE class JumpThreshold: @@ -59,8 +64,8 @@ def __init__(self, *, min_forward: Duration, min_backward: Duration, on_clock_ch class TimeJump: - def __init__(self, clock_change, delta): - if not isinstance(clock_change, ClockChange): + def __init__(self, clock_change: ClockChange, delta): + if not isinstance(clock_change, (ClockChange, _rclpy.ClockChange)): raise TypeError('clock_change must be an instance of rclpy.clock.ClockChange') self._clock_change = clock_change self._delta = delta @@ -124,8 +129,8 @@ def __exit__(self, t, v, tb): class Clock: - def __new__(cls, *, clock_type=ClockType.SYSTEM_TIME): - if not isinstance(clock_type, ClockType): + def __new__(cls, *, clock_type: ClockType = ClockType.SYSTEM_TIME): + if not isinstance(clock_type, (ClockType, _rclpy.ClockType)): raise TypeError('Clock type must be a ClockType enum') if clock_type is ClockType.ROS_TIME: self = super().__new__(ROSClock) diff --git a/rclpy/rclpy/clock_type.py b/rclpy/rclpy/clock_type.py new file mode 100644 index 000000000..9504f462f --- /dev/null +++ b/rclpy/rclpy/clock_type.py @@ -0,0 +1,30 @@ +# Copyright 2024 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 must match the one defined in rclpy/_rclpy_pybind11.cpp. + """ + + UNINITIALIZED = _rclpy.ClockType.UNINITIALIZED + ROS_TIME = _rclpy.ClockType.ROS_TIME + SYSTEM_TIME = _rclpy.ClockType.SYSTEM_TIME + STEADY_TIME = _rclpy.ClockType.STEADY_TIME diff --git a/rclpy/rclpy/executors.py b/rclpy/rclpy/executors.py index a6ed8e508..78544c91c 100644 --- a/rclpy/rclpy/executors.py +++ b/rclpy/rclpy/executors.py @@ -39,7 +39,7 @@ from rclpy.client import Client from rclpy.clock import Clock -from rclpy.clock import ClockType +from rclpy.clock_type import ClockType from rclpy.context import Context from rclpy.exceptions import InvalidHandle from rclpy.guard_condition import GuardCondition diff --git a/rclpy/rclpy/time.py b/rclpy/rclpy/time.py index ff405b39b..a9855fd32 100644 --- a/rclpy/rclpy/time.py +++ b/rclpy/rclpy/time.py @@ -18,6 +18,7 @@ from rclpy.duration import Duration from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy +from .clock_type import ClockType CONVERSION_CONSTANT = 10 ** 9 @@ -35,8 +36,8 @@ class Time: def __init__( self, *, seconds=0, nanoseconds=0, - clock_type: _rclpy.ClockType = _rclpy.ClockType.SYSTEM_TIME): - if not isinstance(clock_type, _rclpy.ClockType): + clock_type: ClockType = ClockType.SYSTEM_TIME): + if not isinstance(clock_type, (ClockType, _rclpy.ClockType)): raise TypeError('Clock type must be a ClockType enum') if seconds < 0: raise ValueError('Seconds value must not be negative') @@ -65,7 +66,7 @@ def seconds_nanoseconds(self) -> Tuple[int, int]: return (nanoseconds // CONVERSION_CONSTANT, nanoseconds % CONVERSION_CONSTANT) @property - def clock_type(self) -> _rclpy.ClockType: + def clock_type(self) -> ClockType: """:return: the type of clock that produced this instance.""" return self._time_handle.clock_type @@ -157,7 +158,7 @@ def to_msg(self) -> builtin_interfaces.msg.Time: @classmethod def from_msg( cls, msg: builtin_interfaces.msg.Time, - clock_type: _rclpy.ClockType = _rclpy.ClockType.ROS_TIME + clock_type: ClockType = ClockType.ROS_TIME ) -> 'Time': """ Create a ``Time`` instance from a ROS message. diff --git a/rclpy/rclpy/time_source.py b/rclpy/rclpy/time_source.py index d912b0926..8975397eb 100644 --- a/rclpy/rclpy/time_source.py +++ b/rclpy/rclpy/time_source.py @@ -15,8 +15,8 @@ import weakref from rcl_interfaces.msg import SetParametersResult -from rclpy.clock import ClockType from rclpy.clock import ROSClock +from rclpy.clock_type import ClockType from rclpy.parameter import Parameter from rclpy.qos import QoSProfile from rclpy.qos import ReliabilityPolicy diff --git a/rclpy/src/rclpy/clock.cpp b/rclpy/src/rclpy/clock.cpp index 32ed808cb..4cd6b6c62 100644 --- a/rclpy/src/rclpy/clock.cpp +++ b/rclpy/src/rclpy/clock.cpp @@ -27,7 +27,7 @@ using pybind11::literals::operator""_a; namespace rclpy { -Clock::Clock(rcl_clock_type_t clock_type) +Clock::Clock(int clock_type) { // Create a client rcl_clock_ = std::shared_ptr( @@ -46,8 +46,9 @@ Clock::Clock(rcl_clock_type_t clock_type) delete clock; }); + rcl_clock_type_t rcl_clock_type = rcl_clock_type_t(clock_type); rcl_allocator_t allocator = rcl_get_default_allocator(); - rcl_ret_t ret = rcl_clock_init(clock_type, rcl_clock_.get(), &allocator); + rcl_ret_t ret = rcl_clock_init(rcl_clock_type, rcl_clock_.get(), &allocator); if (ret != RCL_RET_OK) { throw RCLError("failed to initialize clock"); } @@ -175,7 +176,7 @@ void Clock::remove_clock_callback(py::object pyjump_handle) void define_clock(py::object module) { py::class_>(module, "Clock") - .def(py::init()) + .def(py::init()) .def_property_readonly( "pointer", [](const Clock & clock) { return reinterpret_cast(clock.rcl_ptr()); diff --git a/rclpy/src/rclpy/clock.hpp b/rclpy/src/rclpy/clock.hpp index c303d29b8..5ef3c2a5a 100644 --- a/rclpy/src/rclpy/clock.hpp +++ b/rclpy/src/rclpy/clock.hpp @@ -39,7 +39,7 @@ class Clock : public Destroyable, public std::enable_shared_from_this * \param[in] clock_type enum of type ClockType * This constructor creates a Clock object */ - explicit Clock(rcl_clock_type_t clock_type); + explicit Clock(int clock_type); /// Returns the current value of the clock /** diff --git a/rclpy/src/rclpy/time_point.cpp b/rclpy/src/rclpy/time_point.cpp index 043a80252..54e470d63 100644 --- a/rclpy/src/rclpy/time_point.cpp +++ b/rclpy/src/rclpy/time_point.cpp @@ -22,11 +22,11 @@ namespace rclpy { /// Create a time point rcl_time_point_t -create_time_point(int64_t nanoseconds, rcl_clock_type_t clock_type) +create_time_point(int64_t nanoseconds, int clock_type) { rcl_time_point_t time_point; time_point.nanoseconds = nanoseconds; - time_point.clock_type = clock_type; + time_point.clock_type = rcl_clock_type_t(clock_type); return time_point; } diff --git a/rclpy/test/test_clock.py b/rclpy/test/test_clock.py index 3226afbb2..4d45f1e52 100644 --- a/rclpy/test/test_clock.py +++ b/rclpy/test/test_clock.py @@ -20,10 +20,10 @@ import pytest import rclpy from rclpy.clock import Clock -from rclpy.clock import ClockType from rclpy.clock import JumpHandle from rclpy.clock import JumpThreshold from rclpy.clock import ROSClock +from rclpy.clock_type import ClockType from rclpy.context import Context from rclpy.duration import Duration from rclpy.exceptions import NotInitializedException diff --git a/rclpy/test/test_node.py b/rclpy/test/test_node.py index ec441a384..d64257114 100644 --- a/rclpy/test/test_node.py +++ b/rclpy/test/test_node.py @@ -31,7 +31,7 @@ from rcl_interfaces.msg import SetParametersResult from rcl_interfaces.srv import GetParameters import rclpy -from rclpy.clock import ClockType +from rclpy.clock_type import ClockType from rclpy.duration import Duration from rclpy.exceptions import InvalidParameterException from rclpy.exceptions import InvalidParameterTypeException diff --git a/rclpy/test/test_time.py b/rclpy/test/test_time.py index ae3d74e9f..9323afe96 100644 --- a/rclpy/test/test_time.py +++ b/rclpy/test/test_time.py @@ -14,7 +14,7 @@ import unittest -from rclpy.clock import ClockType +from rclpy.clock_type import ClockType from rclpy.duration import Duration from rclpy.duration import Infinite from rclpy.time import Time diff --git a/rclpy/test/test_time_source.py b/rclpy/test/test_time_source.py index 5ba2815b8..7b0b5ead0 100644 --- a/rclpy/test/test_time_source.py +++ b/rclpy/test/test_time_source.py @@ -19,9 +19,9 @@ import rclpy from rclpy.clock import Clock from rclpy.clock import ClockChange -from rclpy.clock import ClockType from rclpy.clock import JumpThreshold from rclpy.clock import ROSClock +from rclpy.clock_type import ClockType from rclpy.duration import Duration from rclpy.parameter import Parameter from rclpy.time import Time diff --git a/rclpy/test/test_waitable.py b/rclpy/test/test_waitable.py index 5455906dd..3a8f28583 100644 --- a/rclpy/test/test_waitable.py +++ b/rclpy/test/test_waitable.py @@ -19,7 +19,7 @@ import rclpy from rclpy.callback_groups import MutuallyExclusiveCallbackGroup, ReentrantCallbackGroup from rclpy.clock import Clock -from rclpy.clock import ClockType +from rclpy.clock_type import ClockType from rclpy.executors import SingleThreadedExecutor from rclpy.impl.implementation_singleton import rclpy_implementation as _rclpy from rclpy.qos import QoSProfile