From 35b77e9fa25fe4473c9fb0215f902ea587347859 Mon Sep 17 00:00:00 2001 From: HiepLM Date: Sat, 10 Jan 2026 10:17:17 +0700 Subject: [PATCH] add timer --- CMakeLists.txt | 1 + README.md | 227 ++++++++++++++++++++++++++ TIMER_EVENT_EXPLANATION.md | 212 +++++++++++++++++++++++++ TIMER_USAGE.md | 217 +++++++++++++++++++++++++ include/robot/timer.h | 244 ++++++++++++++++++++++++++++ package.xml | 17 +- src/timer.cpp | 318 +++++++++++++++++++++++++++++++++++++ 7 files changed, 1230 insertions(+), 6 deletions(-) create mode 100644 README.md create mode 100644 TIMER_EVENT_EXPLANATION.md create mode 100644 TIMER_USAGE.md create mode 100644 include/robot/timer.h create mode 100644 src/timer.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index ec6b38c..a85355d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -51,6 +51,7 @@ add_library(${PROJECT_NAME} SHARED src/duration.cpp src/rate.cpp src/time.cpp + src/timer.cpp ) if(BUILDING_WITH_CATKIN) diff --git a/README.md b/README.md new file mode 100644 index 0000000..d2422c4 --- /dev/null +++ b/README.md @@ -0,0 +1,227 @@ +# robot_time + +## Tổng quan + +`robot_time` là một thư viện C++ cung cấp các công cụ quản lý thời gian cho các ứng dụng robot. Thư viện này được thiết kế để thay thế `ros::Time` trong ROS nhưng hoàn toàn độc lập, không phụ thuộc vào ROS hay bất kỳ framework nào khác. Đây là một giải pháp nhẹ và linh hoạt cho việc quản lý thời gian trong các hệ thống robot tự động. + +## Mục đích và phạm vi ứng dụng + +Thư viện `robot_time` được phát triển để giải quyết các nhu cầu quản lý thời gian trong các ứng dụng robot, bao gồm: + +- **Đồng bộ hóa thời gian**: Cung cấp một hệ thống thời gian nhất quán cho toàn bộ ứng dụng robot +- **Điều khiển tần suất**: Hỗ trợ chạy các vòng lặp điều khiển ở tần suất cố định (ví dụ: 10Hz, 20Hz, 50Hz) +- **Lập lịch tác vụ**: Cho phép thực thi các tác vụ định kỳ một cách chính xác +- **Đo lường hiệu suất**: Cung cấp công cụ để đo thời gian thực thi và độ trễ +- **Mô phỏng thời gian**: Hỗ trợ cả thời gian thực (wall-clock) và thời gian mô phỏng (simulated time) cho testing và simulation + +## Các thành phần chính + +### 1. Time - Quản lý điểm thời gian + +`Time` đại diện cho một điểm thời gian cụ thể trong hệ thống. Đây là lớp cơ bản nhất để làm việc với thời gian. + +**Đặc điểm:** +- Lưu trữ thời gian với độ chính xác nanosecond (1 phần tỷ giây) +- Hỗ trợ cả thời gian thực và thời gian mô phỏng +- Có thể so sánh, cộng trừ với Duration +- Thread-safe, an toàn khi sử dụng trong môi trường đa luồng +- Cross-platform, hoạt động trên cả Linux và Windows + +**Ứng dụng:** +- Đánh dấu thời điểm xảy ra sự kiện +- Ghi nhận timestamp cho dữ liệu sensor +- So sánh thời gian giữa các sự kiện +- Tính toán độ trễ và latency + +### 2. Duration - Quản lý khoảng thời gian + +`Duration` đại diện cho một khoảng thời gian, có thể là khoảng thời gian đã trôi qua hoặc khoảng thời gian cần chờ đợi. + +**Đặc điểm:** +- Có thể dương (tương lai) hoặc âm (quá khứ) +- Hỗ trợ các phép toán cơ bản: cộng, trừ, nhân, chia +- Có thể chuyển đổi giữa các đơn vị: giây, nanosecond +- Cung cấp các hằng số tiện ích: giây, phút, giờ, ngày +- Có thể sử dụng để sleep (tạm dừng chương trình) + +**Ứng dụng:** +- Tính toán thời gian thực thi của một tác vụ +- Đặt timeout cho các thao tác +- Điều chỉnh tốc độ của các vòng lặp +- Đo lường khoảng thời gian giữa các sự kiện + +### 3. Rate - Điều khiển tần suất vòng lặp + +`Rate` là công cụ để duy trì một vòng lặp chạy ở tần suất cố định. Đây là thành phần quan trọng cho các control loops trong robot. + +**Đặc điểm:** +- Tự động tính toán thời gian sleep để duy trì tần suất mong muốn +- Có thể tạo từ tần suất (Hz) hoặc khoảng thời gian (Duration) +- Theo dõi thời gian thực tế của mỗi chu kỳ +- Tự động điều chỉnh để bù trừ cho overhead của hệ thống + +**Ứng dụng:** +- Control loops với tần suất cố định (ví dụ: 20Hz cho điều khiển động cơ) +- Sensor data processing loops +- State estimation và filtering loops +- Real-time control systems + +**Cơ chế hoạt động:** +Rate tự động tính toán thời gian còn lại trong mỗi chu kỳ và sleep để đảm bảo vòng lặp chạy đúng tần suất. Nếu một chu kỳ mất quá nhiều thời gian, Rate sẽ bỏ qua sleep để không làm chậm hệ thống. + +### 4. Timer - Hẹn giờ định kỳ + +`Timer` tạo một thread riêng để thực thi một hàm callback định kỳ. Đây là công cụ mạnh mẽ cho các tác vụ bất đồng bộ. + +**Đặc điểm:** +- Chạy trong thread riêng, không chặn thread chính +- 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 (TimerEvent) +- Có thể start/stop động +- Tự động cleanup khi hủy + +**TimerEvent chứa thông tin:** +- Thời gian thực tế khi callback được gọi +- Thời gian mong đợi khi callback được gọi +- Thời gian của callback trước đó +- Khoảng thời gian giữa các callback (để phát hiện drift) + +**Ứng dụng:** +- Periodic sensor updates +- Background tasks +- Heartbeat và monitoring +- Scheduled maintenance tasks +- Event-driven programming + +**Ưu điểm:** +- Không chặn thread chính +- 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 + +`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. + +**Khác biệt với Rate:** +- Rate có thể sử dụng simulated time (hữu ích cho testing) +- WallRate luôn dùng thời gian thực, không bị ảnh hưởng bởi simulated time + +**Ứng dụng:** +- Các tác vụ cần chạy theo thời gian thực tuyệt đối +- Interface với hardware cần timing chính xác +- Logging và monitoring theo thời gian thực + +## Tính năng nổi bật + +### 1. Simulated Time Support + +Thư viện hỗ trợ cả simulated time và wall-clock time. Simulated time cho phép: +- Chạy simulation với tốc độ nhanh hơn hoặc chậm hơn thời gian thực +- Testing với các kịch bản thời gian cụ thể +- Debugging với khả năng điều khiển thời gian + +### 2. Thread Safety + +Tất cả các operations đều thread-safe, cho phép: +- Sử dụng an toàn trong multi-threaded applications +- Truy cập đồng thời từ nhiều thread +- Không cần mutex bổ sung cho các thao tác cơ bản + +### 3. High Precision + +Sử dụng nanosecond precision đảm bảo: +- Độ chính xác cao cho các ứng dụng real-time +- Khả năng đo lường chính xác các khoảng thời gian ngắn +- Phù hợp cho các hệ thống điều khiển tốc độ cao + +### 4. Cross-platform Compatibility + +Hỗ trợ đa nền tảng: +- Linux (sử dụng POSIX time functions) +- Windows (sử dụng Windows-specific timing APIs) +- Tự động phát hiện và sử dụng API phù hợp + +## Cấu trúc thư mục + +``` +robot_time/ +├── include/robot/ # Header files +│ ├── time.h # Time class +│ ├── duration.h # Duration class +│ ├── rate.h # Rate và WallRate classes +│ ├── timer.h # Timer class +│ └── exception.h # Exception classes +├── src/ # Implementation files +│ ├── time.cpp +│ ├── duration.cpp +│ ├── rate.cpp +│ └── timer.cpp +├── test/ # Unit tests +├── CMakeLists.txt # Build configuration +├── package.xml # Package metadata +├── README.md # Tài liệu này +``` + +## Dependencies + +- **C++17**: Yêu cầu C++ standard 17 trở lên +- **GTest**: Chỉ cần khi build unit tests (optional) +- **Platform libraries**: + - Linux: POSIX time functions + - Windows: Windows timing APIs + +## Build và Installation + +Thư viện hỗ trợ cả Catkin và Standalone CMake: + +- **Với Catkin**: Tích hợp vào catkin workspace và build như các ROS packages khác +- **Với Standalone CMake**: Có thể build độc lập, không cần ROS + +## Use Cases + +### Control Loops +Sử dụng `Rate` để duy trì control loops ở tần suất cố định, đảm bảo robot phản ứng kịp thời và nhất quán. + +### Sensor Processing +Sử dụng `Timer` để xử lý dữ liệu sensor định kỳ trong background thread, không làm chậm main control loop. + +### Time Synchronization +Sử dụng `Time` để đồng bộ hóa dữ liệu từ nhiều nguồn sensor khác nhau, đảm bảo tất cả dữ liệu có cùng timestamp. + +### Performance Monitoring +Sử dụng `Duration` để đo thời gian thực thi của các thuật toán, giúp tối ưu hóa hiệu suất. + +### Simulation và Testing +Sử dụng simulated time để chạy các test cases với timing cụ thể, cho phép reproduce bugs và test edge cases. + +## Best Practices + +1. **Sử dụng Rate cho control loops**: Đảm bảo tần suất ổn định và có thể dự đoán được + +2. **Sử dụng Timer cho background tasks**: Tránh chặn main thread với các tác vụ định kỳ + +3. **Kiểm tra timing drift**: Monitor sự khác biệt giữa expected time và actual time để phát hiện vấn đề + +4. **Sử dụng Duration constants**: Sử dụng các hằng số có sẵn (SECOND, MINUTE, etc.) thay vì magic numbers + +5. **Thread safety**: Mặc dù các operations đều thread-safe, cần cẩn thận khi chia sẻ dữ liệu giữa các thread + +## Lưu ý quan trọng + +- **Initialization**: Cần khởi tạo hệ thống (gọi `robot::init()` hoặc tạo `robot::NodeHandle`) trước khi sử dụng `Time::now()` + +- **Timer overhead**: Mỗi Timer tạo một thread riêng, cần quản lý số lượng timer hợp lý để tránh overhead + +- **Rate accuracy**: `Rate::sleep()` có thể không chính xác 100% do overhead của system calls, nhưng thường đủ cho hầu hết các ứng dụng + +- **Simulated time**: Khi sử dụng simulated time, tất cả các Time và Duration operations sẽ sử dụng simulated time, trừ WallRate + +## Tài liệu tham khảo + +- `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 + +## License + +BSD License - Xem file LICENSE trong thư mục gốc của project. diff --git a/TIMER_EVENT_EXPLANATION.md b/TIMER_EVENT_EXPLANATION.md new file mode 100644 index 0000000..950c233 --- /dev/null +++ b/TIMER_EVENT_EXPLANATION.md @@ -0,0 +1,212 @@ +# TimerEvent - Giải thích và Cách sử dụng + +## TimerEvent là gì? + +`TimerEvent` là một structure chứa thông tin về timing của timer callback. Nó được truyền vào mỗi lần callback được gọi, giúp bạn biết: +- Timer có chạy đúng thời gian không? +- Timer có bị delay không? +- Khoảng thời gian thực tế giữa các lần callback + +## Các thành phần của TimerEvent + +```cpp +struct TimerEvent +{ + Time current_real; // Thời gian thực tế khi callback được gọi + Time current_expected; // Thời gian dự kiến khi callback nên được gọi + Time last_real; // Thời gian thực tế của lần callback trước + Time last_expected; // Thời gian dự kiến của lần callback trước + Duration last_duration; // Khoảng thời gian thực tế giữa 2 lần callback +}; +``` + +## Tại sao cần TimerEvent? + +### 1. **Phát hiện Timer Drift (Lệch thời gian)** + +Timer có thể bị lệch do: +- Hệ thống bận +- Thread scheduling +- Callback mất nhiều thời gian + +```cpp +void myCallback(const robot::TimerEvent& event) +{ + // Tính độ lệch thời gian + robot::Duration drift = event.current_real - event.current_expected; + + if (drift.toSec() > 0.1) // Lệch hơn 100ms + { + std::cout << "Warning: Timer is " << drift.toSec() << " seconds late!" << std::endl; + } +} +``` + +### 2. **Đo thời gian thực tế giữa các callback** + +Không phải lúc nào timer cũng chạy đúng period. Bạn có thể đo thời gian thực tế: + +```cpp +void myCallback(const robot::TimerEvent& event) +{ + // Thời gian thực tế giữa 2 lần callback + double actual_period = event.last_duration.toSec(); + double expected_period = 1.0; // Period bạn set + + if (actual_period > expected_period * 1.1) // Lệch hơn 10% + { + std::cout << "Timer is running slow! Expected: " << expected_period + << ", Actual: " << actual_period << std::endl; + } +} +``` + +### 3. **Tính toán thời gian xử lý** + +Bạn có thể biết callback mất bao lâu: + +```cpp +robot::Time callback_start; + +void myCallback(const robot::TimerEvent& event) +{ + callback_start = robot::Time::now(); + + // Do some work... + doSomeWork(); + + robot::Duration processing_time = robot::Time::now() - callback_start; + robot::Duration time_until_next = event.current_expected + period - robot::Time::now(); + + if (processing_time > time_until_next) + { + std::cout << "Warning: Callback takes longer than period!" << std::endl; + } +} +``` + +### 4. **Điều chỉnh hành vi dựa trên timing** + +Ví dụ: Nếu timer bị delay, có thể bỏ qua một số công việc: + +```cpp +void myCallback(const robot::TimerEvent& event) +{ + robot::Duration drift = event.current_real - event.current_expected; + + if (drift.toSec() > 0.5) // Delay quá nhiều + { + // Bỏ qua công việc không quan trọng + return; + } + + // Thực hiện công việc bình thường + doImportantWork(); +} +``` + +### 5. **Logging và Debugging** + +TimerEvent rất hữu ích để debug: + +```cpp +void myCallback(const robot::TimerEvent& event) +{ + static int count = 0; + count++; + + if (count % 100 == 0) // Log mỗi 100 lần + { + robot::Duration drift = event.current_real - event.current_expected; + double avg_period = event.last_duration.toSec(); + + std::cout << "Timer Stats:" << std::endl; + std::cout << " Callback #" << count << std::endl; + std::cout << " Current drift: " << drift.toSec() << " seconds" << std::endl; + std::cout << " Average period: " << avg_period << " seconds" << std::endl; + std::cout << " Expected period: " << period.toSec() << " seconds" << std::endl; + } +} +``` + +## Ví dụ thực tế trong move_base + +Trong code của bạn, `wakePlanner` có thể sử dụng TimerEvent để: + +```cpp +void move_base::MoveBase::wakePlanner(const robot::TimerEvent& event) +{ + // Kiểm tra xem timer có chạy đúng không + robot::Duration drift = event.current_real - event.current_expected; + + if (drift.toSec() > 0.05) // Lệch hơn 50ms + { + robot::log_warning("Planner timer is running late by %.3f seconds", drift.toSec()); + } + + // Thời gian thực tế giữa 2 lần wake + double actual_period = event.last_duration.toSec(); + double expected_period = 1.0 / planner_frequency_; + + if (actual_period > expected_period * 1.2) // Lệch hơn 20% + { + robot::log_warning("Planner frequency is lower than expected. " + "Expected: %.2f Hz, Actual: %.2f Hz", + planner_frequency_, 1.0 / actual_period); + } + + // Wake up planner thread + planner_cond_.notify_one(); +} +``` + +## Khi nào nên sử dụng TimerEvent? + +### Nên dùng khi: +- ✅ Cần monitor timer performance +- ✅ Cần phát hiện timer drift +- ✅ Cần điều chỉnh hành vi dựa trên timing +- ✅ Debug timing issues +- ✅ Tính toán thời gian xử lý + +### Không cần dùng khi: +- ❌ Callback đơn giản, không quan tâm timing +- ❌ Chỉ cần biết timer đã fire +- ❌ Không cần thông tin về drift + +## Ví dụ đơn giản + +```cpp +// Callback đơn giản - không dùng TimerEvent +void simpleCallback(const robot::TimerEvent& event) +{ + // Không cần dùng event, chỉ cần biết timer đã fire + doSomething(); +} + +// Callback phức tạp - sử dụng TimerEvent +void advancedCallback(const robot::TimerEvent& event) +{ + // Sử dụng event để monitor + double drift = (event.current_real - event.current_expected).toSec(); + + if (std::abs(drift) > 0.01) // Lệch hơn 10ms + { + adjustBehavior(drift); + } + + doSomething(); +} +``` + +## Tóm tắt + +`TimerEvent` cung cấp thông tin timing chi tiết giúp bạn: +1. **Monitor** timer performance +2. **Phát hiện** timing issues +3. **Điều chỉnh** hành vi dựa trên timing +4. **Debug** timing problems +5. **Tối ưu** performance + +Trong hầu hết trường hợp đơn giản, bạn có thể bỏ qua TimerEvent. Nhưng khi cần monitor hoặc debug timing, nó rất hữu ích! + diff --git a/TIMER_USAGE.md b/TIMER_USAGE.md new file mode 100644 index 0000000..f1e4be6 --- /dev/null +++ b/TIMER_USAGE.md @@ -0,0 +1,217 @@ +# robot::Timer Usage Guide + +## Overview + +`robot::Timer` is a class similar to `ros::Timer` that allows you to call a callback function at a specified rate. It creates a separate thread that periodically invokes your callback function. + +## Basic Usage + +### Simple Timer Example + +```cpp +#include +#include + +void myCallback(const robot::TimerEvent& event) +{ + std::cout << "Timer fired! Current 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 + robot::Timer timer( + robot::Duration(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::Duration(5.0).sleep(); + + // Stop the timer + timer.stop(); + + return 0; +} +``` + +### Using with Class Methods + +```cpp +#include + +class MyClass +{ +public: + void startTimer() + { + // Create timer with member function callback + timer_ = std::make_unique( + robot::Duration(0.5), // 2 Hz + [this](const robot::TimerEvent& event) { + this->timerCallback(event); + }, + false, // Repeating + true // Auto-start + ); + } + + void timerCallback(const robot::TimerEvent& event) + { + // Do periodic work here + std::cout << "Periodic task executed" << std::endl; + } + + void stopTimer() + { + if (timer_) + { + timer_->stop(); + } + } + +private: + std::unique_ptr timer_; +}; +``` + +### One-Shot Timer + +```cpp +#include + +void delayedAction(const robot::TimerEvent& event) +{ + std::cout << "Delayed action executed after 5 seconds" << std::endl; +} + +int main() +{ + // Create a one-shot timer that fires once after 5 seconds + robot::Timer timer( + robot::Duration(5.0), // Wait 5 seconds + delayedAction, // Callback + true, // One-shot + true // Auto-start + ); + + // Wait for timer to fire + robot::Duration(6.0).sleep(); + + return 0; +} +``` + +## TimerEvent Structure + +The `TimerEvent` structure passed to your callback contains: + +- `current_real`: The actual time when the callback was called +- `current_expected`: The expected time when the callback should have been called +- `last_real`: The actual time of the previous callback +- `last_expected`: The expected time of the previous callback +- `last_duration`: The duration between the last two callbacks + +## API Reference + +### Constructor + +```cpp +Timer(const Duration& period, + const Callback& callback, + bool oneshot = false, + bool autostart = true); +``` + +- `period`: Time between callbacks +- `callback`: Function to call (signature: `void(const TimerEvent&)`) +- `oneshot`: If true, timer fires only once +- `autostart`: If true, timer starts automatically + +### Methods + +- `void start()`: Start the timer +- `void stop()`: Stop the timer +- `bool hasStarted()`: Check if timer is running +- `bool isOneShot()`: Check if timer is one-shot +- `void setOneShot(bool oneshot)`: Set one-shot mode +- `Duration getPeriod()`: Get the timer period +- `void setPeriod(const Duration& period)`: Set the timer period + +## Example: Using in move_base + +Here's how you could use `robot::Timer` in the `planThread` function: + +```cpp +#include + +void move_base::MoveBase::planThread() +{ + boost::unique_lock lock(planner_mutex_); + + while (true) + { + // Wait until we should run the planner + while (!runPlanner_) + { + planner_cond_.wait(lock); + } + + lock.unlock(); + + // Do planning work + robot_geometry_msgs::PoseStamped temp_goal = planner_goal_; + planner_plan_->clear(); + bool gotPlan = makePlan(temp_goal, *planner_plan_); + + // ... handle plan result ... + + // If planner_frequency > 0, use timer to wake up later + if (planner_frequency_ > 0) + { + robot::Duration sleep_time = robot::Duration(1.0 / planner_frequency_); + + // Create one-shot timer to wake up planner + robot::Timer wake_timer( + sleep_time, + [this](const robot::TimerEvent&) { + boost::unique_lock lock(planner_mutex_); + planner_cond_.notify_one(); + }, + true, // One-shot + true // Auto-start + ); + + // Wait for timer or condition variable + lock.lock(); + planner_cond_.wait(lock); + } + else + { + lock.lock(); + } + } +} +``` + +## Thread Safety + +- `Timer` is thread-safe and can be started/stopped from any thread +- The callback function is executed in a separate thread +- Be careful when accessing shared data from the callback - use proper synchronization + +## Notes + +- The timer uses `robot::Time` which respects simulation time if enabled +- Timer accuracy depends on system scheduling and may drift slightly +- For very high frequency timers (>100 Hz), consider using `robot::Rate` in a loop instead +- The timer thread will automatically clean up when the `Timer` object is destroyed + diff --git a/include/robot/timer.h b/include/robot/timer.h new file mode 100644 index 0000000..59da1af --- /dev/null +++ b/include/robot/timer.h @@ -0,0 +1,244 @@ +/********************************************************************* + * 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_TIMER_H +#define ROBOT_TIMER_H + +#include +#include +#include "robot_time_decl.h" +#include +#include +#include +#include +#include +#include + +namespace robot +{ + +/** + * @brief Structure containing information about a timer event + * + * Similar to ros::TimerEvent, this structure is passed to timer callbacks + * and contains timing information about the current and previous timer events. + */ +struct ROBOT_TIME_DECL TimerEvent +{ + /** + * @brief The time when the current callback was actually called + */ + Time current_real; + + /** + * @brief The time when the current callback was expected to be called + */ + Time current_expected; + + /** + * @brief The time when the previous callback was actually called + */ + Time last_real; + + /** + * @brief The time when the previous callback was expected to be called + */ + Time last_expected; + + /** + * @brief The time between the last two callbacks + */ + Duration last_duration; + + /** + * @brief Default constructor + */ + TimerEvent() + : current_real(Time::now()) + , current_expected(Time::now()) + , last_real(Time::ZERO) + , last_expected(Time::ZERO) + , last_duration(Duration::ZERO) + {} +}; + +/** + * @class Timer + * @brief A class to call a callback function at a specified rate + * + * This class is similar to ros::Timer. It creates a separate thread that + * calls a callback function at a specified period. The callback receives + * a TimerEvent structure containing timing information. + * + * Example usage: + * @code + * void myCallback(const robot::TimerEvent& event) { + * // Do something periodically + * } + * + * robot::Timer timer(robot::Duration(1.0), myCallback); + * timer.start(); + * // ... later ... + * timer.stop(); + * @endcode + */ +class ROBOT_TIME_DECL Timer +{ +public: + /** + * @brief Callback function type for timer events + */ + using Callback = std::function; + + /** + * @brief Default constructor - creates an uninitialized timer + * Timer must be assigned or constructed with parameters before use + */ + Timer(); + + /** + * @brief Constructor + * @param period The period between timer callbacks + * @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(). + */ + Timer(const Duration& period, + const Callback& callback, + bool oneshot = false, + bool autostart = true); + + /** + * @brief Constructor with member function pointer + * @param period The period between timer callbacks + * @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::TimerEvent& event) { } + * }; + * MyClass obj; + * robot::Timer timer(robot::Duration(1.0), &MyClass::callback, &obj); + * @endcode + */ + template + Timer(const Duration& period, + void (T::*callback)(const TimerEvent&), + T* obj, + bool oneshot = false, + bool autostart = true) + : Timer(period, + [obj, callback](const TimerEvent& event) { (obj->*callback)(event); }, + oneshot, + autostart) + {} + + /** + * @brief Destructor - stops the timer and joins the thread + */ + ~Timer(); + + /** + * @brief Start the timer + */ + void start(); + + /** + * @brief Stop the timer + */ + void stop(); + + /** + * @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 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 + */ + Duration getPeriod() const; + + /** + * @brief Set the timer period + * @param period The new period between timer callbacks + */ + void setPeriod(const Duration& period); + + // Non-copyable + Timer(const Timer&) = delete; + Timer& operator=(const Timer&) = delete; + + // Movable + Timer(Timer&& other) noexcept; + Timer& operator=(Timer&& other) noexcept; + +private: + /** + * @brief The timer thread function + */ + void timerThread(); + + Duration 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 + + Time last_real_; ///< Last actual callback time + Time last_expected_; ///< Last expected callback time +}; + +} // namespace robot + +#endif // ROBOT_TIMER_H + diff --git a/package.xml b/package.xml index 4a733d5..d922279 100644 --- a/package.xml +++ b/package.xml @@ -2,12 +2,17 @@ robot_time 0.7.10 - robot_time is the second generation of the transform library, which lets - the user keep track of multiple coordinate frames over time. robot_time - maintains the relationship between coordinate frames in a tree - structure buffered in time, and lets the user transform points, - vectors, etc between any two coordinate frames at any desired - point in time. + robot_time provides time utilities for robot applications, similar to ros::Time + but independent of ROS. It includes: + - Time: Represents a point in time with nanosecond precision + - Duration: Represents a time interval (can be positive or negative) + - Rate: Helps run loops at a desired frequency (Hz) + - Timer: Calls a callback function periodically in a separate thread + - WallRate: Rate using wall-clock time instead of simulated time + + The library supports both simulated time and wall-clock time, and is designed + for use in control loops, event-driven systems, and multi-threaded applications. + All operations are thread-safe and cross-platform (Linux/Windows). Tully Foote Eitan Marder-Eppstein diff --git a/src/timer.cpp b/src/timer.cpp new file mode 100644 index 0000000..c8f24af --- /dev/null +++ b/src/timer.cpp @@ -0,0 +1,318 @@ +/********************************************************************* + * 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 +{ + +Timer::Timer() + : period_(Duration(1.0)) + , callback_(nullptr) + , oneshot_(false) + , running_(false) + , should_stop_(false) + , last_real_(Time::ZERO) + , last_expected_(Time::ZERO) +{ + // Default constructor - timer is not started +} + +Timer::Timer(const Duration& period, + const Callback& callback, + bool oneshot, + bool autostart) + : period_(period) + , callback_(callback) + , oneshot_(oneshot) + , running_(false) + , should_stop_(false) + , last_real_(Time::ZERO) + , last_expected_(Time::ZERO) +{ + if (autostart) + { + start(); + } +} + +Timer::~Timer() +{ + stop(); +} + +Timer::Timer(Timer&& 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_ = Duration(1.0); + other.last_real_ = Time::ZERO; + other.last_expected_ = Time::ZERO; +} + +Timer& Timer::operator=(Timer&& 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::Timer(..., autostart=true)) + if (was_running) + { + start(); + } + + // Reset other to safe state + other.running_ = false; + other.should_stop_ = false; + other.oneshot_ = false; + other.period_ = Duration(1.0); + other.last_real_ = Time::ZERO; + other.last_expected_ = Time::ZERO; + } + return *this; +} + +void Timer::start() +{ + std::lock_guard lock(mutex_); + + if (running_) + { + return; // Already running + } + + should_stop_ = false; + running_ = true; + last_real_ = Time::ZERO; + last_expected_ = Time::ZERO; + + // Start the timer thread + thread_ = std::thread(&Timer::timerThread, this); +} + +void Timer::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(); + } +} + +bool Timer::hasStarted() const +{ + std::lock_guard lock(mutex_); + return running_; +} + +bool Timer::isOneShot() const +{ + std::lock_guard lock(mutex_); + return oneshot_; +} + +void Timer::setOneShot(bool oneshot) +{ + std::lock_guard lock(mutex_); + oneshot_ = oneshot; +} + +Duration Timer::getPeriod() const +{ + std::lock_guard lock(mutex_); + return period_; +} + +void Timer::setPeriod(const Duration& period) +{ + std::lock_guard lock(mutex_); + period_ = period; + // Wake up the thread so it can use the new period + cv_.notify_all(); +} + +void Timer::timerThread() +{ + Time next_expected = Time::now() + period_; + last_expected_ = next_expected - period_; + + while (!should_stop_) + { + Time current_real = Time::now(); + Time current_expected = next_expected; + + // Calculate how long to sleep + Duration sleep_duration = current_expected - current_real; + + // If we're already past the expected time, don't sleep + if (sleep_duration <= Duration::ZERO) + { + // 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 + Time sleep_until = current_expected; + Duration remaining = sleep_duration; + + while (remaining > Duration::ZERO && !should_stop_) + { + // Sleep in small chunks to allow checking should_stop_ + Duration sleep_chunk = remaining; + if (sleep_chunk.toSec() > 0.1) // Max 100ms chunks + { + sleep_chunk = Duration(0.1); + } + + sleep_chunk.sleep(); + + // Check if we should stop + if (should_stop_) + { + break; + } + + // Update remaining time + Time now = Time::now(); + remaining = sleep_until - now; + } + + // Check if we were woken up to stop + if (should_stop_) + { + break; + } + + // Update times after sleep + current_real = Time::now(); + current_expected = next_expected; + next_expected = current_expected + period_; + } + + // Create timer event + TimerEvent 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 = Duration::ZERO; + } + + // 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 +