476 lines
11 KiB
C++
476 lines
11 KiB
C++
/*********************************************************************
|
|
* 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
|
|
|