robot_time/src/time.cpp

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2010, Willow Garage, Inc.
 *  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.
 *   * Neither the name of Willow Garage, Inc. nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  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.
 *********************************************************************/
#ifdef _MSC_VER
  #ifndef NOMINMAX
    #define NOMINMAX
  #endif
#endif

#include "robot/time.h"
#include "robot/impl/time.h"
#include <cmath>
#include <ctime>
#include <iomanip>
#include <limits>
#include <stdexcept>

// time related includes for macOS
#if defined(__APPLE__)
#include <mach/clock.h>
#include <mach/mach.h>
#endif  // defined(__APPLE__)

#ifdef _WINDOWS
#include <chrono>
#include <thread>
#include <windows.h>
#endif

#include <mutex>
#include <chrono>

/*********************************************************************
 ** Preprocessor
 *********************************************************************/

// Could probably do some better and more elaborate checking
// and definition here.
#define HAS_CLOCK_GETTIME (_POSIX_C_SOURCE >= 199309L)

/*********************************************************************
 ** Namespaces
 *********************************************************************/

namespace robot
{

  /*********************************************************************
   ** Variables
   *********************************************************************/

  const Duration DURATION_MAX(std::numeric_limits<int32_t>::max(), 999999999);
  const Duration DURATION_MIN(std::numeric_limits<int32_t>::min(), 0);

  template<> const Duration DurationBase<Duration>::MAX = DURATION_MAX;
  template<> const Duration DurationBase<Duration>::MIN = DURATION_MIN;
  template<> const Duration DurationBase<Duration>::ZERO = Duration(0, 0);
  template<> const Duration DurationBase<Duration>::NANOSECOND = Duration(0, 1);
  template<> const Duration DurationBase<Duration>::MICROSECOND = Duration(0, 1000);
  template<> const Duration DurationBase<Duration>::MILLISECOND = Duration(0, 1000000);
  template<> const Duration DurationBase<Duration>::SECOND = Duration(1, 0);
  template<> const Duration DurationBase<Duration>::MINUTE = Duration(60, 0);
  template<> const Duration DurationBase<Duration>::HOUR = Duration(60 * 60, 0);
  template<> const Duration DurationBase<Duration>::DAY = Duration(60 * 60 * 24, 0);

  template<> const WallDuration DurationBase<WallDuration>::MAX = WallDuration(Duration::MAX.sec, Duration::MAX.nsec);
  template<> const WallDuration DurationBase<WallDuration>::MIN = WallDuration(Duration::MIN.sec, Duration::MIN.nsec);
  template<> const WallDuration DurationBase<WallDuration>::ZERO = WallDuration(Duration::ZERO.sec, Duration::ZERO.nsec);
  template<> const WallDuration DurationBase<WallDuration>::DAY = WallDuration(Duration::DAY.sec, Duration::DAY.nsec);
  template<> const WallDuration DurationBase<WallDuration>::HOUR = WallDuration(Duration::HOUR.sec, Duration::HOUR.nsec);
  template<> const WallDuration DurationBase<WallDuration>::MINUTE = WallDuration(Duration::MINUTE.sec, Duration::MINUTE.nsec);
  template<> const WallDuration DurationBase<WallDuration>::SECOND = WallDuration(Duration::SECOND.sec, Duration::SECOND.nsec);
  template<> const WallDuration DurationBase<WallDuration>::MILLISECOND = WallDuration(Duration::MILLISECOND.sec, Duration::MILLISECOND.nsec);
  template<> const WallDuration DurationBase<WallDuration>::MICROSECOND = WallDuration(Duration::MICROSECOND.sec, Duration::MICROSECOND.nsec);
  template<> const WallDuration DurationBase<WallDuration>::NANOSECOND = WallDuration(Duration::NANOSECOND.sec, Duration::NANOSECOND.nsec);

  const Time TIME_MAX(std::numeric_limits<uint32_t>::max(), 999999999);
  const Time TIME_MIN(0, 1);

  template<> const Time TimeBase<Time, Duration>::MAX = TIME_MAX;
  template<> const Time TimeBase<Time, Duration>::MIN = TIME_MIN;
  template<> const Time TimeBase<Time, Duration>::ZERO = Time(0, 0);
  template<> const Time TimeBase<Time, Duration>::UNINITIALIZED = Time::ZERO;

  template<> const WallTime TimeBase<WallTime, WallDuration>::MAX = WallTime(Time::MAX.sec, Time::MAX.nsec);
  template<> const WallTime TimeBase<WallTime, WallDuration>::MIN = WallTime(Time::MIN.sec, Time::MIN.nsec);
  template<> const WallTime TimeBase<WallTime, WallDuration>::ZERO = WallTime(Time::ZERO.sec, Time::ZERO.nsec);
  template<> const WallTime TimeBase<WallTime, WallDuration>::UNINITIALIZED = WallTime(Time::UNINITIALIZED.sec, Time::UNINITIALIZED.nsec);

  template<> const SteadyTime TimeBase<SteadyTime, WallDuration>::MAX = SteadyTime(Time::MAX.sec, Time::MAX.nsec);
  template<> const SteadyTime TimeBase<SteadyTime, WallDuration>::MIN = SteadyTime(Time::MIN.sec, Time::MIN.nsec);
  template<> const SteadyTime TimeBase<SteadyTime, WallDuration>::ZERO = SteadyTime(Time::ZERO.sec, Time::ZERO.nsec);
  template<> const SteadyTime TimeBase<SteadyTime, WallDuration>::UNINITIALIZED = SteadyTime(Time::UNINITIALIZED.sec, Time::UNINITIALIZED.nsec);

  // This is declared here because it's set from the Time class but read from
  // the Duration class, and need not be exported to users of either.
  static bool g_stopped(false);

  // I assume that this is declared here, instead of time.h, to keep users
  // of time.h from including boost/thread/mutex.hpp
  static std::mutex g_sim_time_mutex;

  static bool g_initialized(true);
  static bool g_use_sim_time(false);
  static Time g_sim_time(0, 0);

  /*********************************************************************
   ** Cross Platform Functions
   *********************************************************************/
  void robot_walltime(uint32_t& sec, uint32_t& nsec)
  {
    using namespace std::chrono;
    uint64_t now_ns = duration_cast<nanoseconds>(system_clock::now().time_since_epoch()).count();
    uint64_t s = 0;
    normalizeSecNSec(s, now_ns);
    sec = static_cast<uint32_t>(s);
    nsec = static_cast<uint32_t>(now_ns);
  }

  void robot_steadytime(uint32_t& sec, uint32_t& nsec)
  {
    using namespace std::chrono;
    uint64_t now_ns = duration_cast<nanoseconds>(steady_clock::now().time_since_epoch()).count();
    uint64_t s = 0;
    normalizeSecNSec(s, now_ns);
    sec = static_cast<uint32_t>(s);
    nsec = static_cast<uint32_t>(now_ns);
  }

  /*
   * These have only internal linkage to this translation unit.
   * (i.e. not exposed to users of the time classes)
   */

  /**
   * @brief Simple representation of the rt library nanosleep function.
   */
  int robot_nanosleep(const uint32_t &sec, const uint32_t &nsec)
  {
#if defined(_WIN32)
    std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast<int64_t>(sec * 1e9 + nsec)));
    return 0;
#else
    timespec req = { sec, nsec };
    return nanosleep(&req, NULL);
#endif
  }

  /**
   * @brief Go to the wall!
   *
   * @todo Fully implement the win32 parts, currently just like a regular sleep.
   */
  bool robot_wallsleep(uint32_t sec, uint32_t nsec)
  {
#if defined(_WIN32)
    robot_nanosleep(sec,nsec);
#else
    timespec req = { sec, nsec };
    timespec rem = {0, 0};
    while (nanosleep(&req, &rem) && !g_stopped)
      {
        req = rem;
      }
#endif
    return !g_stopped;
  }

  /*********************************************************************
   ** Class Methods
   *********************************************************************/

  bool Time::useSystemTime()
  {
    return !g_use_sim_time;
  }

  bool Time::isSimTime()
  {
    return g_use_sim_time;
  }

  bool Time::isSystemTime()
  {
    return !isSimTime();
  }

  Time Time::now()
  {
    if (!g_initialized)
      {
        throw TimeNotInitializedException();
      }

    if (g_use_sim_time)
      {
        std::lock_guard<std::mutex> lock(g_sim_time_mutex);
        Time t = g_sim_time;
        return t;
      }

    Time t;
    robot_walltime(t.sec, t.nsec);

    return t;
  }

  void Time::setNow(const Time& new_now)
  {
    std::lock_guard<std::mutex> lock(g_sim_time_mutex);

    g_sim_time = new_now;
    g_use_sim_time = true;
  }

  void Time::init()
  {
    g_stopped = false;
    g_use_sim_time = false;
    g_initialized = true;
  }

  void Time::shutdown()
  {
    g_stopped = true;
  }

  bool Time::isValid()
  {
    return (!g_use_sim_time) || !g_sim_time.isZero();
  }

  bool Time::waitForValid()
  {
    return waitForValid(robot::WallDuration());
  }

  bool Time::waitForValid(const robot::WallDuration& timeout)
  {
    robot::WallTime start = robot::WallTime::now();
    while (!isValid() && !g_stopped)
    {
      robot::WallDuration(0.01).sleep();

      if (timeout > robot::WallDuration(0, 0) && (robot::WallTime::now() - start > timeout))
      {
        return false;
      }
    }

    if (g_stopped)
    {
      return false;
    }

    return true;
  }

  // boost::posix_time conversion removed in std-only build

  std::ostream& operator<<(std::ostream& os, const Time &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;
  }

  std::ostream& operator<<(std::ostream& os, const Duration& 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;
  }

  bool Time::sleepUntil(const Time& end)
  {
    if (Time::useSystemTime())
    {
      Duration d(end - Time::now());
      if (d > Duration(0))
      {
        return d.sleep();
      }

      return true;
    }
    else
    {
      Time start = Time::now();
      while (!g_stopped && (Time::now() < end))
      {
        robot_nanosleep(0,1000000);
        if (Time::now() < start)
        {
          return false;
        }
      }

      return true;
    }
  }

  bool WallTime::sleepUntil(const WallTime& end)
  {
    WallDuration d(end - WallTime::now());
    if (d > WallDuration(0))
      {
        return d.sleep();
      }

    return true;
  }

  bool SteadyTime::sleepUntil(const SteadyTime& end)
  {
    WallDuration d(end - SteadyTime::now());
    if (d > WallDuration(0))
    {
      return d.sleep();
    }

    return true;
  }

  bool Duration::sleep() const
  {
    if (Time::useSystemTime())
    {
      return robot_wallsleep(sec, nsec);
    }
    else
    {
      Time start = Time::now();
      Time end = start + *this;
      if (start.isZero())
      {
        end = TIME_MAX;
      }

      bool rc = false;
      // This allows sim time to run up to 10x real-time even for very short sleep durations.
      const uint32_t sleep_nsec = (sec != 0) ? 1000000 : (std::min)(1000000, nsec/10);
      while (!g_stopped && (Time::now() < end))
      {
        robot_wallsleep(0, sleep_nsec);
        rc = true;

        // If we started at time 0 wait for the first actual time to arrive before starting the timer on
        // our sleep
        if (start.isZero())
        {
          start = Time::now();
          end = start + *this;
        }

        // If time jumped backwards from when we started sleeping, return immediately
        if (Time::now() < start)
        {
          return false;
        }
      }

      return rc && !g_stopped;
    }
  }

  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;
  }

  std::ostream &operator<<(std::ostream& os, const SteadyTime &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;
  }

  WallTime WallTime::now()
  {
    WallTime t;
    robot_walltime(t.sec, t.nsec);

    return t;
  }

  SteadyTime SteadyTime::now()
  {
    SteadyTime t;
    robot_steadytime(t.sec, t.nsec);

    return t;
  }

  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;
  }

  bool WallDuration::sleep() const
  {
    return robot_wallsleep(sec, nsec);
  }

  void normalizeSecNSec(uint64_t& sec, uint64_t& nsec)
  {
    uint64_t nsec_part = nsec % 1000000000UL;
    uint64_t sec_part = nsec / 1000000000UL;

    if (sec + sec_part > std::numeric_limits<uint32_t>::max())
      throw std::runtime_error("Time is out of dual 32-bit range");

    sec += sec_part;
    nsec = nsec_part;
  }

  void normalizeSecNSec(uint32_t& sec, uint32_t& nsec)
  {
    uint64_t sec64 = sec;
    uint64_t nsec64 = nsec;

    normalizeSecNSec(sec64, nsec64);

    sec = (uint32_t)sec64;
    nsec = (uint32_t)nsec64;
  }

  void normalizeSecNSecUnsigned(int64_t& sec, int64_t& nsec)
  {
    int64_t nsec_part = nsec % 1000000000L;
    int64_t sec_part = sec + nsec / 1000000000L;
    if (nsec_part < 0)
      {
        nsec_part += 1000000000L;
        --sec_part;
      }

    if (sec_part < 0 || sec_part > std::numeric_limits<uint32_t>::max())
      throw std::runtime_error("Time is out of dual 32-bit range");

    sec = sec_part;
    nsec = nsec_part;
  }

  template class TimeBase<Time, Duration>;
  template class TimeBase<WallTime, WallDuration>;
  template class TimeBase<SteadyTime, WallDuration>;
}