time operators (#351)

* time operators

* explicitely cast to uint64_t and prevent overflow

* check for negative seconds .. again

* split into hpp/cpp

* export symbols

* change test macro

* fix unsigned comparison

* address comments

* test for specific exception

* Fix typo

rclcpp/CMakeLists.txt

@@ -58,6 +58,7 @@ set(${PROJECT_NAME}_SRCS
   src/rclcpp/publisher.cpp
   src/rclcpp/service.cpp
   src/rclcpp/subscription.cpp
+  src/rclcpp/time.cpp
   src/rclcpp/timer.cpp
   src/rclcpp/type_support.cpp
   src/rclcpp/utilities.cpp

rclcpp/include/rclcpp/time.hpp

@@ -15,84 +15,77 @@
 #ifndef RCLCPP__TIME_HPP_
 #define RCLCPP__TIME_HPP_
 
-#include <utility>
-
 #include "builtin_interfaces/msg/time.hpp"
 
+#include "rclcpp/visibility_control.hpp"
+
 #include "rcl/time.h"
 
-#include "rclcpp/exceptions.hpp"
-
-#include "rcutils/logging_macros.h"
-
 namespace rclcpp
 {
 
 class Time
 {
 public:
-  template<rcl_time_source_type_t ClockT = RCL_SYSTEM_TIME>
+  RCLCPP_PUBLIC
-  static Time
+  static
-  now()
+  Time
-  {
+  now(rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
-    // TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
-    // we have to do this, because rcl_time_source_init returns RCL_RET_OK
-    // for RCL_ROS_TIME, however defaults to system time under the hood.
-    // ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
-    // This section can be removed when rcl supports ROS_TIME correctly.
-    if (ClockT == RCL_ROS_TIME) {
-      throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
-    }
 
-    rcl_ret_t ret = RCL_RET_ERROR;
+  RCLCPP_PUBLIC
+  Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
 
-    rcl_time_source_t time_source;
+  RCLCPP_PUBLIC
-    ret = rcl_time_source_init(ClockT, &time_source);
+  explicit Time(uint64_t nanoseconds, rcl_time_source_type_t clock = RCL_SYSTEM_TIME);
 
-    if (ret != RCL_RET_OK) {
+  RCLCPP_PUBLIC
-      rclcpp::exceptions::throw_from_rcl_error(
+  Time(const builtin_interfaces::msg::Time & time_msg);  // NOLINT
-        ret, "could not initialize time source: ");
-    }
 
-    rcl_time_point_t time_point;
+  RCLCPP_PUBLIC
-    ret = rcl_time_point_init(&time_point, &time_source);
+  virtual ~Time();
 
-    if (ret != RCL_RET_OK) {
+  RCLCPP_PUBLIC
-      rclcpp::exceptions::throw_from_rcl_error(
+  operator builtin_interfaces::msg::Time() const;
-        ret, "could not initialize time point: ");
-    }
 
-    ret = rcl_time_point_get_now(&time_point);
+  RCLCPP_PUBLIC
-    if (ret != RCL_RET_OK) {
+  void
-      rclcpp::exceptions::throw_from_rcl_error(
+  operator=(const builtin_interfaces::msg::Time & time_msg);
-        ret, "could not get current time stamp: ");
-    }
 
-    return Time(std::move(time_point));
+  RCLCPP_PUBLIC
-  }
+  bool
+  operator==(const rclcpp::Time & rhs) const;
 
-  operator builtin_interfaces::msg::Time() const
+  RCLCPP_PUBLIC
-  {
+  bool
-    builtin_interfaces::msg::Time msg_time;
+  operator<(const rclcpp::Time & rhs) const;
-    msg_time.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_time_.nanoseconds));
+
-    msg_time.nanosec = static_cast<std::uint32_t>(rcl_time_.nanoseconds % (1000 * 1000 * 1000));
+  RCLCPP_PUBLIC
-    return msg_time;
+  bool
-  }
+  operator<=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>=(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  bool
+  operator>(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator+(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  Time
+  operator-(const rclcpp::Time & rhs) const;
+
+  RCLCPP_PUBLIC
+  uint64_t
+  nanoseconds() const;
 
 private:
+  rcl_time_source_t rcl_time_source_;
   rcl_time_point_t rcl_time_;
-
-  explicit Time(rcl_time_point_t && rcl_time)
-  : rcl_time_(std::forward<decltype(rcl_time)>(rcl_time))
-  {}
-
-public:
-  virtual ~Time()
-  {
-    if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
-      RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::Time")
-    }
-  }
 };
 
 }  // namespace rclcpp

rclcpp/src/rclcpp/time.cpp

@@ -0,0 +1,232 @@
+// 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 "rclcpp/time.hpp"
+
+#include <utility>
+
+#include "builtin_interfaces/msg/time.hpp"
+
+#include "rcl/time.h"
+
+#include "rclcpp/exceptions.hpp"
+
+#include "rcutils/logging_macros.h"
+
+namespace
+{
+
+rcl_time_source_t
+init_time_source(rcl_time_source_type_t clock = RCL_SYSTEM_TIME)
+{
+  rcl_time_source_t time_source;
+  auto ret = rcl_time_source_init(clock, &time_source);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not initialize time source");
+  }
+
+  return time_source;
+}
+
+rcl_time_point_t
+init_time_point(rcl_time_source_t & time_source)
+{
+  rcl_time_point_t time_point;
+  auto ret = rcl_time_point_init(&time_point, &time_source);
+
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not initialize time point");
+  }
+
+  return time_point;
+}
+
+}  // namespace
+
+namespace rclcpp
+{
+
+Time
+Time::now(rcl_time_source_type_t clock)
+{
+  // TODO(karsten1987): This impl throws explicitely on RCL_ROS_TIME
+  // we have to do this, because rcl_time_source_init returns RCL_RET_OK
+  // for RCL_ROS_TIME, however defaults to system time under the hood.
+  // ref: https://github.com/ros2/rcl/blob/master/rcl/src/rcl/time.c#L66-L77
+  // This section can be removed when rcl supports ROS_TIME correctly.
+  if (clock == RCL_ROS_TIME) {
+    throw std::runtime_error("RCL_ROS_TIME is currently not implemented.");
+  }
+
+  Time now(0, 0, clock);
+
+  auto ret = rcl_time_point_get_now(&now.rcl_time_);
+  if (ret != RCL_RET_OK) {
+    rclcpp::exceptions::throw_from_rcl_error(
+      ret, "could not get current time stamp");
+  }
+
+  return now;
+}
+
+Time::Time(int32_t seconds, uint32_t nanoseconds, rcl_time_source_type_t clock)
+: rcl_time_source_(init_time_source(clock)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  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<uint64_t>(seconds));
+  rcl_time_.nanoseconds += nanoseconds;
+}
+
+Time::Time(uint64_t nanoseconds, rcl_time_source_type_t clock)
+: rcl_time_source_(init_time_source(clock)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  rcl_time_.nanoseconds = nanoseconds;
+}
+
+Time::Time(const builtin_interfaces::msg::Time & time_msg)  // NOLINT
+: rcl_time_source_(init_time_source(RCL_SYSTEM_TIME)),
+  rcl_time_(init_time_point(rcl_time_source_))
+{
+  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<uint64_t>(time_msg.sec));
+  rcl_time_.nanoseconds += time_msg.nanosec;
+}
+
+Time::~Time()
+{
+  if (rcl_time_point_fini(&rcl_time_) != RCL_RET_OK) {
+    RCUTILS_LOG_FATAL("failed to reclaim rcl_time_point_t in destructor of rclcpp::Time")
+  }
+}
+
+Time::operator builtin_interfaces::msg::Time() const
+{
+  builtin_interfaces::msg::Time msg_time;
+  msg_time.sec = static_cast<std::int32_t>(RCL_NS_TO_S(rcl_time_.nanoseconds));
+  msg_time.nanosec = static_cast<std::uint32_t>(rcl_time_.nanoseconds % (1000 * 1000 * 1000));
+  return msg_time;
+}
+
+void
+Time::operator=(const builtin_interfaces::msg::Time & time_msg)
+{
+  if (time_msg.sec < 0) {
+    throw std::runtime_error("cannot store a negative time point in rclcpp::Time");
+  }
+
+  this->rcl_time_source_ = init_time_source();
+  this->rcl_time_ = init_time_point(this->rcl_time_source_);
+
+  rcl_time_.nanoseconds = RCL_S_TO_NS(static_cast<uint64_t>(time_msg.sec));
+  rcl_time_.nanoseconds += time_msg.nanosec;
+}
+
+bool
+Time::operator==(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->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_.time_source->type != rhs.rcl_time_.time_source->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_.time_source->type != rhs.rcl_time_.time_source->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_.time_source->type != rhs.rcl_time_.time_source->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_.time_source->type != rhs.rcl_time_.time_source->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::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't add times with different time sources");
+  }
+
+  auto ns = rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds;
+  if (ns < rcl_time_.nanoseconds) {
+    throw std::overflow_error("addition leads to uint64_t overflow");
+  }
+
+  return Time(rcl_time_.nanoseconds + rhs.rcl_time_.nanoseconds);
+}
+
+Time
+Time::operator-(const rclcpp::Time & rhs) const
+{
+  if (rcl_time_.time_source->type != rhs.rcl_time_.time_source->type) {
+    throw std::runtime_error("can't add times with different time sources");
+  }
+
+  auto ns = rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds;
+  if (ns > rcl_time_.nanoseconds) {
+    throw std::underflow_error("subtraction leads to uint64_t underflow");
+  }
+
+  return Time(rcl_time_.nanoseconds - rhs.rcl_time_.nanoseconds);
+}
+
+uint64_t
+Time::nanoseconds() const
+{
+  return rcl_time_.nanoseconds;
+}
+
+}  // namespace rclcpp

rclcpp/test/test_time.cpp

@@ -14,6 +14,8 @@
 
 #include <gtest/gtest.h>
 
+#include <algorithm>
+#include <limits>
 #include <string>
 
 #include "rcl/error_handling.h"
@@ -30,16 +32,16 @@ protected:
   }
 };
 
-TEST(TestTime, rate_basics) {
+TEST(TestTime, time_sources) {
   using builtin_interfaces::msg::Time;
   // TODO(Karsten1987): Fix this test once ROS_TIME is implemented
-  EXPECT_ANY_THROW(rclcpp::Time::now<RCL_ROS_TIME>());
+  EXPECT_ANY_THROW(rclcpp::Time::now(RCL_ROS_TIME));
 
-  Time system_now = rclcpp::Time::now<RCL_SYSTEM_TIME>();
+  Time system_now = rclcpp::Time::now(RCL_SYSTEM_TIME);
   EXPECT_NE(0, system_now.sec);
   EXPECT_NE(0u, system_now.nanosec);
 
-  Time steady_now = rclcpp::Time::now<RCL_STEADY_TIME>();
+  Time steady_now = rclcpp::Time::now(RCL_STEADY_TIME);
   EXPECT_NE(0, steady_now.sec);
   EXPECT_NE(0u, steady_now.nanosec);
 
@@ -48,3 +50,85 @@ TEST(TestTime, rate_basics) {
   EXPECT_NE(0, default_now.sec);
   EXPECT_NE(0u, default_now.nanosec);
 }
+
+TEST(TestTime, convertions) {
+  rclcpp::Time now = rclcpp::Time::now();
+  builtin_interfaces::msg::Time now_msg = now;
+
+  rclcpp::Time now_again = now_msg;
+  EXPECT_EQ(now.nanoseconds(), now_again.nanoseconds());
+
+  builtin_interfaces::msg::Time msg;
+  msg.sec = 12345;
+  msg.nanosec = 67890;
+
+  rclcpp::Time time = msg;
+  EXPECT_EQ(
+    RCL_S_TO_NS(static_cast<uint64_t>(msg.sec)) + static_cast<uint64_t>(msg.nanosec),
+    time.nanoseconds());
+  EXPECT_EQ(static_cast<uint64_t>(msg.sec), RCL_NS_TO_S(time.nanoseconds()));
+
+  builtin_interfaces::msg::Time negative_time_msg;
+  negative_time_msg.sec = -1;
+  negative_time_msg.nanosec = 1;
+
+  EXPECT_ANY_THROW({
+    rclcpp::Time negative_time = negative_time_msg;
+  });
+
+  EXPECT_ANY_THROW(rclcpp::Time(-1, 1));
+
+  EXPECT_ANY_THROW({
+    rclcpp::Time assignment(1, 2);
+    assignment = negative_time_msg;
+  });
+}
+
+TEST(TestTime, operators) {
+  rclcpp::Time old(1, 0);
+  rclcpp::Time young(2, 0);
+
+  EXPECT_TRUE(old < young);
+  EXPECT_TRUE(young > old);
+  EXPECT_TRUE(old <= young);
+  EXPECT_TRUE(young >= old);
+  EXPECT_FALSE(young == old);
+
+  rclcpp::Time add = old + young;
+  EXPECT_EQ(add.nanoseconds(), old.nanoseconds() + young.nanoseconds());
+  EXPECT_EQ(add, old + young);
+
+  rclcpp::Time sub = young - old;
+  EXPECT_EQ(sub.nanoseconds(), young.nanoseconds() - old.nanoseconds());
+  EXPECT_EQ(sub, young - old);
+
+  rclcpp::Time system_time(1, 0, RCL_SYSTEM_TIME);
+  rclcpp::Time steady_time(2, 0, RCL_STEADY_TIME);
+
+  EXPECT_ANY_THROW((void)(system_time == steady_time));
+  EXPECT_ANY_THROW((void)(system_time <= steady_time));
+  EXPECT_ANY_THROW((void)(system_time >= steady_time));
+  EXPECT_ANY_THROW((void)(system_time < steady_time));
+  EXPECT_ANY_THROW((void)(system_time > steady_time));
+  EXPECT_ANY_THROW((void)(system_time + steady_time));
+  EXPECT_ANY_THROW((void)(system_time - steady_time));
+
+  rclcpp::Time now = rclcpp::Time::now(RCL_SYSTEM_TIME);
+  rclcpp::Time later = rclcpp::Time::now(RCL_STEADY_TIME);
+
+  EXPECT_ANY_THROW((void)(now == later));
+  EXPECT_ANY_THROW((void)(now <= later));
+  EXPECT_ANY_THROW((void)(now >= later));
+  EXPECT_ANY_THROW((void)(now < later));
+  EXPECT_ANY_THROW((void)(now > later));
+  EXPECT_ANY_THROW((void)(now + later));
+  EXPECT_ANY_THROW((void)(now - later));
+}
+
+TEST(TestTime, overflows) {
+  rclcpp::Time max(std::numeric_limits<uint64_t>::max());
+  rclcpp::Time one(1);
+
+  EXPECT_THROW(max + one, std::overflow_error);
+  EXPECT_THROW(one - max, std::underflow_error);
+}