Add addition/subtraction assignment operators to Time (#748)

Signed-off-by: Scott K Logan <logans@cottsay.net>

rclcpp/include/rclcpp/time.hpp

@@ -98,6 +98,14 @@ public:
   Time
   operator-(const rclcpp::Duration & rhs) const;
 
+  RCLCPP_PUBLIC
+  Time &
+  operator+=(const rclcpp::Duration & rhs);
+
+  RCLCPP_PUBLIC
+  Time &
+  operator-=(const rclcpp::Duration & rhs);
+
   RCLCPP_PUBLIC
   rcl_time_point_value_t
   nanoseconds() const;

rclcpp/src/rclcpp/time.cpp

@@ -255,6 +255,36 @@ operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs)
   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();
+
+  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();
+
+  return *this;
+}
+
 Time
 Time::max()
 {

rclcpp/test/test_time.cpp

@@ -126,6 +126,10 @@ TEST(TestTime, operators) {
   EXPECT_EQ(sub.nanoseconds(), (rcl_duration_value_t)(young.nanoseconds() - old.nanoseconds()));
   EXPECT_EQ(sub, young - old);
 
+  rclcpp::Time young_changed(young);
+  young_changed -= rclcpp::Duration(old.nanoseconds());
+  EXPECT_EQ(sub.nanoseconds(), young_changed.nanoseconds());
+
   rclcpp::Time system_time(0, 0, RCL_SYSTEM_TIME);
   rclcpp::Time steady_time(0, 0, RCL_STEADY_TIME);
 
@@ -230,15 +234,19 @@ TEST(TestTime, overflows) {
   EXPECT_THROW(min_time - one, std::underflow_error);
   EXPECT_THROW(max_time - min_time, std::overflow_error);
   EXPECT_THROW(min_time - max_time, std::underflow_error);
+  EXPECT_THROW(rclcpp::Time(max_time) += one, std::overflow_error);
+  EXPECT_THROW(rclcpp::Time(min_time) -= one, std::underflow_error);
   EXPECT_NO_THROW(max_time - max_time);
   EXPECT_NO_THROW(min_time - min_time);
 
   // Cross zero in both directions
   rclcpp::Time one_time(1);
   EXPECT_NO_THROW(one_time - two);
+  EXPECT_NO_THROW(rclcpp::Time(one_time) -= two);
 
   rclcpp::Time minus_one_time(-1);
   EXPECT_NO_THROW(minus_one_time + two);
+  EXPECT_NO_THROW(rclcpp::Time(minus_one_time) += two);
 
   EXPECT_NO_THROW(one_time - minus_one_time);
   EXPECT_NO_THROW(minus_one_time - one_time);