Add rclcpp::RosTime

rclcpp/include/rclcpp/time.hpp

@@ -210,7 +210,7 @@ class Time
   rcl_clock_type_t
   get_clock_type() const;
 
-private:
+protected:
   rcl_time_point_t rcl_time_;
   friend Clock;  // Allow clock to manipulate internal data
 };
@@ -222,6 +222,58 @@ RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 
+/// Time class that always uses ROS time
+class RosTime : public Time
+{
+public:
+  /// RosTime constructor
+  /**
+   * Initializes the time values for seconds and nanoseconds individually.
+   * Large values for nanoseconds are wrapped automatically with the remainder added to seconds.
+   * Both inputs must be integers.
+   *
+   * \param seconds part of the time in seconds since time epoch
+   * \param nanoseconds part of the time in nanoseconds since time epoch
+   * \throws std::runtime_error if seconds are negative
+   */
+  RCLCPP_PUBLIC
+  explicit RosTime(int32_t seconds, uint32_t nanoseconds = 0)
+  : Time(seconds, nanoseconds, RCL_ROS_TIME) {}
+
+  /// Construct from non-ros time
+  RCLCPP_PUBLIC
+  explicit RosTime(const Time & rhs);
+
+  /// RosTime constructor
+  /**
+   * \param time_msg builtin_interfaces time message to copy
+   * \throws std::runtime_error if seconds are negative
+   */
+  RCLCPP_PUBLIC
+  explicit RosTime(const builtin_interfaces::msg::Time & time_msg)
+  : Time(time_msg, RCL_ROS_TIME) {}
+
+  /// RosTime constructor
+  /**
+   * \param time_point rcl_time_point_t structure to copy
+   * \throws std::runtime_error if clock type
+   */
+  RCLCPP_PUBLIC
+  explicit RosTime(const rcl_time_point_t & time_point);
+
+  /// RosTime Destructor
+  RCLCPP_PUBLIC
+  virtual ~RosTime();
+
+  /**
+   * \throws std::runtime_error if seconds are negative
+   * \throws std::runtime_error if rhs clock type is not RCL_ROS_TIME
+   */
+  RCLCPP_PUBLIC
+  RosTime &
+  operator=(const Time & rhs);
+};
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__TIME_HPP_

rclcpp/src/rclcpp/time.cpp

@@ -282,4 +282,35 @@ Time::max()
 }
 
 
+RosTime::RosTime(const rcl_time_point_t & time_point)
+{
+  if (RCL_ROS_TIME != time_point.clock_type) {
+    throw std::runtime_error("RosTime requires a clock type of RCL_ROS_TIME");
+  }
+  rcl_time_ = time_point;
+}
+
+RosTime::RosTime(const Time & other)
+{
+  if (RCL_ROS_TIME != other.get_clock_type()) {
+    throw std::runtime_error("RosTime requires a clock type of RCL_ROS_TIME");
+  }
+  rcl_time_.clock_type = RCL_ROS_TIME;
+  rcl_time_.nanoseconds = other.nanoseconds();
+}
+
+RosTime::~RosTime()
+{
+}
+
+RosTime &
+RosTime::operator=(const Time & rhs)
+{
+  if (RCL_ROS_TIME != rhs.get_clock_type()) {
+    throw std::runtime_error("RosTime requires a clock type of RCL_ROS_TIME");
+  }
+  rcl_time_.nanoseconds = rhs.nanoseconds();
+  return *this;
+}
+
 }  // namespace rclcpp

rclcpp/test/rclcpp/test_time.cpp

@@ -918,3 +918,86 @@ TEST_F(TestClockStarted, started_timeout) {
   EXPECT_FALSE(ros_clock.wait_until_started());
   t.join();
 }
+
+TEST(TestRosTime, construct) {
+  rclcpp::RosTime t1{0};
+  rclcpp::RosTime t2{0, 1000};
+  EXPECT_EQ(0, t1.nanoseconds());
+  EXPECT_EQ(1000, t2.nanoseconds());
+}
+
+TEST(TestRosTime, convert_from_time) {
+  rclcpp::Time ros_t{0, 1000, RCL_ROS_TIME};
+  rclcpp::Time sys_t{0, 1000, RCL_SYSTEM_TIME};
+
+  {
+    rclcpp::RosTime rt{ros_t};
+    EXPECT_EQ(1000, rt.nanoseconds());
+  }
+  {
+    rclcpp::RosTime rt{5, 4321};
+    rt = ros_t;
+    EXPECT_EQ(1000, rt.nanoseconds());
+  }
+
+  RCLCPP_EXPECT_THROW_EQ(
+    rclcpp::RosTime rt2{rclcpp::Time{}},
+    std::runtime_error("RosTime requires a clock type of RCL_ROS_TIME"));
+}
+
+TEST(TestRosTime, assignment) {
+  rclcpp::RosTime rt1{0, 1000};
+  rclcpp::RosTime rt2{5, 4321};
+  rt2 = rt1;
+  EXPECT_EQ(1000, rt2.nanoseconds());
+}
+
+TEST(TestRosTime, comparison) {
+  rclcpp::RosTime rt1{0, 1000};
+  rclcpp::RosTime rt2{5, 4321};
+
+  EXPECT_TRUE(rt1 < rt2);
+  EXPECT_TRUE(rt1 <= rt2);
+  EXPECT_TRUE(rt2 > rt1);
+  EXPECT_TRUE(rt2 >= rt1);
+  EXPECT_TRUE(rt1 == rt1);
+  EXPECT_TRUE(rt2 == rt2);
+}
+
+TEST(TestRosTime, comparison_with_time) {
+  rclcpp::Time t{0, 1000, RCL_ROS_TIME};
+  rclcpp::RosTime rt{5, 4321};
+
+  EXPECT_TRUE(t < rt);
+  EXPECT_TRUE(t <= rt);
+  EXPECT_TRUE(rt > t);
+  EXPECT_TRUE(rt >= t);
+  EXPECT_TRUE(rt != t);
+
+  {
+    rclcpp::Time t2{5, 4321, RCL_ROS_TIME};
+    rclcpp::RosTime rt2{5, 4321};
+    EXPECT_TRUE(rt2 == t2);
+    EXPECT_FALSE(rt2 != t2);
+  }
+
+  rclcpp::Time sys_t;
+  RCLCPP_EXPECT_THROW_EQ(
+    rt == sys_t,
+    std::runtime_error("can't compare times with different time sources"));
+  RCLCPP_EXPECT_THROW_EQ(
+    rt != sys_t,
+    std::runtime_error("can't compare times with different time sources"));
+  RCLCPP_EXPECT_THROW_EQ(
+    rt < sys_t,
+    std::runtime_error("can't compare times with different time sources"));
+  RCLCPP_EXPECT_THROW_EQ(
+    rt <= sys_t,
+    std::runtime_error("can't compare times with different time sources"));
+  RCLCPP_EXPECT_THROW_EQ(
+    rt > sys_t,
+    std::runtime_error("can't compare times with different time sources"));
+  RCLCPP_EXPECT_THROW_EQ(
+    rt >= sys_t,
+    std::runtime_error("can't compare times with different time sources"));
+}