Compare commits

...

4 Commits

Author SHA1 Message Date
0c007fdab3 fix bug 2026-03-11 14:56:40 +07:00
750dc94c61 add WallTime và WallTimer 2026-01-13 14:29:16 +07:00
35b77e9fa2 add timer 2026-01-10 10:17:17 +07:00
6b23f32c6e update for ROS 2026-01-07 16:55:09 +07:00
17 changed files with 4572 additions and 71 deletions

View File

@@ -1,99 +1,163 @@
cmake_minimum_required(VERSION 3.10.2)
project(robot_time)
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# Cấu hình RPATH để tránh cycle trong runtime search path
set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
# ========================================================
# Find Packages
# ========================================================
cmake_minimum_required(VERSION 3.0.2)
project(robot_time VERSION 1.0.0 LANGUAGES CXX)
if(DEFINED CATKIN_DEVEL_PREFIX OR DEFINED CATKIN_TOPLEVEL)
set(BUILDING_WITH_CATKIN TRUE)
message(STATUS "Building robot_time with Catkin")
else()
set(BUILDING_WITH_CATKIN FALSE)
message(STATUS "Building robot_time with Standalone CMake")
endif()
if(BUILDING_WITH_CATKIN)
find_package(catkin REQUIRED)
endif()
# C++ Standard - must be set before find_package
set(CMAKE_CXX_STANDARD 17)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
# Find dependencies
find_package(GTest REQUIRED)
if (NOT BUILDING_WITH_CATKIN)
# Enable Position Independent Code
set(CMAKE_POSITION_INDEPENDENT_CODE ON)
# Cấu hình RPATH để tránh cycle trong runtime search path
set(CMAKE_BUILD_RPATH_USE_ORIGIN TRUE)
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
set(CMAKE_BUILD_RPATH "${CMAKE_BINARY_DIR}")
else()
# ========================================================
# Catkin specific configuration
# ========================================================
find_package(catkin REQUIRED)
if(BUILDING_WITH_CATKIN)
## The catkin_package macro generates cmake config files for your package
catkin_package(
INCLUDE_DIRS include
LIBRARIES robot_time
LIBRARIES ${PROJECT_NAME}
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
endif()
include_directories(${GTEST_INCLUDE_DIRS})
if(BUILDING_WITH_CATKIN)
include_directories(${catkin_INCLUDE_DIRS})
endif()
add_library(robot_time
# Libraries
add_library(${PROJECT_NAME} SHARED
src/duration.cpp
src/rate.cpp
src/time.cpp)
target_include_directories(robot_time PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
set_target_properties(robot_time PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
src/time.cpp
src/timer.cpp
src/wall_timer.cpp
)
## Add cmake target dependencies
if(BUILDING_WITH_CATKIN)
add_dependencies(robot_time ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
target_link_libraries(${PROJECT_NAME}
PUBLIC ${catkin_LIBRARIES}
)
else()
target_include_directories(${PROJECT_NAME}
PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>
)
set_target_properties(${PROJECT_NAME} PROPERTIES
LIBRARY_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}
BUILD_RPATH "${CMAKE_BINARY_DIR}"
INSTALL_RPATH "${CMAKE_INSTALL_PREFIX}/lib"
)
endif()
# Export target trong mọi trường hợp để các target khác có thể export và phụ thuộc vào nó
install(TARGETS robot_time
EXPORT robot_timeTargets
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin)
if(BUILDING_WITH_CATKIN)
## Mark libraries for installation
## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
install(TARGETS ${PROJECT_NAME}
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
)
if(NOT BUILDING_WITH_CATKIN)
## Mark cpp header files for installation
install(DIRECTORY include/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
else()
install(TARGETS ${PROJECT_NAME}
EXPORT ${PROJECT_NAME}-targets
ARCHIVE DESTINATION lib
LIBRARY DESTINATION lib
RUNTIME DESTINATION bin
)
# Export targets
install(EXPORT ${PROJECT_NAME}-targets
FILE ${PROJECT_NAME}-targets.cmake
NAMESPACE ${PROJECT_NAME}::
DESTINATION lib/cmake/${PROJECT_NAME}
)
## Mark cpp header files for installation
install(DIRECTORY include/
DESTINATION include
FILES_MATCHING PATTERN "*.h")
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
# Print configuration info
message(STATUS "=================================")
message(STATUS "Project: ${PROJECT_NAME}")
message(STATUS "Version: ${PROJECT_VERSION}")
message(STATUS "C++ Standard: ${CMAKE_CXX_STANDARD}")
message(STATUS "Dependencies: GTest")
message(STATUS "=================================")
endif()
# Export targets - cần trong mọi trường hợp
install(EXPORT robot_timeTargets
NAMESPACE robot::
DESTINATION lib/cmake/robot_time)
# ========================================================
# Test executables
# ========================================================
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/time.cpp)
add_executable(${PROJECT_NAME}_test test/time.cpp)
target_link_libraries(${PROJECT_NAME}_test PRIVATE
${PROJECT_NAME}
GTest::GTest
GTest::Main
)
endif()
add_executable(robot_time_test test/time.cpp)
add_executable(robot_duration_test test/duration.cpp)
target_link_libraries(robot_time_test PRIVATE
robot_time
${GTEST_LIBRARIES}
GTest::GTest
GTest::Main)
target_link_libraries(robot_duration_test PRIVATE
robot_time
${GTEST_LIBRARIES}
GTest::GTest
GTest::Main)
if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/test/duration.cpp)
add_executable(${PROJECT_NAME}_duration_test test/duration.cpp)
target_link_libraries(${PROJECT_NAME}_duration_test PRIVATE
${PROJECT_NAME}
GTest::GTest
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()

291
README.md Normal file
View File

@@ -0,0 +1,291 @@
# 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. 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.
**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
- `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
## License
BSD License - Xem file LICENSE trong thư mục gốc của project.

212
TIMER_EVENT_EXPLANATION.md Normal file
View File

@@ -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!

217
TIMER_USAGE.md Normal file
View File

@@ -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 <robot/timer.h>
#include <iostream>
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 <robot/timer.h>
class MyClass
{
public:
void startTimer()
{
// Create timer with member function callback
timer_ = std::make_unique<robot::Timer>(
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<robot::Timer> timer_;
};
```
### One-Shot Timer
```cpp
#include <robot/timer.h>
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 <robot/timer.h>
void move_base::MoveBase::planThread()
{
boost::unique_lock<boost::recursive_mutex> 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<boost::recursive_mutex> 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

256
WALLTIMER_USAGE.md Normal file
View File

@@ -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 <robot/wall_timer.h>
#include <iostream>
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 <robot/wall_timer.h>
class PerformanceMonitor
{
public:
void startMonitoring()
{
// Create timer with member function callback
timer_ = std::make_unique<robot::WallTimer>(
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<robot::WallTimer> timer_;
};
```
### One-Shot WallTimer
```cpp
#include <robot/wall_timer.h>
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 <robot/wall_timer.h>
class Profiler
{
public:
void startProfiling()
{
profile_timer_ = std::make_unique<robot::WallTimer>(
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<robot::WallTimer> profile_timer_;
};
```
## Example: Dynamic Period Adjustment
```cpp
#include <robot/wall_timer.h>
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`

362
WALLTIME_LIBRARY.md Normal file
View File

@@ -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``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 <robot/walltime.h>
```
## 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 <robot/walltime.h>
#include <iostream>
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 <robot/walltime.h>
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 <robot/walltime.h>
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 <robot/walltime.h>
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

87
WALLTIME_QUICKSTART.md Normal file
View File

@@ -0,0 +1,87 @@
# WallTime - Quick Start Guide
## Sử dụng nhanh
### 1. Include header
```cpp
#include <robot/walltime.h>
```
### 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 <robot/walltime.h>
#include <iostream>
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
```

466
WALLTIME_USAGE.md Normal file
View File

@@ -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``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<robot::WallDuration> 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 <robot/time.h>
#include <iostream>
#include <vector>
void benchmarkOperations()
{
std::vector<std::string> 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

View File

@@ -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 <robot/time.h>
#include <iostream>
#include <vector>
#include <cmath>
// 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 ? "" : "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<std::string> 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;
}

View File

@@ -0,0 +1,229 @@
/*********************************************************************
* WallTimer Example
*
* Demonstrates various uses of robot::WallTimer
*********************************************************************/
#include <robot/wall_timer.h>
#include <iostream>
#include <thread>
#include <chrono>
// 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::WallTimer>(
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<robot::WallTimer> 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::WallTimer>(
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<robot::WallTimer> timer_;
std::thread adjust_thread_;
};
// Example 5: Timer with member function
class TimerClass
{
public:
void startTimer()
{
timer_ = std::make_unique<robot::WallTimer>(
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<robot::WallTimer> 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;
}

244
include/robot/timer.h Normal file
View File

@@ -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 <robot/time.h>
#include <robot/duration.h>
#include "robot_time_decl.h"
#include <functional>
#include <thread>
#include <mutex>
#include <atomic>
#include <condition_variable>
#include <memory>
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<void(const TimerEvent&)>;
/**
* @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<typename T>
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<bool> running_; ///< Whether timer is running
std::atomic<bool> 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

289
include/robot/wall_timer.h Normal file
View File

@@ -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 <robot/time.h>
#include "robot_time_decl.h"
#include <functional>
#include <thread>
#include <mutex>
#include <atomic>
#include <condition_variable>
#include <memory>
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<void(const WallTimerEvent&)>;
/**
* @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<typename T>
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<bool> running_; ///< Whether timer is running
std::atomic<bool> 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

475
include/robot/walltime.h Normal file
View File

@@ -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.h>
*
* robot::WallTime now = robot::WallTime::now();
* robot::WallDuration elapsed = robot::WallTime::now() - now;
*********************************************************************/
#ifndef ROBOT_WALLTIME_H
#define ROBOT_WALLTIME_H
#include <chrono>
#include <cstdint>
#include <iostream>
#include <iomanip>
#include <limits>
#include <cmath>
#include <thread>
#include <stdexcept>
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<int32_t>::max() ||
sec64 < std::numeric_limits<int32_t>::min())
{
throw std::runtime_error("WallDuration out of range");
}
sec = static_cast<int32_t>(sec64);
nsec = static_cast<int32_t>(nsec64);
}
// Convert to seconds (double)
double toSec() const
{
return static_cast<double>(sec) + 1e-9 * static_cast<double>(nsec);
}
// Convert to nanoseconds
int64_t toNSec() const
{
return static_cast<int64_t>(sec) * 1000000000LL + static_cast<int64_t>(nsec);
}
// Initialize from seconds
WallDuration& fromSec(double t)
{
sec = static_cast<int32_t>(std::floor(t));
nsec = static_cast<int32_t>((t - sec) * 1e9);
normalize();
return *this;
}
// Initialize from nanoseconds
WallDuration& fromNSec(int64_t t)
{
sec = static_cast<int32_t>(t / 1000000000LL);
nsec = static_cast<int32_t>(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<uint32_t>::max())
{
throw std::runtime_error("WallTime out of range");
}
sec = static_cast<uint32_t>(sec64);
nsec = static_cast<uint32_t>(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<std::chrono::nanoseconds>(duration).count();
WallTime t;
uint64_t sec64 = nanoseconds_count / 1000000000ULL;
uint64_t nsec64 = nanoseconds_count % 1000000000ULL;
if (sec64 > std::numeric_limits<uint32_t>::max())
{
throw std::runtime_error("WallTime::now() - time out of range");
}
t.sec = static_cast<uint32_t>(sec64);
t.nsec = static_cast<uint32_t>(nsec64);
return t;
}
// Convert to seconds (double)
double toSec() const
{
return static_cast<double>(sec) + 1e-9 * static_cast<double>(nsec);
}
// Convert to nanoseconds
uint64_t toNSec() const
{
return static_cast<uint64_t>(sec) * 1000000000ULL + static_cast<uint64_t>(nsec);
}
// Initialize from seconds
WallTime& fromSec(double t)
{
sec = static_cast<uint32_t>(std::floor(t));
nsec = static_cast<uint32_t>((t - sec) * 1e9);
normalize();
return *this;
}
// Initialize from nanoseconds
WallTime& fromNSec(uint64_t t)
{
sec = static_cast<uint32_t>(t / 1000000000ULL);
nsec = static_cast<uint32_t>(t % 1000000000ULL);
return *this;
}
// Arithmetic operations with WallDuration
WallTime operator+(const WallDuration& d) const
{
int64_t total_sec = static_cast<int64_t>(sec) + d.sec;
int64_t total_nsec = static_cast<int64_t>(nsec) + d.nsec;
if (total_sec < 0)
{
throw std::runtime_error("WallTime::operator+ - result would be negative");
}
WallTime result;
result.sec = static_cast<uint32_t>(total_sec);
result.nsec = static_cast<uint32_t>(total_nsec);
result.normalize();
return result;
}
WallTime operator-(const WallDuration& d) const
{
int64_t total_sec = static_cast<int64_t>(sec) - d.sec;
int64_t total_nsec = static_cast<int64_t>(nsec) - d.nsec;
if (total_sec < 0)
{
throw std::runtime_error("WallTime::operator- - result would be negative");
}
WallTime result;
result.sec = static_cast<uint32_t>(total_sec);
result.nsec = static_cast<uint32_t>(total_nsec);
result.normalize();
return result;
}
WallDuration operator-(const WallTime& rhs) const
{
int64_t sec_diff = static_cast<int64_t>(sec) - static_cast<int64_t>(rhs.sec);
int64_t nsec_diff = static_cast<int64_t>(nsec) - static_cast<int64_t>(rhs.nsec);
return WallDuration(static_cast<int32_t>(sec_diff), static_cast<int32_t>(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<int32_t>::max(), 999999999);
inline const WallDuration WallDuration::MIN(
std::numeric_limits<int32_t>::min(), 0);
inline const WallTime WallTime::ZERO(0, 0);
inline const WallTime WallTime::MAX(
std::numeric_limits<uint32_t>::max(), 999999999);
inline const WallTime WallTime::MIN(0, 1);
} // namespace robot
#endif // ROBOT_WALLTIME_H

View File

@@ -2,12 +2,17 @@
<name>robot_time</name>
<version>0.7.10</version>
<description>
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).
</description>
<author>Tully Foote</author>
<author>Eitan Marder-Eppstein</author>

318
src/timer.cpp Normal file
View File

@@ -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 <robot/timer.h>
#include <chrono>
#include <algorithm>
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<std::mutex> 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<std::mutex> 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<std::mutex> lock(mutex_);
return running_;
}
bool Timer::isOneShot() const
{
std::lock_guard<std::mutex> lock(mutex_);
return oneshot_;
}
void Timer::setOneShot(bool oneshot)
{
std::lock_guard<std::mutex> lock(mutex_);
oneshot_ = oneshot;
}
Duration Timer::getPeriod() const
{
std::lock_guard<std::mutex> lock(mutex_);
return period_;
}
void Timer::setPeriod(const Duration& period)
{
std::lock_guard<std::mutex> 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

375
src/wall_timer.cpp Normal file
View File

@@ -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 <robot/wall_timer.h>
#include <chrono>
#include <algorithm>
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<std::mutex> 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<std::mutex> 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<std::mutex> 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<std::mutex> lock(mutex_);
return running_;
}
bool WallTimer::isValid() const
{
std::lock_guard<std::mutex> lock(mutex_);
return callback_ != nullptr;
}
bool WallTimer::hasPending() const
{
std::lock_guard<std::mutex> 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<std::mutex> lock(mutex_);
return oneshot_;
}
void WallTimer::setOneShot(bool oneshot)
{
std::lock_guard<std::mutex> lock(mutex_);
oneshot_ = oneshot;
}
WallDuration WallTimer::getPeriod() const
{
std::lock_guard<std::mutex> lock(mutex_);
return period_;
}
WallTimer::operator void*() const
{
return isValid() ? const_cast<WallTimer*>(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

View File

@@ -0,0 +1,280 @@
/*********************************************************************
* Standalone WallTime Library Test
*
* Test file for the header-only WallTime library
*********************************************************************/
#include <robot/walltime.h>
#include <iostream>
#include <cassert>
#include <cmath>
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;
}
}