diff --git a/rclcpp/include/rclcpp/time.hpp b/rclcpp/include/rclcpp/time.hpp index c10b6ce..d8f65c8 100644 --- a/rclcpp/include/rclcpp/time.hpp +++ b/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; diff --git a/rclcpp/src/rclcpp/time.cpp b/rclcpp/src/rclcpp/time.cpp index b7f0bd2..6328916 100644 --- a/rclcpp/src/rclcpp/time.cpp +++ b/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() { diff --git a/rclcpp/test/test_time.cpp b/rclcpp/test/test_time.cpp index 22c9772..cc5cc32 100644 --- a/rclcpp/test/test_time.cpp +++ b/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);