From b6b35e306222c701b7ab17f12eb678be246af1a2 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Sun, 20 Jul 2025 21:12:26 +0200 Subject: [PATCH] Add new RAW_STEADY_TIME changes Signed-off-by: Sai Kishor Kothakota --- rclpy/rclpy/clock.py | 2 ++ rclpy/rclpy/impl/_rclpy_pybind11.pyi | 4 ++++ rclpy/src/rclpy/_rclpy_pybind11.cpp | 1 + rclpy/src/rclpy/clock_event.cpp | 3 +++ rclpy/test/test_clock.py | 25 +++++++++++++++++++++---- rclpy/test/test_time.py | 21 ++++++++++++++++++++- rclpy/test/test_time_source.py | 4 ++++ 7 files changed, 55 insertions(+), 5 deletions(-) diff --git a/rclpy/rclpy/clock.py b/rclpy/rclpy/clock.py index 8cef3b2c3..1e1977d34 100644 --- a/rclpy/rclpy/clock.py +++ b/rclpy/rclpy/clock.py @@ -279,6 +279,8 @@ def on_time_jump(time_jump: TimeJump) -> None: event.wait_until_system(self.__clock, until._time_handle) elif ClockType.STEADY_TIME == self._clock_type: event.wait_until_steady(self.__clock, until._time_handle) + elif ClockType.RAW_STEADY_TIME == self._clock_type: + event.wait_until_raw_steady(self.__clock, until._time_handle) elif ClockType.ROS_TIME == self._clock_type: event.wait_until_ros(self.__clock, until._time_handle) diff --git a/rclpy/rclpy/impl/_rclpy_pybind11.pyi b/rclpy/rclpy/impl/_rclpy_pybind11.pyi index ed6399e8d..775839a69 100644 --- a/rclpy/rclpy/impl/_rclpy_pybind11.pyi +++ b/rclpy/rclpy/impl/_rclpy_pybind11.pyi @@ -60,6 +60,7 @@ class ClockType(IntEnum): ROS_TIME = ... SYSTEM_TIME = ... STEADY_TIME = ... + RAW_STEADY_TIME = ... class GoalEvent(IntEnum): @@ -1118,6 +1119,9 @@ class ClockEvent: def wait_until_steady(self, clock: Clock, until: rcl_time_point_t) -> None: """Wait for the event to be set (monotonic wait).""" + def wait_until_raw_steady(self, clock: Clock, until: rcl_time_point_t) -> None: + """Wait for the event to be set (monotonic slew-free wait).""" + def wait_until_system(self, clock: Clock, until: rcl_time_point_t) -> None: """Wait for the event to be set (system timed wait).""" diff --git a/rclpy/src/rclpy/_rclpy_pybind11.cpp b/rclpy/src/rclpy/_rclpy_pybind11.cpp index f33fdaa2f..470e55ae3 100644 --- a/rclpy/src/rclpy/_rclpy_pybind11.cpp +++ b/rclpy/src/rclpy/_rclpy_pybind11.cpp @@ -67,6 +67,7 @@ PYBIND11_MODULE(_rclpy_pybind11, m) { .value("ROS_TIME", RCL_ROS_TIME) .value("SYSTEM_TIME", RCL_SYSTEM_TIME) .value("STEADY_TIME", RCL_STEADY_TIME); + .value("RAW_STEADY_TIME", RCL_RAW_STEADY_TIME); py::enum_(m, "GoalEvent") .value("EXECUTE", GOAL_EVENT_EXECUTE) diff --git a/rclpy/src/rclpy/clock_event.cpp b/rclpy/src/rclpy/clock_event.cpp index 71056dee3..508adb91a 100644 --- a/rclpy/src/rclpy/clock_event.cpp +++ b/rclpy/src/rclpy/clock_event.cpp @@ -99,6 +99,9 @@ void define_clock_event(py::object module) .def( "wait_until_steady", &ClockEvent::wait_until, "Wait for the event to be set (monotonic wait)") + .def( + "wait_until_raw_steady", &ClockEvent::wait_until, + "Wait for the event to be set (monotonic slew-free wait)") .def( "wait_until_system", &ClockEvent::wait_until, "Wait for the event to be set (system timed wait)") diff --git a/rclpy/test/test_clock.py b/rclpy/test/test_clock.py index f89f2e823..7924cdc97 100644 --- a/rclpy/test/test_clock.py +++ b/rclpy/test/test_clock.py @@ -75,6 +75,8 @@ def test_clock_construction(self) -> None: assert clock.clock_type == ClockType.STEADY_TIME clock = Clock(clock_type=ClockType.SYSTEM_TIME) assert clock.clock_type == ClockType.SYSTEM_TIME + clock = Clock(clock_type=ClockType.RAW_STEADY_TIME) + assert clock.clock_type == ClockType.RAW_STEADY_TIME # A subclass ROSClock is returned if ROS_TIME is specified. clock = Clock(clock_type=ClockType.ROS_TIME) assert clock.clock_type == ClockType.ROS_TIME @@ -107,6 +109,15 @@ def test_clock_now(self) -> None: assert now2 > now now = now2 + # RAW Steady time should always return increasing values + clock = Clock(clock_type=ClockType.RAW_STEADY_TIME) + now = clock.now() + now2 = now + for i in range(10): + now2 = clock.now() + assert now2 > now + now = now2 + def test_ros_time_is_active(self) -> None: clock = ROSClock() clock._set_ros_time_is_active(True) @@ -237,6 +248,8 @@ def test_sleep_until_mismatched_clock_type(default_context: Context) -> None: clock = Clock(clock_type=ClockType.SYSTEM_TIME) with pytest.raises(ValueError, match='.*clock type does not match.*'): clock.sleep_until(Time(clock_type=ClockType.STEADY_TIME)) + with pytest.raises(ValueError, match='.*clock type does not match.*'): + clock.sleep_until(Time(clock_type=ClockType.RAW_STEADY_TIME)) def test_sleep_until_non_default_context(non_default_context: Context) -> None: @@ -262,7 +275,8 @@ def test_sleep_for_invalid_context() -> None: @pytest.mark.parametrize( - 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.ROS_TIME)) + 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.RAW_STEADY_TIME, + ClockType.ROS_TIME)) def test_sleep_until_basic(default_context: Context, clock_type: ClockType) -> None: clock = Clock(clock_type=clock_type) sleep_duration = Duration(seconds=0.1) @@ -273,7 +287,8 @@ def test_sleep_until_basic(default_context: Context, clock_type: ClockType) -> N @pytest.mark.parametrize( - 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.ROS_TIME)) + 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.RAW_STEADY_TIME, + ClockType.ROS_TIME)) def test_sleep_for_basic(default_context: Context, clock_type: ClockType) -> None: clock = Clock(clock_type=clock_type) sleep_duration = Duration(seconds=0.1) @@ -284,7 +299,8 @@ def test_sleep_for_basic(default_context: Context, clock_type: ClockType) -> Non @pytest.mark.parametrize( - 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.ROS_TIME)) + 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.RAW_STEADY_TIME, + ClockType.ROS_TIME)) def test_sleep_until_time_in_past(default_context: Context, clock_type: ClockType) -> None: clock = Clock(clock_type=clock_type) sleep_duration = Duration(seconds=-1) @@ -295,7 +311,8 @@ def test_sleep_until_time_in_past(default_context: Context, clock_type: ClockTyp @pytest.mark.parametrize( - 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.ROS_TIME)) + 'clock_type', (ClockType.SYSTEM_TIME, ClockType.STEADY_TIME, ClockType.RAW_STEADY_TIME, + ClockType.ROS_TIME)) def test_sleep_for_negative_duration(default_context: Context, clock_type: ClockType) -> None: clock = Clock(clock_type=clock_type) sleep_duration = Duration(seconds=-1) diff --git a/rclpy/test/test_time.py b/rclpy/test/test_time.py index 82175d121..22c589f76 100644 --- a/rclpy/test/test_time.py +++ b/rclpy/test/test_time.py @@ -157,6 +157,21 @@ def test_time_comparators(self) -> None: with self.assertRaises(TypeError): time1 <= time2 + time1 = Time(nanoseconds=1, clock_type=ClockType.SYSTEM_TIME) + time2 = Time(nanoseconds=2, clock_type=ClockType.RAW_STEADY_TIME) + with self.assertRaises(TypeError): + time1 == time2 + with self.assertRaises(TypeError): + time1 != time2 + with self.assertRaises(TypeError): + time1 > time2 + with self.assertRaises(TypeError): + time1 >= time2 + with self.assertRaises(TypeError): + time1 < time2 + with self.assertRaises(TypeError): + time1 <= time2 + # Invalid combinations time1 = Time(nanoseconds=1) self.assertFalse(time1 == 1) @@ -254,5 +269,9 @@ def test_time_datetime_conversions(self) -> None: time3.to_datetime() with self.assertRaises(TypeError): - time4 = Time(nanoseconds=0, clock_type=ClockType.UNINITIALIZED) + time4 = Time(nanoseconds=5e8, clock_type=ClockType.RAW_STEADY_TIME) time4.to_datetime() + + with self.assertRaises(TypeError): + time5 = Time(nanoseconds=0, clock_type=ClockType.UNINITIALIZED) + time5.to_datetime() diff --git a/rclpy/test/test_time_source.py b/rclpy/test/test_time_source.py index 2a5697110..8493bef5c 100644 --- a/rclpy/test/test_time_source.py +++ b/rclpy/test/test_time_source.py @@ -98,6 +98,10 @@ def test_time_source_attach_clock(self) -> None: time_source.attach_clock( Clock(clock_type=ClockType.STEADY_TIME)) # type: ignore[arg-type] + with self.assertRaises(ValueError): + time_source.attach_clock( + Clock(clock_type=ClockType.RAW_STEADY_TIME)) # type: ignore[arg-type] + def test_time_source_not_using_sim_time(self) -> None: time_source = TimeSource(node=self.node) clock = ROSClock()