feat: add/minus for msg::Time and rclcpp::Duration

rclcpp/include/rclcpp/duration.hpp

@@ -18,6 +18,7 @@
 #include <chrono>
 
 #include "builtin_interfaces/msg/duration.hpp"
+#include "builtin_interfaces/msg/time.hpp"
 #include "rcl/time.h"
 #include "rclcpp/visibility_control.hpp"
 
@@ -158,6 +159,14 @@ class RCLCPP_PUBLIC Duration
   Duration() = default;
 };
 
+RCLCPP_PUBLIC
+builtin_interfaces::msg::Time
+operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs);
+
+RCLCPP_PUBLIC
+builtin_interfaces::msg::Time
+operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__DURATION_HPP_

rclcpp/src/rclcpp/duration.cpp

@@ -28,6 +28,8 @@
 
 #include "rcutils/logging_macros.h"
 
+#include "rclcpp/utilities.hpp"
+
 namespace rclcpp
 {
 
@@ -316,4 +318,70 @@ Duration::from_nanoseconds(rcl_duration_value_t nanoseconds)
   return ret;
 }
 
+builtin_interfaces::msg::Time
+operator+(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs)
+{
+  if (lhs.sec < 0) {
+    throw std::runtime_error("message time is negative");
+  }
+
+  rcl_time_point_value_t rcl_time;
+  rcl_time = RCL_S_TO_NS(static_cast<int64_t>(lhs.sec));
+  rcl_time += lhs.nanosec;
+
+  if (rclcpp::add_will_overflow(rcl_time, rhs.nanoseconds())) {
+    throw std::overflow_error("addition leads to int64_t overflow");
+  }
+  if (rclcpp::add_will_underflow(rcl_time, rhs.nanoseconds())) {
+    throw std::underflow_error("addition leads to int64_t underflow");
+  }
+
+  rcl_time += rhs.nanoseconds();
+
+  builtin_interfaces::msg::Time ret;
+  constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
+  const auto result = std::div(rcl_time, 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;
+}
+
+builtin_interfaces::msg::Time
+operator-(const builtin_interfaces::msg::Time & lhs, const rclcpp::Duration & rhs)
+{
+  if (lhs.sec < 0) {
+    throw std::runtime_error("message time is negative");
+  }
+
+  rcl_time_point_value_t rcl_time;
+  rcl_time = RCL_S_TO_NS(static_cast<int64_t>(lhs.sec));
+  rcl_time += lhs.nanosec;
+
+  if (rclcpp::sub_will_overflow(rcl_time, rhs.nanoseconds())) {
+    throw std::overflow_error("addition leads to int64_t overflow");
+  }
+  if (rclcpp::sub_will_underflow(rcl_time, rhs.nanoseconds())) {
+    throw std::underflow_error("addition leads to int64_t underflow");
+  }
+
+  rcl_time -= rhs.nanoseconds();
+
+  builtin_interfaces::msg::Time ret;
+  constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
+  const auto result = std::div(rcl_time, 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

rclcpp/test/rclcpp/test_duration.cpp

@@ -79,6 +79,37 @@ TEST_F(TestDuration, operators) {
   EXPECT_TRUE(time == assignment_op_duration);
 }
 
+TEST_F(TestDuration, operators_with_message_stamp) {
+  builtin_interfaces::msg::Time time_msg;  // 0.1s
+  time_msg.sec = 0;
+  time_msg.nanosec = 100000000;
+  rclcpp::Duration pos_duration(1, 100000000);  // 1.1s
+  rclcpp::Duration neg_duration(-2, 900000000);  // -1.1s
+
+  builtin_interfaces::msg::Time res_addpos = time_msg + pos_duration;
+  EXPECT_EQ(res_addpos.sec, 1);
+  EXPECT_EQ(res_addpos.nanosec, 200000000);
+
+  builtin_interfaces::msg::Time res_addneg = time_msg + neg_duration;
+  EXPECT_EQ(res_addneg.sec, -1);
+  EXPECT_EQ(res_addneg.nanosec, 0);
+
+  builtin_interfaces::msg::Time res_subpos = time_msg - pos_duration;
+  EXPECT_EQ(res_subpos.sec, -1);
+  EXPECT_EQ(res_subpos.nanosec, 0);
+
+  builtin_interfaces::msg::Time res_subneg = time_msg - neg_duration;
+  EXPECT_EQ(res_subneg.sec, 1);
+  EXPECT_EQ(res_subneg.nanosec, 200000000);
+
+  builtin_interfaces::msg::Time neg_time_msg;
+  neg_time_msg.sec = -1;
+  auto max = rclcpp::Duration::from_nanoseconds(std::numeric_limits<rcl_duration_value_t>::max());
+
+  EXPECT_THROW(neg_time_msg + max, std::runtime_error);
+  EXPECT_THROW(time_msg + max, std::overflow_error);
+}
+
 TEST_F(TestDuration, chrono_overloads) {
   int64_t ns = 123456789l;
   auto chrono_ns = std::chrono::nanoseconds(ns);