time.cpp

// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
 
#include <limits>
#include <string>
#include <utility>
 
#include "rclcpp/clock.hpp"
#include "rclcpp/duration.hpp"
 
#include "builtin_interfaces/msg/time.hpp"
 
#include "rcl/time.h"
 
#include "rclcpp/exceptions.hpp"
 
#include "rcutils/logging_macros.h"
 
#include "rclcpp/utilities.hpp"
 
namespace
{
 
rcl_time_point_t
init_time_point(rcl_clock_type_t & clock_type)
{
  rcl_time_point_t time_point;
  time_point.clock_type = clock_type;
 
  return time_point;
}
 
}  // namespace
 
namespace rclcpp
{
 
Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_clock_type_t clock_type)
: rcl_time_(init_time_point(clock_type))
{
  if (seconds < 0) {
    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
  }
 
  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(seconds));
  rcl_time_.nanoseconds += nanoseconds;
}
 
Time::Time(int64_t nanoseconds, rcl_clock_type_t clock_type)
: rcl_time_(init_time_point(clock_type))
{
  if (nanoseconds < 0) {
    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
  }
 
  rcl_time_.nanoseconds = nanoseconds;
}
 
Time::Time(const Time & rhs) = default;
 
Time::Time(Time && rhs) noexcept = default;
 
Time::Time(
  const builtin_interfaces::msg::Time & time_msg,
  rcl_clock_type_t clock_type)
: rcl_time_(init_time_point(clock_type))
{
  if (time_msg.sec < 0) {
    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
  }
 
  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<int64_t>(time_msg.sec));
  rcl_time_.nanoseconds += time_msg.nanosec;
}
 
Time::Time(const rcl_time_point_t & time_point)
: rcl_time_(time_point)
{
  // noop
}
 
Time::~Time() = default;
 
Time::operator builtin_interfaces::msg::Time() const
{
  return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds);
}
 
Time &
Time::operator=(const Time & rhs) = default;
 
Time &
Time::operator=(const builtin_interfaces::msg::Time & time_msg)
{
  *this = Time(time_msg);
  return *this;
}
 
Time &
Time::operator=(Time && rhs) noexcept = default;
 
bool
Time::operator==(const rclcpp::Time & rhs) const
{
  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
    throw std::runtime_error("can't compare times with different time sources");
  }
 
  return rcl_time_.nanoseconds == rhs.rcl_time_.nanoseconds;
}
 
bool
Time::operator!=(const rclcpp::Time & rhs) const
{
  return !(*this == rhs);
}
 
bool
Time::operator<(const rclcpp::Time & rhs) const
{
  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
    throw std::runtime_error("can't compare times with different time sources");
  }
 
  return rcl_time_.nanoseconds < rhs.rcl_time_.nanoseconds;
}
 
bool
Time::operator<=(const rclcpp::Time & rhs) const
{
  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
    throw std::runtime_error("can't compare times with different time sources");
  }
 
  return rcl_time_.nanoseconds <= rhs.rcl_time_.nanoseconds;
}
 
bool
Time::operator>=(const rclcpp::Time & rhs) const
{
  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
    throw std::runtime_error("can't compare times with different time sources");
  }
 
  return rcl_time_.nanoseconds >= rhs.rcl_time_.nanoseconds;
}
 
bool
Time::operator>(const rclcpp::Time & rhs) const
{
  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
    throw std::runtime_error("can't compare times with different time sources");
  }
 
  return rcl_time_.nanoseconds > rhs.rcl_time_.nanoseconds;
}
 
Time
Time::operator+(const rclcpp::Duration & rhs) const
{
  if (rclcpp::add_will_overflow(rhs.nanoseconds(), this->nanoseconds())) {
    throw std::overflow_error("addition leads to int64_t overflow");
  }
  if (rclcpp::add_will_underflow(rhs.nanoseconds(), this->nanoseconds())) {
    throw std::underflow_error("addition leads to int64_t underflow");
  }
  return Time(this->nanoseconds() + rhs.nanoseconds(), this->get_clock_type());
}
 
Duration
Time::operator-(const rclcpp::Time & rhs) const
{
  if (rcl_time_.clock_type != rhs.rcl_time_.clock_type) {
    throw std::runtime_error(
            std::string("can't subtract times with different time sources [") +
            std::to_string(rcl_time_.clock_type) + " != " +
            std::to_string(rhs.rcl_time_.clock_type) + "]");
  }
 
  if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) {
    throw std::overflow_error("time subtraction leads to int64_t overflow");
  }
 
  if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.rcl_time_.nanoseconds)) {
    throw std::underflow_error("time subtraction leads to int64_t underflow");
  }
 
  return Duration::from_nanoseconds(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
}
 
Time
Time::operator-(const rclcpp::Duration & rhs) const
{
  if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.nanoseconds())) {
    throw std::overflow_error("time subtraction leads to int64_t overflow");
  }
  if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.nanoseconds())) {
    throw std::underflow_error("time subtraction leads to int64_t underflow");
  }
 
  return Time(rcl_time_.nanoseconds - rhs.nanoseconds(), rcl_time_.clock_type);
}
 
int64_t
Time::nanoseconds() const
{
  return rcl_time_.nanoseconds;
}
 
double
Time::seconds() const
{
  return std::chrono::duration<double>(std::chrono::nanoseconds(rcl_time_.nanoseconds)).count();
}
 
rcl_clock_type_t
Time::get_clock_type() const
{
  return rcl_time_.clock_type;
}
 
Time
operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs)
{
  if (rclcpp::add_will_overflow(rhs.nanoseconds(), lhs.nanoseconds())) {
    throw std::overflow_error("addition leads to int64_t overflow");
  }
  if (rclcpp::add_will_underflow(rhs.nanoseconds(), lhs.nanoseconds())) {
    throw std::underflow_error("addition leads to int64_t underflow");
  }
  return Time(lhs.nanoseconds() + rhs.nanoseconds(), rhs.get_clock_type());
}
 
Time &
Time::operator+=(const rclcpp::Duration & rhs)
{
  if (rclcpp::add_will_overflow(rhs.nanoseconds(), this->nanoseconds())) {
    throw std::overflow_error("addition leads to int64_t overflow");
  }
  if (rclcpp::add_will_underflow(rhs.nanoseconds(), this->nanoseconds())) {
    throw std::underflow_error("addition leads to int64_t underflow");
  }
 
  rcl_time_.nanoseconds += rhs.nanoseconds();
  if (rcl_time_.nanoseconds < 0) {
    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
  }
 
  return *this;
}
 
Time &
Time::operator-=(const rclcpp::Duration & rhs)
{
  if (rclcpp::sub_will_overflow(rcl_time_.nanoseconds, rhs.nanoseconds())) {
    throw std::overflow_error("time subtraction leads to int64_t overflow");
  }
  if (rclcpp::sub_will_underflow(rcl_time_.nanoseconds, rhs.nanoseconds())) {
    throw std::underflow_error("time subtraction leads to int64_t underflow");
  }
 
  rcl_time_.nanoseconds -= rhs.nanoseconds();
  if (rcl_time_.nanoseconds < 0) {
    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
  }
 
  return *this;
}
 
Time
Time::max(rcl_clock_type_t clock_type)
{
  return Time(std::numeric_limits<int32_t>::max(), 999999999, clock_type);
}
 
builtin_interfaces::msg::Time
convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point)
{
  builtin_interfaces::msg::Time ret;
  constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
  const auto result = std::div(time_point, kRemainder);
  if (result.rem >= 0) {
    ret.sec = static_cast<std::int32_t>(result.quot);
    ret.nanosec = static_cast<std::uint32_t>(result.rem);
  } else {
    ret.sec = static_cast<std::int32_t>(result.quot - 1);
    ret.nanosec = static_cast<std::uint32_t>(kRemainder + result.rem);
  }
  return ret;
}
 
}  // namespace rclcpp