From 7c57ad55e58d94590b57b0414cb03d196eacad7b Mon Sep 17 00:00:00 2001
From: HuaTsai <huatsai.eed07g@nctu.edu.tw>
Date: Tue, 30 Jan 2024 17:37:27 +0800
Subject: [PATCH] feat: add/minus for msg::Time and rclcpp::Duration
 Signed-off-by: HuaTsai <huatsai.eed07g@nctu.edu.tw>

---
 rclcpp/include/rclcpp/duration.hpp |  8 ++++
 rclcpp/src/rclcpp/duration.cpp     | 64 ++++++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)

diff --git a/rclcpp/include/rclcpp/duration.hpp b/rclcpp/include/rclcpp/duration.hpp
index 537e2a9bf6..bddd327e88 100644
--- a/rclcpp/include/rclcpp/duration.hpp
+++ b/rclcpp/include/rclcpp/duration.hpp
@@ -158,6 +158,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_
diff --git a/rclcpp/src/rclcpp/duration.cpp b/rclcpp/src/rclcpp/duration.cpp
index 2eac7f6b58..02d57e347a 100644
--- a/rclcpp/src/rclcpp/duration.cpp
+++ b/rclcpp/src/rclcpp/duration.cpp
@@ -316,4 +316,68 @@ 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