From 750dc94c61be406937ad49e0f215a4fdce538fc5 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 13 Jan 2026 14:29:16 +0700 Subject: [PATCH] =?UTF-8?q?add=20WallTime=20v=C3=A0=20WallTimer?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- CMakeLists.txt | 10 + README.md | 66 ++++- WALLTIMER_USAGE.md | 256 ++++++++++++++++ WALLTIME_LIBRARY.md | 362 +++++++++++++++++++++++ WALLTIME_QUICKSTART.md | 87 ++++++ WALLTIME_USAGE.md | 466 +++++++++++++++++++++++++++++ examples/walltime_example.cpp | 331 +++++++++++++++++++++ examples/walltimer_example.cpp | 229 ++++++++++++++ include/robot/wall_timer.h | 289 ++++++++++++++++++ include/robot/walltime.h | 475 ++++++++++++++++++++++++++++++ src/wall_timer.cpp | 375 +++++++++++++++++++++++ test/walltime_standalone_test.cpp | 280 ++++++++++++++++++ 12 files changed, 3225 insertions(+), 1 deletion(-) create mode 100644 WALLTIMER_USAGE.md create mode 100644 WALLTIME_LIBRARY.md create mode 100644 WALLTIME_QUICKSTART.md create mode 100644 WALLTIME_USAGE.md create mode 100644 examples/walltime_example.cpp create mode 100644 examples/walltimer_example.cpp create mode 100644 include/robot/wall_timer.h create mode 100644 include/robot/walltime.h create mode 100644 src/wall_timer.cpp create mode 100644 test/walltime_standalone_test.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a85355d..e4afcd6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -52,6 +52,7 @@ add_library(${PROJECT_NAME} SHARED src/rate.cpp src/time.cpp src/timer.cpp + src/wall_timer.cpp ) if(BUILDING_WITH_CATKIN) @@ -151,3 +152,12 @@ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/duration.cpp) GTest::Main ) endif() + +# Standalone WallTime test (header-only) +if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/walltime_standalone_test.cpp) + add_executable(${PROJECT_NAME}_walltime_standalone_test test/walltime_standalone_test.cpp) + target_include_directories(${PROJECT_NAME}_walltime_standalone_test PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include + ) + target_compile_features(${PROJECT_NAME}_walltime_standalone_test PUBLIC cxx_std_17) +endif() diff --git a/README.md b/README.md index d2422c4..efde215 100644 --- a/README.md +++ b/README.md @@ -98,7 +98,70 @@ Rate tự động tính toán thời gian còn lại trong mỗi chu kỳ và sl - Có thể quản lý nhiều timer đồng thời - Cung cấp thông tin chi tiết về timing để debug và monitoring -### 5. WallRate - Rate dùng thời gian thực +### 5. WallTime - Thời gian thực (Wall-clock Time) + +`WallTime` đại diện cho thời gian thực từ hệ thống, luôn sử dụng wall-clock time và không bị ảnh hưởng bởi simulated time. + +**Đặc điểm:** +- Luôn sử dụng thời gian thực từ system clock +- Không cần khởi tạo (`Time::init()`) +- Không bao giờ throw exception +- Không bị ảnh hưởng bởi simulated time +- Thread-safe và cross-platform + +**Khác biệt với Time:** +- `Time` có thể sử dụng simulated time (hữu ích cho testing) +- `WallTime` luôn dùng thời gian thực, không phụ thuộc vào simulated time +- `Time` cần khởi tạo, `WallTime` không cần + +**Ứng dụng:** +- Đo thời gian thực thi (profiling/benchmarking) +- Timeout và thời gian chờ thực tế +- Logging với timestamp thực +- Giao tiếp với hardware cần timing chính xác +- Performance monitoring + +### 6. WallTimer - Hẹn giờ định kỳ với thời gian thực + +`WallTimer` tương tự như `Timer` nhưng sử dụng wall-clock time thay vì simulated time. Đây là công cụ mạnh mẽ cho các tác vụ cần timing chính xác trong thời gian thực. + +**Đặc điểm:** +- Chạy trong thread riêng, không chặn thread chính +- Luôn sử dụng wall-clock time (không bị ảnh hưởng bởi simulated time) +- Có thể là one-shot (chỉ chạy một lần) hoặc repeating (lặp lại) +- Callback nhận thông tin chi tiết về timing (WallTimerEvent) +- Có thể start/stop và điều chỉnh period động +- Tự động cleanup khi hủy + +**Khác biệt với Timer:** +- `Timer` sử dụng `Time`/`Duration` (có thể bị ảnh hưởng bởi simulated time) +- `WallTimer` sử dụng `WallTime`/`WallDuration` (luôn dùng thời gian thực) +- `Timer` phù hợp cho ROS messages và simulation +- `WallTimer` phù hợp cho performance monitoring, hardware interfaces, và real-time operations + +**WallTimerEvent chứa thông tin:** +- Thời gian thực tế khi callback được gọi (wall-clock time) +- Thời gian mong đợi khi callback được gọi (wall-clock time) +- Thời gian của callback trước đó (wall-clock time) +- Khoảng thời gian giữa các callback (để phát hiện drift) + +**Ứng dụng:** +- Performance monitoring và profiling +- Real-time deadlines và timeouts +- Hardware interfaces cần timing chính xác +- Benchmarking và measurement +- Background tasks cần chạy theo thời gian thực + +**Ví dụ:** +```cpp +// Đo thời gian thực thi +robot::WallTime start = robot::WallTime::now(); +doSomething(); +robot::WallDuration elapsed = robot::WallTime::now() - start; +std::cout << "Took " << elapsed.toSec() << " seconds" << std::endl; +``` + +### 6. WallRate - Rate dùng thời gian thực `WallRate` tương tự như `Rate` nhưng luôn sử dụng thời gian thực (wall-clock time) thay vì simulated time. @@ -218,6 +281,7 @@ Sử dụng simulated time để chạy các test cases với timing cụ thể, ## Tài liệu tham khảo +- `WALLTIME_USAGE.md` - Hướng dẫn chi tiết về WallTime với các ví dụ và use cases - `TIMER_USAGE.md` - Hướng dẫn chi tiết về cách sử dụng Timer với các ví dụ - `TIMER_EVENT_EXPLANATION.md` - Giải thích chi tiết về TimerEvent structure và các trường dữ liệu - `CHANGELOG.rst` - Lịch sử thay đổi và version history diff --git a/WALLTIMER_USAGE.md b/WALLTIMER_USAGE.md new file mode 100644 index 0000000..56ccdc2 --- /dev/null +++ b/WALLTIMER_USAGE.md @@ -0,0 +1,256 @@ +# robot::WallTimer Usage Guide + +## Overview + +`robot::WallTimer` is a class similar to `ros::WallTimer` that allows you to call a callback function at a specified rate using **wall-clock time**. It creates a separate thread that periodically invokes your callback function. + +**Key Difference from Timer:** +- `Timer` uses `Time` and `Duration` (can be affected by simulated time) +- `WallTimer` uses `WallTime` and `WallDuration` (always uses real wall-clock time) + +This makes `WallTimer` ideal for: +- Performance monitoring and profiling +- Real-time deadlines and timeouts +- Hardware interfaces requiring precise timing +- Benchmarking and measurement +- Any task that needs to run at real-world intervals + +## Basic Usage + +### Simple WallTimer Example + +```cpp +#include +#include + +void myCallback(const robot::WallTimerEvent& event) +{ + std::cout << "WallTimer fired! Current wall time: " + << event.current_real.toSec() << std::endl; + std::cout << "Time since last callback: " + << event.last_duration.toSec() << " seconds" << std::endl; +} + +int main() +{ + // Create a timer that fires every 1 second (wall-clock time) + robot::WallTimer timer( + robot::WallDuration(1.0), // Period: 1 second + myCallback, // Callback function + false, // Not one-shot (repeats) + true // Auto-start + ); + + // Timer is now running... + // Do other work here + + // Sleep for 5 seconds to see timer fire multiple times + robot::WallDuration(5.0).sleep(); + + // Stop the timer + timer.stop(); + + return 0; +} +``` + +### Using with Class Methods + +```cpp +#include + +class PerformanceMonitor +{ +public: + void startMonitoring() + { + // Create timer with member function callback + timer_ = std::make_unique( + robot::WallDuration(0.5), // 2 Hz (every 0.5 seconds) + [this](const robot::WallTimerEvent& event) { + this->monitorCallback(event); + }, + false, // Repeating + true // Auto-start + ); + } + + void monitorCallback(const robot::WallTimerEvent& event) + { + // Do periodic performance monitoring here + // This always uses real wall-clock time, not simulated time + std::cout << "Performance check at real time: " + << event.current_real.toSec() << std::endl; + } + + void stopMonitoring() + { + if (timer_) + { + timer_->stop(); + } + } + +private: + std::unique_ptr timer_; +}; +``` + +### One-Shot WallTimer + +```cpp +#include + +void delayedAction(const robot::WallTimerEvent& event) +{ + std::cout << "Delayed action executed after 5 seconds (real time)" << std::endl; +} + +int main() +{ + // Create a one-shot timer that fires once after 5 seconds (wall-clock) + robot::WallTimer timer( + robot::WallDuration(5.0), // Wait 5 seconds + delayedAction, // Callback + true, // One-shot + true // Auto-start + ); + + // Wait for timer to fire + robot::WallDuration(6.0).sleep(); + + return 0; +} +``` + +## WallTimerEvent Structure + +The `WallTimerEvent` structure passed to your callback contains: + +- `current_real`: The actual wall-clock time when the callback was called +- `current_expected`: The expected wall-clock time when the callback should have been called +- `last_real`: The actual wall-clock time of the previous callback +- `last_expected`: The expected wall-clock time of the previous callback +- `last_duration`: The wall-clock duration between the last two callbacks + +All times are in wall-clock time (real time), not simulated time. + +## API Reference + +### Constructor + +```cpp +WallTimer(const WallDuration& period, + const Callback& callback, + bool oneshot = false, + bool autostart = true); +``` + +- `period`: Time between callbacks (wall-clock duration) +- `callback`: Function to call (signature: `void(const WallTimerEvent&)`) +- `oneshot`: If true, timer fires only once +- `autostart`: If true, timer starts automatically + +### Methods + +- `void start()`: Start the timer. Does nothing if already started. +- `void stop()`: Stop the timer. Once this returns, no more callbacks will be called. +- `void setPeriod(const WallDuration& period, bool reset = true)`: Set the timer period. If `reset` is true, timer ignores elapsed time and next callback occurs at now()+period. +- `bool hasStarted()`: Check if timer is running +- `bool isValid()`: Check if timer has a valid callback +- `bool hasPending()`: Check if timer has any pending events to call +- `bool isOneShot()`: Check if timer is one-shot +- `void setOneShot(bool oneshot)`: Set one-shot mode +- `WallDuration getPeriod()`: Get the timer period + +### Operators + +- `operator void*()`: Conversion to bool (for checking validity) +- `operator==(const WallTimer&)`: Equality comparison +- `operator!=(const WallTimer&)`: Inequality comparison +- `operator<(const WallTimer&)`: Less-than comparison (for ordering in containers) + +## Comparison: Timer vs WallTimer + +| Feature | `Timer` | `WallTimer` | +|---------|---------|-------------| +| Time Type | `Time` / `Duration` | `WallTime` / `WallDuration` | +| Simulated Time | Can be affected | Never affected | +| Use Case | ROS message timestamps, simulation | Performance, real-time, hardware | +| Initialization | Requires `Time::init()` | No initialization needed | +| Exception | Can throw if time not initialized | Never throws | + +## Example: Performance Profiling + +```cpp +#include + +class Profiler +{ +public: + void startProfiling() + { + profile_timer_ = std::make_unique( + robot::WallDuration(1.0), // Profile every 1 second + [this](const robot::WallTimerEvent& event) { + this->profileCallback(event); + }, + false, // Repeating + true // Auto-start + ); + } + + void profileCallback(const robot::WallTimerEvent& event) + { + // Measure actual wall-clock time between callbacks + double actual_interval = event.last_duration.toSec(); + double expected_interval = 1.0; + + if (actual_interval > expected_interval * 1.1) // 10% tolerance + { + std::cout << "WARNING: Timer drift detected! " + << "Expected: " << expected_interval + << "s, Actual: " << actual_interval << "s" << std::endl; + } + } + +private: + std::unique_ptr profile_timer_; +}; +``` + +## Example: Dynamic Period Adjustment + +```cpp +#include + +robot::WallTimer timer(robot::WallDuration(1.0), myCallback); + +// Later, change the period +timer.setPeriod(robot::WallDuration(0.5), true); // Reset to 0.5s, reset timer + +// Or change without reset +timer.setPeriod(robot::WallDuration(2.0), false); // Change to 2s, don't reset +``` + +## Best Practices + +1. **Use WallTimer for real-time operations**: When you need precise wall-clock timing +2. **Use Timer for ROS messages**: When working with ROS messages that use simulated time +3. **Always stop timers**: Make sure to call `stop()` before destroying the timer +4. **Handle exceptions in callbacks**: Exceptions in callbacks are caught to prevent timer thread crashes +5. **Check validity**: Use `isValid()` or `operator void*()` to check if timer has a callback + +## Thread Safety + +`WallTimer` is thread-safe: +- Multiple threads can safely call `start()`, `stop()`, `setPeriod()`, etc. +- The callback is executed in a separate thread +- All internal state is protected by mutexes + +## References + +- [ROS WallTimer Documentation](http://docs.ros.org/en/indigo/api/roscpp/html/classros_1_1WallTimer.html) +- `TIMER_USAGE.md` - Guide for `robot::Timer` (uses simulated time) +- `WALLTIME_USAGE.md` - Guide for `WallTime` and `WallDuration` + diff --git a/WALLTIME_LIBRARY.md b/WALLTIME_LIBRARY.md new file mode 100644 index 0000000..678120a --- /dev/null +++ b/WALLTIME_LIBRARY.md @@ -0,0 +1,362 @@ +# WallTime Standalone Library + +## Tổng quan + +`walltime.h` là một **header-only library** độc lập cung cấp các class `WallTime` và `WallDuration` để làm việc với thời gian thực (wall-clock time). Thư viện này có thể được sử dụng độc lập hoặc như một phần của `robot_time` library. + +## Đặc điểm + +- ✅ **Header-only**: Chỉ cần include một file header +- ✅ **Không cần khởi tạo**: Có thể sử dụng ngay +- ✅ **Thread-safe**: An toàn khi sử dụng trong multi-threaded applications +- ✅ **Cross-platform**: Hoạt động trên Linux, Windows, macOS +- ✅ **Nanosecond precision**: Độ chính xác nanosecond +- ✅ **Không phụ thuộc**: Chỉ sử dụng C++ standard library + +## Cài đặt + +### Cách 1: Sử dụng trực tiếp (Header-only) + +Chỉ cần copy file `walltime.h` vào project của bạn: + +```cpp +#include "walltime.h" // hoặc đường dẫn tương đối +``` + +### Cách 2: Sử dụng với robot_time library + +Nếu bạn đã có `robot_time` library: + +```cpp +#include +``` + +## API Reference + +### WallTime + +#### Constructors + +```cpp +robot::WallTime t1; // Default: (0, 0) +robot::WallTime t2(1234567890, 123456789); // (sec, nsec) +robot::WallTime t3(1234567890.123456789); // From seconds (double) +``` + +#### Static Methods + +```cpp +// Get current wall-clock time +robot::WallTime now = robot::WallTime::now(); + +// Sleep until a specific time +robot::WallTime target = robot::WallTime::now() + robot::WallDuration(5.0); +bool success = robot::WallTime::sleepUntil(target); + +// Check if using system time (always true) +bool is_system = robot::WallTime::isSystemTime(); // Always returns true +``` + +#### Instance Methods + +```cpp +robot::WallTime t = robot::WallTime::now(); + +// Convert to seconds +double seconds = t.toSec(); + +// Convert to nanoseconds +uint64_t nanoseconds = t.toNSec(); + +// Initialize from seconds +t.fromSec(1234567890.123456789); + +// Initialize from nanoseconds +t.fromNSec(1234567890123456789ULL); + +// Check if zero +bool is_zero = t.isZero(); +``` + +#### Arithmetic Operations + +```cpp +robot::WallTime t = robot::WallTime::now(); +robot::WallDuration d(5.0); + +// Addition +robot::WallTime future = t + d; + +// Subtraction +robot::WallTime past = t - d; + +// Duration subtraction +robot::WallDuration elapsed = future - t; + +// Compound assignment +t += d; +t -= d; +``` + +#### Comparison Operators + +```cpp +robot::WallTime t1, t2; + +bool eq = (t1 == t2); +bool ne = (t1 != t2); +bool lt = (t1 < t2); +bool gt = (t1 > t2); +bool le = (t1 <= t2); +bool ge = (t1 >= t2); +``` + +#### Constants + +```cpp +robot::WallTime::ZERO // (0, 0) +robot::WallTime::MAX // Maximum representable time +robot::WallTime::MIN // Minimum representable time +``` + +### WallDuration + +#### Constructors + +```cpp +robot::WallDuration d1; // Default: (0, 0) +robot::WallDuration d2(5, 123456789); // (sec, nsec) +robot::WallDuration d3(5.123456789); // From seconds (double) +``` + +#### Instance Methods + +```cpp +robot::WallDuration d(5.0); + +// Convert to seconds +double seconds = d.toSec(); + +// Convert to nanoseconds +int64_t nanoseconds = d.toNSec(); + +// Initialize from seconds +d.fromSec(5.123456789); + +// Initialize from nanoseconds +d.fromNSec(5123456789LL); + +// Sleep for this duration +bool success = d.sleep(); + +// Check if zero +bool is_zero = d.isZero(); +``` + +#### Arithmetic Operations + +```cpp +robot::WallDuration d1(5.0); +robot::WallDuration d2(3.0); + +// Addition +robot::WallDuration sum = d1 + d2; + +// Subtraction +robot::WallDuration diff = d1 - d2; + +// Negation +robot::WallDuration neg = -d1; + +// Multiplication +robot::WallDuration scaled = d1 * 2.0; + +// Compound assignment +d1 += d2; +d1 -= d2; +``` + +#### Comparison Operators + +```cpp +robot::WallDuration d1, d2; + +bool eq = (d1 == d2); +bool ne = (d1 != d2); +bool lt = (d1 < d2); +bool gt = (d1 > d2); +bool le = (d1 <= d2); +bool ge = (d1 >= d2); +``` + +#### Constants + +```cpp +robot::WallDuration::ZERO // (0, 0) +robot::WallDuration::MAX // Maximum representable duration +robot::WallDuration::MIN // Minimum representable duration (negative) +``` + +## Ví dụ sử dụng + +### Ví dụ 1: Đo thời gian thực thi + +```cpp +#include +#include + +void measureExecutionTime() +{ + robot::WallTime start = robot::WallTime::now(); + + // Do some work + for (int i = 0; i < 1000000; ++i) + { + // ... your code ... + } + + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration elapsed = end - start; + + std::cout << "Execution time: " << elapsed.toSec() << " seconds" << std::endl; +} +``` + +### Ví dụ 2: Timeout + +```cpp +#include + +bool doWorkWithTimeout(double timeout_seconds) +{ + robot::WallTime start = robot::WallTime::now(); + robot::WallDuration timeout(timeout_seconds); + + while (true) + { + if (robot::WallTime::now() - start > timeout) + { + std::cout << "Timeout!" << std::endl; + return false; + } + + // Do work + if (workComplete()) + { + return true; + } + + robot::WallDuration(0.1).sleep(); // Sleep 100ms + } +} +``` + +### Ví dụ 3: Sleep until + +```cpp +#include + +void scheduleTask() +{ + // Schedule task 5 seconds from now + robot::WallTime target = robot::WallTime::now() + robot::WallDuration(5.0); + + // Do other work... + + // Sleep until target time + robot::WallTime::sleepUntil(target); + + // Execute task + executeTask(); +} +``` + +### Ví dụ 4: Rate limiting + +```cpp +#include + +class RateLimiter +{ +private: + robot::WallTime last_time_; + robot::WallDuration min_interval_; + +public: + RateLimiter(double min_rate_hz) + : min_interval_(1.0 / min_rate_hz) + , last_time_(robot::WallTime::now()) + {} + + void wait() + { + robot::WallTime now = robot::WallTime::now(); + robot::WallDuration elapsed = now - last_time_; + + if (elapsed < min_interval_) + { + robot::WallDuration remaining = min_interval_ - elapsed; + remaining.sleep(); + } + + last_time_ = robot::WallTime::now(); + } +}; + +// Usage +RateLimiter limiter(10.0); // 10 Hz max +while (running) +{ + limiter.wait(); + doWork(); +} +``` + +## So sánh với robot_time::WallTime + +Thư viện standalone này tương thích với `robot::WallTime` trong `robot_time` library: + +| Đặc điểm | Standalone `walltime.h` | `robot_time::WallTime` | +|----------|------------------------|------------------------| +| Header-only | ✅ Có | ❌ Cần link library | +| Dependencies | Chỉ C++ stdlib | Cần robot_time | +| API | Giống nhau | Giống nhau | +| Performance | Tương đương | Tương đương | +| Kích thước | Nhỏ hơn | Lớn hơn | + +## Build và Test + +### Compile test file + +```bash +g++ -std=c++17 -I./include test/walltime_standalone_test.cpp -o walltime_test +./walltime_test +``` + +### Sử dụng trong CMake + +```cmake +# Option 1: Header-only (không cần link) +target_include_directories(your_target PRIVATE + ${CMAKE_CURRENT_SOURCE_DIR}/include) + +# Option 2: Với robot_time library +find_package(robot_time REQUIRED) +target_link_libraries(your_target PRIVATE robot_time) +``` + +## Requirements + +- **C++17** hoặc cao hơn +- **Compiler**: GCC 7+, Clang 5+, MSVC 2017+ + +## License + +BSD License - Tương tự như robot_time library + +## Tài liệu tham khảo + +- `WALLTIME_USAGE.md` - Hướng dẫn chi tiết về WallTime +- `examples/walltime_example.cpp` - Các ví dụ sử dụng +- `test/walltime_standalone_test.cpp` - Unit tests + diff --git a/WALLTIME_QUICKSTART.md b/WALLTIME_QUICKSTART.md new file mode 100644 index 0000000..f7f4bb5 --- /dev/null +++ b/WALLTIME_QUICKSTART.md @@ -0,0 +1,87 @@ +# WallTime - Quick Start Guide + +## Sử dụng nhanh + +### 1. Include header + +```cpp +#include +``` + +### 2. Đo thời gian thực thi + +```cpp +robot::WallTime start = robot::WallTime::now(); +// ... your code ... +robot::WallDuration elapsed = robot::WallTime::now() - start; +std::cout << "Took " << elapsed.toSec() << " seconds" << std::endl; +``` + +### 3. Timeout + +```cpp +robot::WallTime deadline = robot::WallTime::now() + robot::WallDuration(5.0); +while (robot::WallTime::now() < deadline) { + // Do work +} +``` + +### 4. Sleep + +```cpp +// Sleep for 1 second +robot::WallDuration(1.0).sleep(); + +// Sleep until specific time +robot::WallTime target = robot::WallTime::now() + robot::WallDuration(5.0); +robot::WallTime::sleepUntil(target); +``` + +## Đặc điểm + +- ✅ **Header-only**: Chỉ cần include một file +- ✅ **Không cần khởi tạo**: Dùng ngay +- ✅ **Thread-safe**: An toàn với multi-threading +- ✅ **Cross-platform**: Linux, Windows, macOS + +## Tài liệu đầy đủ + +- `WALLTIME_LIBRARY.md` - API reference đầy đủ +- `WALLTIME_USAGE.md` - Hướng dẫn chi tiết và ví dụ +- `examples/walltime_example.cpp` - Nhiều ví dụ thực tế + +## So sánh với Time + +| | `Time` | `WallTime` | +|---|---|---| +| Cần init | ✅ Có | ❌ Không | +| Simulated time | ✅ Có | ❌ Không | +| Dùng cho ROS messages | ✅ Nên | ❌ Không nên | +| Đo thời gian thực | ❌ Không phù hợp | ✅ Phù hợp | + +## Ví dụ hoàn chỉnh + +```cpp +#include +#include + +int main() +{ + // Đo thời gian + robot::WallTime start = robot::WallTime::now(); + + // Simulate work + robot::WallDuration(0.5).sleep(); + + robot::WallDuration elapsed = robot::WallTime::now() - start; + std::cout << "Elapsed: " << elapsed.toSec() << "s" << std::endl; + + return 0; +} +``` + +Compile: +```bash +g++ -std=c++17 -I./include your_file.cpp -o your_program +``` + diff --git a/WALLTIME_USAGE.md b/WALLTIME_USAGE.md new file mode 100644 index 0000000..f47abb4 --- /dev/null +++ b/WALLTIME_USAGE.md @@ -0,0 +1,466 @@ +# WallTime - Hướng dẫn sử dụng + +## Tổng quan + +`WallTime` là class đại diện cho **thời gian thực (wall-clock time)** trong thư viện `robot_time`. Khác với `Time`, `WallTime` luôn sử dụng thời gian thực từ hệ thống và **không bị ảnh hưởng bởi simulated time**. + +## Đặc điểm chính + +### 1. Luôn sử dụng thời gian thực +- `WallTime` luôn lấy thời gian từ system clock +- Không phụ thuộc vào simulated time +- Không cần khởi tạo (`Time::init()`) +- Không bao giờ throw exception + +### 2. Không cần khởi tạo +```cpp +// Time - CẦN khởi tạo +robot::Time::init(); // Phải gọi trước +robot::Time t = robot::Time::now(); // Có thể throw exception + +// WallTime - KHÔNG cần khởi tạo +robot::WallTime t = robot::WallTime::now(); // Luôn hoạt động +``` + +### 3. Không bị ảnh hưởng bởi simulated time +```cpp +// Trong simulation với /use_sim_time = true +robot::Time t1 = robot::Time::now(); // Trả về simulated time +robot::WallTime t2 = robot::WallTime::now(); // Vẫn trả về thời gian thực +``` + +## API Reference + +### Constructors + +```cpp +// Default constructor - khởi tạo với giá trị 0 +robot::WallTime t1; + +// Constructor với giây và nanosecond +robot::WallTime t2(1234567890, 123456789); + +// Constructor từ số thực (giây) +robot::WallTime t3(1234567890.123456789); +``` + +### Static Methods + +#### `static WallTime now()` +Trả về thời gian hiện tại (wall-clock time). + +```cpp +robot::WallTime current_time = robot::WallTime::now(); +``` + +**Đặc điểm:** +- Luôn trả về thời gian thực +- Không cần khởi tạo trước +- Không throw exception +- Thread-safe + +#### `static bool sleepUntil(const WallTime& end)` +Ngủ cho đến khi đạt đến thời điểm `end`. + +```cpp +robot::WallTime end_time = robot::WallTime::now() + robot::WallDuration(5.0); +bool success = robot::WallTime::sleepUntil(end_time); +``` + +**Return value:** +- `true`: Đã ngủ đến thời điểm mong muốn +- `false`: Bị gián đoạn hoặc có lỗi + +#### `static bool isSystemTime()` +Luôn trả về `true` vì `WallTime` luôn sử dụng system time. + +```cpp +bool is_system = robot::WallTime::isSystemTime(); // Luôn là true +``` + +### Methods kế thừa từ `TimeBase` + +#### Chuyển đổi đơn vị + +```cpp +robot::WallTime t(1234567890, 123456789); + +// Chuyển sang giây (double) +double seconds = t.toSec(); // 1234567890.123456789 + +// Chuyển sang nanosecond +uint64_t nanoseconds = t.toNSec(); + +// Khởi tạo từ giây +t.fromSec(1234567890.123456789); + +// Khởi tạo từ nanosecond +t.fromNSec(1234567890123456789ULL); +``` + +#### So sánh + +```cpp +robot::WallTime t1 = robot::WallTime::now(); +robot::WallDuration d(1.0); +robot::WallTime t2 = t1 + d; + +// So sánh +bool is_equal = (t1 == t2); // false +bool is_less = (t1 < t2); // true +bool is_greater = (t1 > t2); // false +bool is_less_equal = (t1 <= t2); // true +bool is_greater_equal = (t1 >= t2); // false +``` + +#### Phép toán với WallDuration + +```cpp +robot::WallTime t = robot::WallTime::now(); +robot::WallDuration d(5.0); + +// Cộng duration +robot::WallTime future = t + d; + +// Trừ duration +robot::WallTime past = t - d; + +// Trừ hai WallTime để được WallDuration +robot::WallDuration elapsed = future - t; + +// Compound assignment +t += d; // t = t + d +t -= d; // t = t - d +``` + +#### Kiểm tra giá trị + +```cpp +robot::WallTime t; + +// Kiểm tra có phải zero không +bool is_zero = t.isZero(); // true + +// Kiểm tra các giá trị đặc biệt +bool is_max = (t == robot::WallTime::MAX); +bool is_min = (t == robot::WallTime::MIN); +bool is_zero_const = (t == robot::WallTime::ZERO); +``` + +### Constants + +```cpp +robot::WallTime::MAX // Giá trị lớn nhất +robot::WallTime::MIN // Giá trị nhỏ nhất +robot::WallTime::ZERO // Zero (0, 0) +robot::WallTime::UNINITIALIZED // Uninitialized (0, 0) +``` + +## So sánh với `Time` + +| Đặc điểm | `Time` | `WallTime` | +|----------|--------|------------| +| Nguồn thời gian | Simulated hoặc wall-clock | Luôn wall-clock | +| Cần khởi tạo | Có (`Time::init()`) | Không | +| Có thể throw exception | Có (nếu chưa init) | Không | +| Ảnh hưởng bởi sim time | Có | Không | +| Dùng trong ROS messages | Nên dùng | Không nên | +| Đo thời gian thực thi | Không phù hợp | Phù hợp | +| Timeout thực tế | Không phù hợp | Phù hợp | +| Logging với timestamp | Có thể | Phù hợp | + +## Use Cases + +### 1. Đo thời gian thực thi (Profiling/Benchmarking) + +```cpp +// Đo thời gian thực thi của một hàm +robot::WallTime start = robot::WallTime::now(); + +// Thực hiện công việc +doSomething(); + +robot::WallTime end = robot::WallTime::now(); +robot::WallDuration elapsed = end - start; + +std::cout << "Thời gian thực thi: " << elapsed.toSec() << " giây" << std::endl; +``` + +**Ví dụ thực tế từ codebase:** +```cpp +// Trong clear_costmap_recovery.cpp +robot::WallTime t0 = robot::WallTime::now(); +clear(global_costmap_); +ROS_DEBUG("Global costmap cleared in %fs", + (robot::WallTime::now() - t0).toSec()); +``` + +### 2. Timeout và thời gian chờ + +```cpp +// Đặt timeout cho một thao tác +robot::WallTime start = robot::WallTime::now(); +robot::WallDuration timeout(5.0); // 5 giây + +while (!operation_complete) +{ + if (robot::WallTime::now() - start > timeout) + { + std::cout << "Timeout!" << std::endl; + break; + } + + // Thực hiện công việc + doWork(); + + // Sleep một chút + robot::WallDuration(0.1).sleep(); +} +``` + +### 3. Logging với timestamp thực + +```cpp +void logMessage(const std::string& msg) +{ + robot::WallTime now = robot::WallTime::now(); + std::cout << "[" << now.toSec() << "] " << msg << std::endl; +} +``` + +### 4. Giao tiếp với hardware + +```cpp +// Giao tiếp với hardware cần timing chính xác +robot::WallTime last_update = robot::WallTime::now(); +robot::WallDuration update_interval(0.02); // 50Hz + +while (running) +{ + // Cập nhật hardware + updateHardware(); + + // Đợi đến lần cập nhật tiếp theo + robot::WallTime next_update = last_update + update_interval; + robot::WallTime::sleepUntil(next_update); + last_update = robot::WallTime::now(); +} +``` + +### 5. Performance monitoring + +```cpp +class PerformanceMonitor +{ +private: + robot::WallTime start_time_; + std::vector measurements_; + +public: + void start() + { + start_time_ = robot::WallTime::now(); + } + + void record() + { + robot::WallTime now = robot::WallTime::now(); + robot::WallDuration elapsed = now - start_time_; + measurements_.push_back(elapsed); + start_time_ = now; + } + + double getAverageTime() + { + if (measurements_.empty()) return 0.0; + + double total = 0.0; + for (const auto& d : measurements_) + { + total += d.toSec(); + } + return total / measurements_.size(); + } +}; +``` + +## Best Practices + +### 1. Sử dụng WallTime cho đo đạc thời gian thực + +```cpp +// ✅ ĐÚNG - Dùng WallTime để đo thời gian thực thi +robot::WallTime start = robot::WallTime::now(); +processData(); +robot::WallDuration elapsed = robot::WallTime::now() - start; + +// ❌ SAI - Dùng Time có thể không chính xác trong simulation +robot::Time start = robot::Time::now(); +processData(); +robot::Duration elapsed = robot::Time::now() - start; +``` + +### 2. Sử dụng Time cho ROS messages + +```cpp +// ✅ ĐÚNG - Dùng Time cho ROS messages +geometry_msgs::PoseStamped msg; +msg.header.stamp = robot::Time::now(); + +// ❌ SAI - Không nên dùng WallTime cho ROS messages +msg.header.stamp = robot::WallTime::now(); // Không tương thích +``` + +### 3. Kết hợp Time và WallTime + +```cpp +// Sử dụng Time cho ROS operations +robot::Time ros_time = robot::Time::now(); +publishMessage(msg); + +// Sử dụng WallTime cho performance monitoring +robot::WallTime wall_start = robot::WallTime::now(); +expensiveOperation(); +robot::WallDuration wall_elapsed = robot::WallTime::now() - wall_start; +std::cout << "Operation took " << wall_elapsed.toSec() << " seconds" << std::endl; +``` + +### 4. Timeout với WallTime + +```cpp +// ✅ ĐÚNG - Dùng WallTime cho timeout thực tế +robot::WallTime deadline = robot::WallTime::now() + robot::WallDuration(10.0); +while (robot::WallTime::now() < deadline) +{ + if (tryOperation()) + break; + robot::WallDuration(0.1).sleep(); +} +``` + +## Ví dụ hoàn chỉnh + +### Ví dụ 1: Đo thời gian thực thi của nhiều operations + +```cpp +#include +#include +#include + +void benchmarkOperations() +{ + std::vector operations = {"operation1", "operation2", "operation3"}; + + for (const auto& op_name : operations) + { + robot::WallTime start = robot::WallTime::now(); + + // Thực hiện operation (giả lập) + performOperation(op_name); + + robot::WallDuration elapsed = robot::WallTime::now() - start; + std::cout << op_name << " took " << elapsed.toSec() << " seconds" << std::endl; + } +} +``` + +### Ví dụ 2: Rate limiting với WallTime + +```cpp +class RateLimiter +{ +private: + robot::WallTime last_time_; + robot::WallDuration min_interval_; + +public: + RateLimiter(double min_rate_hz) + : min_interval_(1.0 / min_rate_hz) + , last_time_(robot::WallTime::now()) + {} + + bool canProceed() + { + robot::WallTime now = robot::WallTime::now(); + robot::WallDuration elapsed = now - last_time_; + + if (elapsed >= min_interval_) + { + last_time_ = now; + return true; + } + return false; + } +}; +``` + +### Ví dụ 3: Timeout handler + +```cpp +class TimeoutHandler +{ +private: + robot::WallTime start_time_; + robot::WallDuration timeout_; + +public: + TimeoutHandler(double timeout_seconds) + : timeout_(timeout_seconds) + , start_time_(robot::WallTime::now()) + {} + + bool isExpired() const + { + robot::WallTime now = robot::WallTime::now(); + return (now - start_time_) > timeout_; + } + + robot::WallDuration remaining() const + { + robot::WallTime now = robot::WallTime::now(); + robot::WallDuration elapsed = now - start_time_; + robot::WallDuration remaining = timeout_ - elapsed; + return (remaining > robot::WallDuration(0)) ? remaining : robot::WallDuration(0); + } + + void reset() + { + start_time_ = robot::WallTime::now(); + } +}; +``` + +## Tương thích với ROS + +`robot::WallTime` được thiết kế để tương thích với `ros::WallTime` trong ROS. Các API và behavior đều giống nhau: + +```cpp +// ROS +ros::WallTime t1 = ros::WallTime::now(); +ros::WallDuration d = ros::WallDuration(1.0); +ros::WallTime t2 = t1 + d; + +// robot_time (tương đương) +robot::WallTime t1 = robot::WallTime::now(); +robot::WallDuration d = robot::WallDuration(1.0); +robot::WallTime t2 = t1 + d; +``` + +## Lưu ý quan trọng + +1. **Không dùng WallTime cho ROS messages**: ROS messages yêu cầu `Time`, không phải `WallTime` + +2. **Precision**: `WallTime` có độ chính xác nanosecond, nhưng độ chính xác thực tế phụ thuộc vào hệ thống + +3. **Thread safety**: Tất cả operations của `WallTime` đều thread-safe + +4. **Performance**: `WallTime::now()` rất nhanh, có thể gọi thường xuyên mà không lo về performance + +5. **Cross-platform**: Hoạt động trên cả Linux và Windows + +## Tài liệu tham khảo + +- `README.md` - Tổng quan về thư viện robot_time +- `time.h` - Header file với API đầy đủ +- ROS Documentation: http://docs.ros.org/en/diamondback/api/rostime/html/classros_1_1WallTime.html + diff --git a/examples/walltime_example.cpp b/examples/walltime_example.cpp new file mode 100644 index 0000000..31fda28 --- /dev/null +++ b/examples/walltime_example.cpp @@ -0,0 +1,331 @@ +/********************************************************************* + * WallTime Usage Examples + * + * File này minh họa cách sử dụng WallTime trong các tình huống thực tế + *********************************************************************/ + +#include +#include +#include +#include + +// Ví dụ 1: Đo thời gian thực thi của một hàm +void example1_measureExecutionTime() +{ + std::cout << "\n=== Ví dụ 1: Đo thời gian thực thi ===" << std::endl; + + robot::WallTime start = robot::WallTime::now(); + + // Giả lập một công việc tốn thời gian + double sum = 0.0; + for (int i = 0; i < 1000000; ++i) + { + sum += std::sin(i); + } + + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration elapsed = end - start; + + std::cout << "Thời gian thực thi: " << elapsed.toSec() << " giây" << std::endl; + std::cout << "Thời gian thực thi: " << elapsed.toNSec() << " nanoseconds" << std::endl; +} + +// Ví dụ 2: Timeout với WallTime +void example2_timeout() +{ + std::cout << "\n=== Ví dụ 2: Timeout ===" << std::endl; + + robot::WallTime start = robot::WallTime::now(); + robot::WallDuration timeout(2.0); // 2 giây timeout + + int iterations = 0; + while (robot::WallTime::now() - start < timeout) + { + iterations++; + // Giả lập công việc + robot::WallDuration(0.1).sleep(); + } + + robot::WallDuration actual_elapsed = robot::WallTime::now() - start; + std::cout << "Số lần lặp: " << iterations << std::endl; + std::cout << "Thời gian thực tế: " << actual_elapsed.toSec() << " giây" << std::endl; +} + +// Ví dụ 3: So sánh Time và WallTime +void example3_compareTimeAndWallTime() +{ + std::cout << "\n=== Ví dụ 3: So sánh Time và WallTime ===" << std::endl; + + // Khởi tạo Time (cần thiết) + robot::Time::init(); + + robot::Time ros_time = robot::Time::now(); + robot::WallTime wall_time = robot::WallTime::now(); + + std::cout << "ROS Time: " << ros_time.toSec() << std::endl; + std::cout << "Wall Time: " << wall_time.toSec() << std::endl; + + // Trong trường hợp không có simulated time, chúng sẽ giống nhau + // Nhưng WallTime không cần init() +} + +// Ví dụ 4: Sleep until một thời điểm cụ thể +void example4_sleepUntil() +{ + std::cout << "\n=== Ví dụ 4: Sleep until ===" << std::endl; + + robot::WallTime start = robot::WallTime::now(); + robot::WallTime target = start + robot::WallDuration(1.5); // 1.5 giây sau + + std::cout << "Bắt đầu: " << start.toSec() << std::endl; + std::cout << "Mục tiêu: " << target.toSec() << std::endl; + + bool success = robot::WallTime::sleepUntil(target); + + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration actual = end - start; + + std::cout << "Kết thúc: " << end.toSec() << std::endl; + std::cout << "Thời gian thực tế: " << actual.toSec() << " giây" << std::endl; + std::cout << "Thành công: " << (success ? "Có" : "Không") << std::endl; +} + +// Ví dụ 5: Benchmark nhiều operations +void example5_benchmark() +{ + std::cout << "\n=== Ví dụ 5: Benchmark ===" << std::endl; + + std::vector operations = { + "Operation A", + "Operation B", + "Operation C" + }; + + for (const auto& op_name : operations) + { + robot::WallTime start = robot::WallTime::now(); + + // Giả lập công việc với thời gian khác nhau + if (op_name == "Operation A") + { + robot::WallDuration(0.1).sleep(); + } + else if (op_name == "Operation B") + { + robot::WallDuration(0.2).sleep(); + } + else + { + robot::WallDuration(0.15).sleep(); + } + + robot::WallDuration elapsed = robot::WallTime::now() - start; + std::cout << op_name << ": " << elapsed.toSec() << " giây" << std::endl; + } +} + +// Ví dụ 6: Rate limiting với WallTime +class RateLimiter +{ +private: + robot::WallTime last_time_; + robot::WallDuration min_interval_; + +public: + RateLimiter(double min_rate_hz) + : min_interval_(1.0 / min_rate_hz) + , last_time_(robot::WallTime::now()) + {} + + bool canProceed() + { + robot::WallTime now = robot::WallTime::now(); + robot::WallDuration elapsed = now - last_time_; + + if (elapsed >= min_interval_) + { + last_time_ = now; + return true; + } + return false; + } + + void wait() + { + robot::WallTime now = robot::WallTime::now(); + robot::WallDuration elapsed = now - last_time_; + + if (elapsed < min_interval_) + { + robot::WallDuration remaining = min_interval_ - elapsed; + remaining.sleep(); + last_time_ = robot::WallTime::now(); + } + else + { + last_time_ = now; + } + } +}; + +void example6_rateLimiter() +{ + std::cout << "\n=== Ví dụ 6: Rate Limiter ===" << std::endl; + + RateLimiter limiter(2.0); // Tối đa 2 lần/giây + + for (int i = 0; i < 5; ++i) + { + robot::WallTime start = robot::WallTime::now(); + limiter.wait(); + robot::WallTime end = robot::WallTime::now(); + + robot::WallDuration elapsed = end - start; + std::cout << "Lần " << (i+1) << ": " << elapsed.toSec() << " giây" << std::endl; + } +} + +// Ví dụ 7: Timeout handler +class TimeoutHandler +{ +private: + robot::WallTime start_time_; + robot::WallDuration timeout_; + +public: + TimeoutHandler(double timeout_seconds) + : timeout_(timeout_seconds) + , start_time_(robot::WallTime::now()) + {} + + bool isExpired() const + { + robot::WallTime now = robot::WallTime::now(); + return (now - start_time_) > timeout_; + } + + robot::WallDuration remaining() const + { + robot::WallTime now = robot::WallTime::now(); + robot::WallDuration elapsed = now - start_time_; + robot::WallDuration remaining = timeout_ - elapsed; + return (remaining > robot::WallDuration(0)) ? remaining : robot::WallDuration(0); + } + + void reset() + { + start_time_ = robot::WallTime::now(); + } +}; + +void example7_timeoutHandler() +{ + std::cout << "\n=== Ví dụ 7: Timeout Handler ===" << std::endl; + + TimeoutHandler handler(3.0); // 3 giây timeout + + int count = 0; + while (!handler.isExpired() && count < 10) + { + robot::WallDuration remaining = handler.remaining(); + std::cout << "Còn lại: " << remaining.toSec() << " giây" << std::endl; + + robot::WallDuration(0.5).sleep(); + count++; + } + + if (handler.isExpired()) + { + std::cout << "Timeout!" << std::endl; + } + else + { + std::cout << "Hoàn thành trước khi timeout" << std::endl; + } +} + +// Ví dụ 8: Phép toán với WallTime và WallDuration +void example8_arithmetic() +{ + std::cout << "\n=== Ví dụ 8: Phép toán ===" << std::endl; + + robot::WallTime t1 = robot::WallTime::now(); + robot::WallDuration d(5.0); // 5 giây + + // Cộng duration + robot::WallTime t2 = t1 + d; + std::cout << "t1: " << t1.toSec() << std::endl; + std::cout << "t2 = t1 + 5s: " << t2.toSec() << std::endl; + + // Trừ duration + robot::WallTime t3 = t2 - robot::WallDuration(2.0); + std::cout << "t3 = t2 - 2s: " << t3.toSec() << std::endl; + + // Trừ hai WallTime để được WallDuration + robot::WallDuration elapsed = t2 - t1; + std::cout << "t2 - t1 = " << elapsed.toSec() << " giây" << std::endl; + + // So sánh + std::cout << "t1 < t2: " << (t1 < t2) << std::endl; + std::cout << "t1 == t3: " << (t1 == t3) << std::endl; +} + +// Ví dụ 9: Chuyển đổi đơn vị +void example9_conversion() +{ + std::cout << "\n=== Ví dụ 9: Chuyển đổi đơn vị ===" << std::endl; + + // Tạo WallTime từ giây + robot::WallTime t1(1234567890.123456789); + std::cout << "Từ giây: " << t1.toSec() << std::endl; + std::cout << "Sang nanosecond: " << t1.toNSec() << std::endl; + + // Tạo từ giây và nanosecond + robot::WallTime t2(1234567890, 123456789); + std::cout << "Từ sec/nsec: " << t2.toSec() << std::endl; + + // Chuyển đổi + double seconds = t2.toSec(); + uint64_t nanoseconds = t2.toNSec(); + std::cout << "Giây: " << seconds << std::endl; + std::cout << "Nanosecond: " << nanoseconds << std::endl; +} + +// Ví dụ 10: Constants +void example10_constants() +{ + std::cout << "\n=== Ví dụ 10: Constants ===" << std::endl; + + std::cout << "MAX: " << robot::WallTime::MAX.toSec() << std::endl; + std::cout << "MIN: " << robot::WallTime::MIN.toSec() << std::endl; + std::cout << "ZERO: " << robot::WallTime::ZERO.toSec() << std::endl; + std::cout << "UNINITIALIZED: " << robot::WallTime::UNINITIALIZED.toSec() << std::endl; + + robot::WallTime t; + std::cout << "Default WallTime is zero: " << t.isZero() << std::endl; +} + +int main(int argc, char** argv) +{ + std::cout << "========================================" << std::endl; + std::cout << "WallTime Usage Examples" << std::endl; + std::cout << "========================================" << std::endl; + + example1_measureExecutionTime(); + example2_timeout(); + example3_compareTimeAndWallTime(); + example4_sleepUntil(); + example5_benchmark(); + example6_rateLimiter(); + example7_timeoutHandler(); + example8_arithmetic(); + example9_conversion(); + example10_constants(); + + std::cout << "\n========================================" << std::endl; + std::cout << "Tất cả ví dụ đã hoàn thành!" << std::endl; + std::cout << "========================================" << std::endl; + + return 0; +} + diff --git a/examples/walltimer_example.cpp b/examples/walltimer_example.cpp new file mode 100644 index 0000000..d0aa87e --- /dev/null +++ b/examples/walltimer_example.cpp @@ -0,0 +1,229 @@ +/********************************************************************* + * WallTimer Example + * + * Demonstrates various uses of robot::WallTimer + *********************************************************************/ + +#include +#include +#include +#include + +// Example 1: Simple periodic callback +void simpleCallback(const robot::WallTimerEvent& event) +{ + std::cout << "[Simple] Timer fired at wall time: " + << event.current_real.toSec() << std::endl; + std::cout << "[Simple] Time since last callback: " + << event.last_duration.toSec() << " seconds" << std::endl; +} + +// Example 2: Performance monitoring +class PerformanceMonitor +{ +public: + void start() + { + timer_ = std::make_unique( + robot::WallDuration(1.0), // Check every 1 second + [this](const robot::WallTimerEvent& event) { + this->monitor(event); + }, + false, // Repeating + true // Auto-start + ); + } + + void monitor(const robot::WallTimerEvent& event) + { + static int count = 0; + count++; + + double actual_interval = event.last_duration.toSec(); + double expected_interval = 1.0; + + std::cout << "[Monitor] Check #" << count + << " - Expected: " << expected_interval + << "s, Actual: " << actual_interval << "s"; + + if (actual_interval > expected_interval * 1.1) + { + std::cout << " (DRIFT DETECTED!)"; + } + std::cout << std::endl; + } + + void stop() + { + if (timer_) + { + timer_->stop(); + } + } + +private: + std::unique_ptr timer_; +}; + +// Example 3: One-shot delayed action +void delayedAction(const robot::WallTimerEvent& event) +{ + std::cout << "[Delayed] Action executed after delay!" << std::endl; + std::cout << "[Delayed] Executed at wall time: " + << event.current_real.toSec() << std::endl; +} + +// Example 4: Dynamic period adjustment +class DynamicTimer +{ +public: + void start() + { + timer_ = std::make_unique( + robot::WallDuration(1.0), // Start with 1 second + [this](const robot::WallTimerEvent& event) { + this->callback(event); + }, + false, // Repeating + true // Auto-start + ); + + // After 3 seconds, change period to 0.5 seconds + adjust_thread_ = std::thread([this]() { + robot::WallDuration(3.0).sleep(); + std::cout << "[Dynamic] Changing period to 0.5 seconds..." << std::endl; + timer_->setPeriod(robot::WallDuration(0.5), true); + }); + } + + void callback(const robot::WallTimerEvent& event) + { + static int count = 0; + count++; + std::cout << "[Dynamic] Callback #" << count + << " at " << event.current_real.toSec() << std::endl; + } + + void stop() + { + if (timer_) + { + timer_->stop(); + } + if (adjust_thread_.joinable()) + { + adjust_thread_.join(); + } + } + +private: + std::unique_ptr timer_; + std::thread adjust_thread_; +}; + +// Example 5: Timer with member function +class TimerClass +{ +public: + void startTimer() + { + timer_ = std::make_unique( + robot::WallDuration(0.5), // 2 Hz + &TimerClass::timerCallback, + this, + false, // Repeating + true // Auto-start + ); + } + + void timerCallback(const robot::WallTimerEvent& event) + { + static int count = 0; + count++; + std::cout << "[Class] Member function callback #" << count + << " at " << event.current_real.toSec() << std::endl; + } + + void stopTimer() + { + if (timer_) + { + timer_->stop(); + } + } + +private: + std::unique_ptr timer_; +}; + +int main() +{ + std::cout << "=== WallTimer Examples ===" << std::endl; + std::cout << std::endl; + + // Example 1: Simple timer + std::cout << "Example 1: Simple periodic callback" << std::endl; + { + robot::WallTimer timer( + robot::WallDuration(1.0), + simpleCallback, + false, // Repeating + true // Auto-start + ); + + robot::WallDuration(3.0).sleep(); // Run for 3 seconds + timer.stop(); + } + std::cout << std::endl; + + // Example 2: Performance monitoring + std::cout << "Example 2: Performance monitoring" << std::endl; + { + PerformanceMonitor monitor; + monitor.start(); + + robot::WallDuration(5.0).sleep(); // Run for 5 seconds + monitor.stop(); + } + std::cout << std::endl; + + // Example 3: One-shot timer + std::cout << "Example 3: One-shot delayed action" << std::endl; + { + robot::WallTimer timer( + robot::WallDuration(2.0), + delayedAction, + true, // One-shot + true // Auto-start + ); + + robot::WallDuration(3.0).sleep(); // Wait for it to fire + } + std::cout << std::endl; + + // Example 4: Dynamic period adjustment + std::cout << "Example 4: Dynamic period adjustment" << std::endl; + { + DynamicTimer dynamic; + dynamic.start(); + + robot::WallDuration(8.0).sleep(); // Run for 8 seconds + dynamic.stop(); + } + std::cout << std::endl; + + // Example 5: Member function callback + std::cout << "Example 5: Member function callback" << std::endl; + { + TimerClass timer_obj; + timer_obj.startTimer(); + + robot::WallDuration(3.0).sleep(); // Run for 3 seconds + timer_obj.stopTimer(); + } + std::cout << std::endl; + + std::cout << "All examples completed!" << std::endl; + return 0; +} + diff --git a/include/robot/wall_timer.h b/include/robot/wall_timer.h new file mode 100644 index 0000000..3eb9bde --- /dev/null +++ b/include/robot/wall_timer.h @@ -0,0 +1,289 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024 + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#ifndef ROBOT_WALL_TIMER_H +#define ROBOT_WALL_TIMER_H + +#include +#include "robot_time_decl.h" +#include +#include +#include +#include +#include +#include + +namespace robot +{ + +/** + * @brief Structure containing information about a wall timer event + * + * Similar to ros::WallTimerEvent, this structure is passed to wall timer callbacks + * and contains timing information about the current and previous timer events. + * Uses wall-clock time (not affected by simulated time). + */ +struct ROBOT_TIME_DECL WallTimerEvent +{ + /** + * @brief The time when the current callback was actually called (wall-clock time) + */ + WallTime current_real; + + /** + * @brief The time when the current callback was expected to be called (wall-clock time) + */ + WallTime current_expected; + + /** + * @brief The time when the previous callback was actually called (wall-clock time) + */ + WallTime last_real; + + /** + * @brief The time when the previous callback was expected to be called (wall-clock time) + */ + WallTime last_expected; + + /** + * @brief The time between the last two callbacks (wall-clock duration) + */ + WallDuration last_duration; + + /** + * @brief Default constructor + */ + WallTimerEvent() + : current_real(WallTime::now()) + , current_expected(WallTime::now()) + , last_real(WallTime()) + , last_expected(WallTime()) + , last_duration(WallDuration()) + {} +}; + +/** + * @class WallTimer + * @brief A class to call a callback function at a specified rate using wall-clock time + * + * This class is similar to ros::WallTimer. It creates a separate thread that + * calls a callback function at a specified period. The callback receives + * a WallTimerEvent structure containing timing information. + * + * Unlike Timer, WallTimer always uses wall-clock time and is not affected + * by simulated time. This makes it ideal for: + * - Performance monitoring + * - Real-time deadlines + * - Hardware interfaces + * - Profiling and benchmarking + * + * Example usage: + * @code + * void myCallback(const robot::WallTimerEvent& event) { + * // Do something periodically + * } + * + * robot::WallTimer timer(robot::WallDuration(1.0), myCallback); + * timer.start(); + * // ... later ... + * timer.stop(); + * @endcode + */ +class ROBOT_TIME_DECL WallTimer +{ +public: + /** + * @brief Callback function type for wall timer events + */ + using Callback = std::function; + + /** + * @brief Default constructor - creates an uninitialized timer + * Timer must be assigned or constructed with parameters before use + */ + WallTimer(); + + /** + * @brief Constructor + * @param period The period between timer callbacks (wall-clock duration) + * @param callback The callback function to call + * @param oneshot If true, the timer will only fire once. If false, it will fire repeatedly. + * @param autostart If true, the timer will start automatically. If false, you must call start(). + */ + WallTimer(const WallDuration& period, + const Callback& callback, + bool oneshot = false, + bool autostart = true); + + /** + * @brief Constructor with member function pointer + * @param period The period between timer callbacks (wall-clock duration) + * @param callback Member function pointer to call + * @param obj Object instance to call the member function on + * @param oneshot If true, the timer will only fire once. If false, it will fire repeatedly. + * @param autostart If true, the timer will start automatically. If false, you must call start(). + * + * Example: + * @code + * class MyClass { + * void callback(const robot::WallTimerEvent& event) { } + * }; + * MyClass obj; + * robot::WallTimer timer(robot::WallDuration(1.0), &MyClass::callback, &obj); + * @endcode + */ + template + WallTimer(const WallDuration& period, + void (T::*callback)(const WallTimerEvent&), + T* obj, + bool oneshot = false, + bool autostart = true) + : WallTimer(period, + [obj, callback](const WallTimerEvent& event) { (obj->*callback)(event); }, + oneshot, + autostart) + {} + + /** + * @brief Copy constructor + */ + WallTimer(const WallTimer& rhs); + + /** + * @brief Destructor - stops the timer and joins the thread + */ + ~WallTimer(); + + /** + * @brief Start the timer. Does nothing if the timer is already started. + */ + void start(); + + /** + * @brief Stop the timer. Once this call returns, no more callbacks will be called. + * Does nothing if the timer is already stopped. + */ + void stop(); + + /** + * @brief Set the period of this timer + * @param period The new period between timer callbacks + * @param reset Whether to reset the timer. If true, timer ignores elapsed time and next callback occurs at now()+period + */ + void setPeriod(const WallDuration& period, bool reset = true); + + /** + * @brief Check if the timer has been started + * @return True if the timer is running, false otherwise + */ + bool hasStarted() const; + + /** + * @brief Check if the timer is valid (has a callback) + * @return True if the timer is valid, false otherwise + */ + bool isValid() const; + + /** + * @brief Check if the timer has any pending events to call + * @return True if there are pending events, false otherwise + */ + bool hasPending() const; + + /** + * @brief Check if the timer is one-shot + * @return True if the timer is one-shot, false if it repeats + */ + bool isOneShot() const; + + /** + * @brief Set whether the timer is one-shot + * @param oneshot If true, the timer will only fire once + */ + void setOneShot(bool oneshot); + + /** + * @brief Get the timer period + * @return The period between timer callbacks + */ + WallDuration getPeriod() const; + + /** + * @brief Conversion to bool (for checking validity) + */ + operator void*() const; + + /** + * @brief Equality operator + */ + bool operator==(const WallTimer& rhs) const; + + /** + * @brief Inequality operator + */ + bool operator!=(const WallTimer& rhs) const; + + /** + * @brief Less-than operator (for ordering in containers) + */ + bool operator<(const WallTimer& rhs) const; + + // Non-copyable assignment + WallTimer& operator=(const WallTimer&) = delete; + + // Movable + WallTimer(WallTimer&& other) noexcept; + WallTimer& operator=(WallTimer&& other) noexcept; + +private: + /** + * @brief The timer thread function + */ + void timerThread(); + + WallDuration period_; ///< Period between callbacks + Callback callback_; ///< Callback function + bool oneshot_; ///< Whether timer is one-shot + std::atomic running_; ///< Whether timer is running + std::atomic should_stop_; ///< Flag to stop the timer thread + + std::thread thread_; ///< Timer thread + mutable std::mutex mutex_; ///< Mutex for thread safety + std::condition_variable cv_; ///< Condition variable for timing + + WallTime last_real_; ///< Last actual callback time + WallTime last_expected_; ///< Last expected callback time +}; + +} // namespace robot + +#endif // ROBOT_WALL_TIMER_H + diff --git a/include/robot/walltime.h b/include/robot/walltime.h new file mode 100644 index 0000000..53b4a12 --- /dev/null +++ b/include/robot/walltime.h @@ -0,0 +1,475 @@ +/********************************************************************* + * WallTime - Standalone Wall-clock Time Library + * + * Header-only library for wall-clock time operations. + * This is a simplified, standalone version of WallTime that can be + * used independently or as a convenience wrapper. + * + * Features: + * - Always uses real wall-clock time (not affected by simulated time) + * - No initialization required + * - Thread-safe + * - Cross-platform (Linux, Windows, macOS) + * - Nanosecond precision + * + * Usage: + * #include + * + * robot::WallTime now = robot::WallTime::now(); + * robot::WallDuration elapsed = robot::WallTime::now() - now; + *********************************************************************/ + +#ifndef ROBOT_WALLTIME_H +#define ROBOT_WALLTIME_H + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace robot +{ + +/** + * \brief WallDuration - Represents a time interval using wall-clock time + */ +class WallDuration +{ +public: + int32_t sec; // Seconds (can be negative) + int32_t nsec; // Nanoseconds (0-999999999) + + // Constructors + WallDuration() : sec(0), nsec(0) {} + + WallDuration(int32_t _sec, int32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalize(); + } + + explicit WallDuration(double t) + { + fromSec(t); + } + + // Normalize nanoseconds to [0, 999999999] range + void normalize() + { + int64_t sec64 = sec; + int64_t nsec64 = nsec; + + // Handle negative nanoseconds + while (nsec64 < 0) + { + nsec64 += 1000000000LL; + sec64 -= 1; + } + + // Handle overflow nanoseconds + while (nsec64 >= 1000000000LL) + { + nsec64 -= 1000000000LL; + sec64 += 1; + } + + // Check bounds + if (sec64 > std::numeric_limits::max() || + sec64 < std::numeric_limits::min()) + { + throw std::runtime_error("WallDuration out of range"); + } + + sec = static_cast(sec64); + nsec = static_cast(nsec64); + } + + // Convert to seconds (double) + double toSec() const + { + return static_cast(sec) + 1e-9 * static_cast(nsec); + } + + // Convert to nanoseconds + int64_t toNSec() const + { + return static_cast(sec) * 1000000000LL + static_cast(nsec); + } + + // Initialize from seconds + WallDuration& fromSec(double t) + { + sec = static_cast(std::floor(t)); + nsec = static_cast((t - sec) * 1e9); + normalize(); + return *this; + } + + // Initialize from nanoseconds + WallDuration& fromNSec(int64_t t) + { + sec = static_cast(t / 1000000000LL); + nsec = static_cast(t % 1000000000LL); + normalize(); + return *this; + } + + // Arithmetic operations + WallDuration operator+(const WallDuration& rhs) const + { + return WallDuration(sec + rhs.sec, nsec + rhs.nsec); + } + + WallDuration operator-(const WallDuration& rhs) const + { + return WallDuration(sec - rhs.sec, nsec - rhs.nsec); + } + + WallDuration operator-() const + { + return WallDuration(-sec, -nsec); + } + + WallDuration operator*(double scale) const + { + return WallDuration(toSec() * scale); + } + + WallDuration& operator+=(const WallDuration& rhs) + { + sec += rhs.sec; + nsec += rhs.nsec; + normalize(); + return *this; + } + + WallDuration& operator-=(const WallDuration& rhs) + { + sec -= rhs.sec; + nsec -= rhs.nsec; + normalize(); + return *this; + } + + // Comparison operators + bool operator==(const WallDuration& rhs) const + { + return sec == rhs.sec && nsec == rhs.nsec; + } + + bool operator!=(const WallDuration& rhs) const + { + return !(*this == rhs); + } + + bool operator<(const WallDuration& rhs) const + { + if (sec < rhs.sec) return true; + if (sec > rhs.sec) return false; + return nsec < rhs.nsec; + } + + bool operator>(const WallDuration& rhs) const + { + return rhs < *this; + } + + bool operator<=(const WallDuration& rhs) const + { + return !(rhs < *this); + } + + bool operator>=(const WallDuration& rhs) const + { + return !(*this < rhs); + } + + // Sleep for this duration + bool sleep() const + { + if (sec < 0) return false; + + using namespace std::chrono; + std::chrono::nanoseconds ns(toNSec()); + std::this_thread::sleep_for(ns); + return true; + } + + // Check if zero + bool isZero() const + { + return sec == 0 && nsec == 0; + } + + // Constants + static const WallDuration ZERO; + static const WallDuration MAX; + static const WallDuration MIN; +}; + +/** + * \brief WallTime - Represents a point in time using wall-clock time + */ +class WallTime +{ +public: + uint32_t sec; // Seconds since epoch + uint32_t nsec; // Nanoseconds (0-999999999) + + // Constructors + WallTime() : sec(0), nsec(0) {} + + WallTime(uint32_t _sec, uint32_t _nsec) : sec(_sec), nsec(_nsec) + { + normalize(); + } + + explicit WallTime(double t) + { + fromSec(t); + } + + // Normalize nanoseconds to [0, 999999999] range + void normalize() + { + uint64_t sec64 = sec; + uint64_t nsec64 = nsec; + + // Handle overflow nanoseconds + uint64_t sec_part = nsec64 / 1000000000ULL; + nsec64 = nsec64 % 1000000000ULL; + sec64 += sec_part; + + // Check bounds + if (sec64 > std::numeric_limits::max()) + { + throw std::runtime_error("WallTime out of range"); + } + + sec = static_cast(sec64); + nsec = static_cast(nsec64); + } + + // Get current wall-clock time + static WallTime now() + { + using namespace std::chrono; + auto now_time = system_clock::now(); + auto duration = now_time.time_since_epoch(); + auto nanoseconds_count = duration_cast(duration).count(); + + WallTime t; + uint64_t sec64 = nanoseconds_count / 1000000000ULL; + uint64_t nsec64 = nanoseconds_count % 1000000000ULL; + + if (sec64 > std::numeric_limits::max()) + { + throw std::runtime_error("WallTime::now() - time out of range"); + } + + t.sec = static_cast(sec64); + t.nsec = static_cast(nsec64); + + return t; + } + + // Convert to seconds (double) + double toSec() const + { + return static_cast(sec) + 1e-9 * static_cast(nsec); + } + + // Convert to nanoseconds + uint64_t toNSec() const + { + return static_cast(sec) * 1000000000ULL + static_cast(nsec); + } + + // Initialize from seconds + WallTime& fromSec(double t) + { + sec = static_cast(std::floor(t)); + nsec = static_cast((t - sec) * 1e9); + normalize(); + return *this; + } + + // Initialize from nanoseconds + WallTime& fromNSec(uint64_t t) + { + sec = static_cast(t / 1000000000ULL); + nsec = static_cast(t % 1000000000ULL); + return *this; + } + + // Arithmetic operations with WallDuration + WallTime operator+(const WallDuration& d) const + { + int64_t total_sec = static_cast(sec) + d.sec; + int64_t total_nsec = static_cast(nsec) + d.nsec; + + if (total_sec < 0) + { + throw std::runtime_error("WallTime::operator+ - result would be negative"); + } + + WallTime result; + result.sec = static_cast(total_sec); + result.nsec = static_cast(total_nsec); + result.normalize(); + return result; + } + + WallTime operator-(const WallDuration& d) const + { + int64_t total_sec = static_cast(sec) - d.sec; + int64_t total_nsec = static_cast(nsec) - d.nsec; + + if (total_sec < 0) + { + throw std::runtime_error("WallTime::operator- - result would be negative"); + } + + WallTime result; + result.sec = static_cast(total_sec); + result.nsec = static_cast(total_nsec); + result.normalize(); + return result; + } + + WallDuration operator-(const WallTime& rhs) const + { + int64_t sec_diff = static_cast(sec) - static_cast(rhs.sec); + int64_t nsec_diff = static_cast(nsec) - static_cast(rhs.nsec); + + return WallDuration(static_cast(sec_diff), static_cast(nsec_diff)); + } + + WallTime& operator+=(const WallDuration& d) + { + *this = *this + d; + return *this; + } + + WallTime& operator-=(const WallDuration& d) + { + *this = *this - d; + return *this; + } + + // Comparison operators + bool operator==(const WallTime& rhs) const + { + return sec == rhs.sec && nsec == rhs.nsec; + } + + bool operator!=(const WallTime& rhs) const + { + return !(*this == rhs); + } + + bool operator<(const WallTime& rhs) const + { + if (sec < rhs.sec) return true; + if (sec > rhs.sec) return false; + return nsec < rhs.nsec; + } + + bool operator>(const WallTime& rhs) const + { + return rhs < *this; + } + + bool operator<=(const WallTime& rhs) const + { + return !(rhs < *this); + } + + bool operator>=(const WallTime& rhs) const + { + return !(*this < rhs); + } + + // Sleep until this time + static bool sleepUntil(const WallTime& end) + { + WallTime now_time = now(); + if (end <= now_time) + { + return true; // Already past the target time + } + + WallDuration remaining = end - now_time; + return remaining.sleep(); + } + + // Check if zero + bool isZero() const + { + return sec == 0 && nsec == 0; + } + + // Check if this is system time (always true for WallTime) + static bool isSystemTime() + { + return true; + } + + // Constants + static const WallTime ZERO; + static const WallTime MAX; + static const WallTime MIN; +}; + +// Stream operators +inline std::ostream& operator<<(std::ostream& os, const WallTime& rhs) +{ + auto flags = os.flags(); + auto fillc = os.fill(); + auto width = os.width(); + os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec; + os.flags(flags); + os.fill(fillc); + os.width(width); + return os; +} + +inline std::ostream& operator<<(std::ostream& os, const WallDuration& rhs) +{ + auto flags = os.flags(); + auto fillc = os.fill(); + auto width = os.width(); + if (rhs.sec >= 0 || rhs.nsec == 0) + { + os << rhs.sec << "." << std::setw(9) << std::setfill('0') << rhs.nsec; + } + else + { + os << (rhs.sec == -1 ? "-" : "") << (rhs.sec + 1) << "." + << std::setw(9) << std::setfill('0') << (1000000000 - rhs.nsec); + } + os.flags(flags); + os.fill(fillc); + os.width(width); + return os; +} + +// Constants definitions (inline to avoid multiple definition) +inline const WallDuration WallDuration::ZERO(0, 0); +inline const WallDuration WallDuration::MAX( + std::numeric_limits::max(), 999999999); +inline const WallDuration WallDuration::MIN( + std::numeric_limits::min(), 0); + +inline const WallTime WallTime::ZERO(0, 0); +inline const WallTime WallTime::MAX( + std::numeric_limits::max(), 999999999); +inline const WallTime WallTime::MIN(0, 1); + +} // namespace robot + +#endif // ROBOT_WALLTIME_H + diff --git a/src/wall_timer.cpp b/src/wall_timer.cpp new file mode 100644 index 0000000..0a93d23 --- /dev/null +++ b/src/wall_timer.cpp @@ -0,0 +1,375 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024 + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +#include +#include +#include + +namespace robot +{ + +WallTimer::WallTimer() + : period_(WallDuration(1.0)) + , callback_(nullptr) + , oneshot_(false) + , running_(false) + , should_stop_(false) + , last_real_(WallTime()) + , last_expected_(WallTime()) +{ + // Default constructor - timer is not started +} + +WallTimer::WallTimer(const WallDuration& period, + const Callback& callback, + bool oneshot, + bool autostart) + : period_(period) + , callback_(callback) + , oneshot_(oneshot) + , running_(false) + , should_stop_(false) + , last_real_(WallTime()) + , last_expected_(WallTime()) +{ + if (autostart) + { + start(); + } +} + +WallTimer::WallTimer(const WallTimer& rhs) + : period_(rhs.period_) + , callback_(rhs.callback_) + , oneshot_(rhs.oneshot_) + , running_(false) + , should_stop_(false) + , last_real_(rhs.last_real_) + , last_expected_(rhs.last_expected_) +{ + // Copy constructor - timer is not started + // If rhs was running, we don't copy the running state +} + +WallTimer::~WallTimer() +{ + stop(); +} + +WallTimer::WallTimer(WallTimer&& other) noexcept + : period_(other.period_) + , callback_(std::move(other.callback_)) + , oneshot_(other.oneshot_) + , running_(false) + , should_stop_(false) + , last_real_(other.last_real_) + , last_expected_(other.last_expected_) +{ + // Note: We don't move a running thread because it references 'this' of the old object. + // If other was running, it will be stopped by its destructor. + // We only move the configuration, not the running state. + + // Reset other to safe state + other.running_ = false; + other.should_stop_ = false; + other.oneshot_ = false; + other.period_ = WallDuration(1.0); + other.last_real_ = WallTime(); + other.last_expected_ = WallTime(); +} + +WallTimer& WallTimer::operator=(WallTimer&& other) noexcept +{ + if (this != &other) + { + // Stop current timer if running + stop(); + + // Stop other timer if running (before moving, to ensure thread is properly stopped) + bool was_running = other.running_.load(); + if (was_running) + { + other.stop(); + } + + // Move configuration from other + period_ = other.period_; + callback_ = std::move(other.callback_); + oneshot_ = other.oneshot_; + last_real_ = other.last_real_; + last_expected_ = other.last_expected_; + + // Reset running state - we'll start fresh if needed + running_ = false; + should_stop_ = false; + + // If other was running (and had autostart), we need to start this timer + // (This handles the case: timer = robot::WallTimer(..., autostart=true)) + if (was_running) + { + start(); + } + + // Reset other to safe state + other.running_ = false; + other.should_stop_ = false; + other.oneshot_ = false; + other.period_ = WallDuration(1.0); + other.last_real_ = WallTime(); + other.last_expected_ = WallTime(); + } + return *this; +} + +void WallTimer::start() +{ + std::lock_guard lock(mutex_); + + if (running_) + { + return; // Already running + } + + should_stop_ = false; + running_ = true; + last_real_ = WallTime(); + last_expected_ = WallTime(); + + // Start the timer thread + thread_ = std::thread(&WallTimer::timerThread, this); +} + +void WallTimer::stop() +{ + { + std::lock_guard lock(mutex_); + + if (!running_) + { + return; // Not running + } + + should_stop_ = true; + running_ = false; + } + + // Notify the thread to wake up and check should_stop_ + cv_.notify_all(); + + // Wait for thread to finish + if (thread_.joinable()) + { + thread_.join(); + } +} + +void WallTimer::setPeriod(const WallDuration& period, bool reset) +{ + std::lock_guard lock(mutex_); + period_ = period; + + if (reset && running_) + { + // Reset timing - next callback will occur at now() + period + last_real_ = WallTime(); + last_expected_ = WallTime(); + } + + // Wake up the thread so it can use the new period + cv_.notify_all(); +} + +bool WallTimer::hasStarted() const +{ + std::lock_guard lock(mutex_); + return running_; +} + +bool WallTimer::isValid() const +{ + std::lock_guard lock(mutex_); + return callback_ != nullptr; +} + +bool WallTimer::hasPending() const +{ + std::lock_guard lock(mutex_); + // For simplicity, we consider a timer to have pending events if it's running + // In a more sophisticated implementation, we could track pending callbacks + return running_ && !should_stop_; +} + +bool WallTimer::isOneShot() const +{ + std::lock_guard lock(mutex_); + return oneshot_; +} + +void WallTimer::setOneShot(bool oneshot) +{ + std::lock_guard lock(mutex_); + oneshot_ = oneshot; +} + +WallDuration WallTimer::getPeriod() const +{ + std::lock_guard lock(mutex_); + return period_; +} + +WallTimer::operator void*() const +{ + return isValid() ? const_cast(this) : nullptr; +} + +bool WallTimer::operator==(const WallTimer& rhs) const +{ + // Compare by pointer address (same as ROS implementation) + return this == &rhs; +} + +bool WallTimer::operator!=(const WallTimer& rhs) const +{ + return !(*this == rhs); +} + +bool WallTimer::operator<(const WallTimer& rhs) const +{ + // Compare by pointer address (same as ROS implementation) + return this < &rhs; +} + +void WallTimer::timerThread() +{ + WallTime next_expected = WallTime::now() + period_; + last_expected_ = next_expected - period_; + + while (!should_stop_) + { + WallTime current_real = WallTime::now(); + WallTime current_expected = next_expected; + + // Calculate how long to sleep + WallDuration sleep_duration = current_expected - current_real; + + // If we're already past the expected time, don't sleep + if (sleep_duration <= WallDuration()) + { + // We're late, adjust next_expected + next_expected = current_real + period_; + } + else + { + // Sleep until the expected time + // Use a polling approach with small sleep intervals to allow for stop signal + WallTime sleep_until = current_expected; + WallDuration remaining = sleep_duration; + + while (remaining > WallDuration() && !should_stop_) + { + // Sleep in small chunks to allow checking should_stop_ + WallDuration sleep_chunk = remaining; + if (sleep_chunk.toSec() > 0.1) // Max 100ms chunks + { + sleep_chunk = WallDuration(0.1); + } + + sleep_chunk.sleep(); + + // Check if we should stop + if (should_stop_) + { + break; + } + + // Update remaining time + WallTime now = WallTime::now(); + remaining = sleep_until - now; + } + + // Check if we were woken up to stop + if (should_stop_) + { + break; + } + + // Update times after sleep + current_real = WallTime::now(); + current_expected = next_expected; + next_expected = current_expected + period_; + } + + // Create timer event + WallTimerEvent event; + event.current_real = current_real; + event.current_expected = current_expected; + event.last_real = last_real_; + event.last_expected = last_expected_; + + if (!last_real_.isZero()) + { + event.last_duration = current_real - last_real_; + } + else + { + event.last_duration = WallDuration(); + } + + // Update last times + last_real_ = current_real; + last_expected_ = current_expected; + + // Call the callback + if (callback_) + { + try + { + callback_(event); + } + catch (...) + { + // Swallow exceptions to prevent timer thread from crashing + // In production, you might want to log this + } + } + + // If one-shot, stop after first callback + if (oneshot_) + { + should_stop_ = true; + running_ = false; + break; + } + } +} + +} // namespace robot + diff --git a/test/walltime_standalone_test.cpp b/test/walltime_standalone_test.cpp new file mode 100644 index 0000000..0203a89 --- /dev/null +++ b/test/walltime_standalone_test.cpp @@ -0,0 +1,280 @@ +/********************************************************************* + * Standalone WallTime Library Test + * + * Test file for the header-only WallTime library + *********************************************************************/ + +#include +#include +#include +#include + +void test_walltime_basic() +{ + std::cout << "Testing basic WallTime operations..." << std::endl; + + // Test default constructor + robot::WallTime t1; + assert(t1.sec == 0); + assert(t1.nsec == 0); + assert(t1.isZero()); + + // Test constructor with values + robot::WallTime t2(1234567890, 123456789); + assert(t2.sec == 1234567890); + assert(t2.nsec == 123456789); + + // Test constructor from double + robot::WallTime t3(1234567890.123456789); + assert(std::abs(t3.toSec() - 1234567890.123456789) < 1e-6); + + std::cout << " ✓ Basic operations passed" << std::endl; +} + +void test_walltime_now() +{ + std::cout << "Testing WallTime::now()..." << std::endl; + + robot::WallTime t1 = robot::WallTime::now(); + robot::WallTime t2 = robot::WallTime::now(); + + // t2 should be >= t1 + assert(t2 >= t1); + + // Should be system time + assert(robot::WallTime::isSystemTime()); + + std::cout << " ✓ WallTime::now() passed" << std::endl; +} + +void test_wallduration_basic() +{ + std::cout << "Testing basic WallDuration operations..." << std::endl; + + // Test default constructor + robot::WallDuration d1; + assert(d1.sec == 0); + assert(d1.nsec == 0); + assert(d1.isZero()); + + // Test constructor with values + robot::WallDuration d2(5, 123456789); + assert(d2.sec == 5); + assert(d2.nsec == 123456789); + + // Test constructor from double + robot::WallDuration d3(5.123456789); + assert(std::abs(d3.toSec() - 5.123456789) < 1e-6); + + std::cout << " ✓ Basic operations passed" << std::endl; +} + +void test_wallduration_normalize() +{ + std::cout << "Testing WallDuration normalization..." << std::endl; + + // Test overflow nanoseconds + robot::WallDuration d1(0, 2000000000); + assert(d1.sec == 2); + assert(d1.nsec == 0); + + // Test negative nanoseconds + robot::WallDuration d2(5, -500000000); + assert(d2.sec == 4); + assert(d2.nsec == 500000000); + + std::cout << " ✓ Normalization passed" << std::endl; +} + +void test_arithmetic() +{ + std::cout << "Testing arithmetic operations..." << std::endl; + + robot::WallTime t1(1000, 0); + robot::WallDuration d1(5, 0); + + // Test addition + robot::WallTime t2 = t1 + d1; + assert(t2.sec == 1005); + assert(t2.nsec == 0); + + // Test subtraction + robot::WallTime t3 = t2 - d1; + assert(t3.sec == 1000); + assert(t3.nsec == 0); + + // Test duration subtraction + robot::WallDuration d2 = t2 - t1; + assert(d2.sec == 5); + assert(d2.nsec == 0); + + // Test compound assignment + robot::WallTime t4 = t1; + t4 += d1; + assert(t4.sec == 1005); + + t4 -= d1; + assert(t4.sec == 1000); + + std::cout << " ✓ Arithmetic operations passed" << std::endl; +} + +void test_comparison() +{ + std::cout << "Testing comparison operations..." << std::endl; + + robot::WallTime t1(1000, 0); + robot::WallTime t2(1000, 500000000); + robot::WallTime t3(2000, 0); + + // Test equality + assert(t1 == t1); + assert(t1 != t2); + + // Test less than + assert(t1 < t2); + assert(t1 < t3); + + // Test greater than + assert(t2 > t1); + assert(t3 > t1); + + // Test less than or equal + assert(t1 <= t1); + assert(t1 <= t2); + + // Test greater than or equal + assert(t2 >= t1); + assert(t2 >= t2); + + std::cout << " ✓ Comparison operations passed" << std::endl; +} + +void test_conversion() +{ + std::cout << "Testing conversion operations..." << std::endl; + + robot::WallTime t(1234567890, 123456789); + + // Test toSec + double seconds = t.toSec(); + assert(std::abs(seconds - 1234567890.123456789) < 1e-6); + + // Test toNSec + uint64_t nanoseconds = t.toNSec(); + assert(nanoseconds == 1234567890123456789ULL); + + // Test fromSec + robot::WallTime t2; + t2.fromSec(1234567890.123456789); + assert(std::abs(t2.toSec() - 1234567890.123456789) < 1e-6); + + // Test fromNSec + robot::WallTime t3; + t3.fromNSec(1234567890123456789ULL); + assert(t3.sec == 1234567890); + assert(t3.nsec == 123456789); + + std::cout << " ✓ Conversion operations passed" << std::endl; +} + +void test_sleep() +{ + std::cout << "Testing sleep operations..." << std::endl; + + robot::WallTime start = robot::WallTime::now(); + + // Test WallDuration::sleep() + robot::WallDuration sleep_duration(0.1); // 100ms + bool sleep_result = sleep_duration.sleep(); + assert(sleep_result); + + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration elapsed = end - start; + + // Should have slept at least 100ms + assert(elapsed.toSec() >= 0.09); // Allow some tolerance + + // Test WallTime::sleepUntil() + robot::WallTime target = robot::WallTime::now() + robot::WallDuration(0.1); + bool sleep_until_result = robot::WallTime::sleepUntil(target); + assert(sleep_until_result); + + robot::WallTime after = robot::WallTime::now(); + assert(after >= target); + + std::cout << " ✓ Sleep operations passed" << std::endl; +} + +void test_measurement() +{ + std::cout << "Testing time measurement..." << std::endl; + + robot::WallTime start = robot::WallTime::now(); + + // Simulate some work + double sum = 0.0; + for (int i = 0; i < 100000; ++i) + { + sum += i; + } + + robot::WallTime end = robot::WallTime::now(); + robot::WallDuration elapsed = end - start; + + // Should have taken some time + assert(elapsed.toSec() >= 0.0); + assert(elapsed.toSec() < 1.0); // Should be fast + + std::cout << " ✓ Time measurement passed (elapsed: " << elapsed.toSec() << "s)" << std::endl; +} + +void test_constants() +{ + std::cout << "Testing constants..." << std::endl; + + // Test WallTime constants + assert(robot::WallTime::ZERO.isZero()); + assert(robot::WallTime::MAX > robot::WallTime::ZERO); + assert(robot::WallTime::MIN < robot::WallTime::MAX); + + // Test WallDuration constants + assert(robot::WallDuration::ZERO.isZero()); + assert(robot::WallDuration::MAX > robot::WallDuration::ZERO); + assert(robot::WallDuration::MIN < robot::WallDuration::ZERO); + + std::cout << " ✓ Constants passed" << std::endl; +} + +int main() +{ + std::cout << "========================================" << std::endl; + std::cout << "WallTime Standalone Library Tests" << std::endl; + std::cout << "========================================" << std::endl; + + try + { + test_walltime_basic(); + test_walltime_now(); + test_wallduration_basic(); + test_wallduration_normalize(); + test_arithmetic(); + test_comparison(); + test_conversion(); + test_sleep(); + test_measurement(); + test_constants(); + + std::cout << "\n========================================" << std::endl; + std::cout << "All tests passed!" << std::endl; + std::cout << "========================================" << std::endl; + + return 0; + } + catch (const std::exception& e) + { + std::cerr << "Test failed with exception: " << e.what() << std::endl; + return 1; + } +} +