add WallTime và WallTimer

include/robot/wall_timer.h

@@ -0,0 +1,289 @@
+/*********************************************************************
+ * Software License Agreement (BSD License)
+ *
+ *  Copyright (c) 2024
+ *  All rights reserved.
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *   * Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *   * Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *********************************************************************/
+
+#ifndef ROBOT_WALL_TIMER_H
+#define ROBOT_WALL_TIMER_H
+
+#include <robot/walltime.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
+

include/robot/walltime.h

@@ -0,0 +1,475 @@
+/*********************************************************************
+ * WallTime - Standalone Wall-clock Time Library
+ * 
+ * Header-only library for wall-clock time operations.
+ * This is a simplified, standalone version of WallTime that can be
+ * used independently or as a convenience wrapper.
+ * 
+ * Features:
+ * - Always uses real wall-clock time (not affected by simulated time)
+ * - No initialization required
+ * - Thread-safe
+ * - Cross-platform (Linux, Windows, macOS)
+ * - Nanosecond precision
+ * 
+ * Usage:
+ *   #include <robot/walltime.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
+