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/include/rclcpp/utilities.hpp

@@ -19,6 +19,7 @@
 #include <functional>
 #include <limits>
 #include <string>
+#include <utility>
 #include <vector>
 
 #include "rclcpp/context.hpp"
@@ -330,6 +331,14 @@ RCLCPP_PUBLIC
 std::vector<const char *>
 get_c_vector_string(const std::vector<std::string> & strings_in);
 
+/// Return the std::pair of seconds and nanoseconds from the given rcl_time_point_value_t.
+/**
+ * \param[in] time_point is a rcl_time_point_value_t
+ * \return the std::pair of seconds and nanoseconds from the rcl_time_point_value_t
+ */
+std::pair<int32_t, uint32_t>
+convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__UTILITIES_HPP_

rclcpp/src/rclcpp/duration.cpp

@@ -28,6 +28,8 @@
 
 #include "rcutils/logging_macros.h"
 
+#include "rclcpp/utilities.hpp"
+
 namespace rclcpp
 {
 
@@ -316,4 +318,58 @@ 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;
+  auto sec_nanos = convert_rcl_time_to_sec_nanos(rcl_time);
+  ret.sec = sec_nanos.first;
+  ret.nanosec = sec_nanos.second;
+  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;
+  auto sec_nanos = convert_rcl_time_to_sec_nanos(rcl_time);
+  ret.sec = sec_nanos.first;
+  ret.nanosec = sec_nanos.second;
+  return ret;
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/time.cpp

@@ -91,15 +91,9 @@ Time::~Time()
 Time::operator builtin_interfaces::msg::Time() const
 {
   builtin_interfaces::msg::Time msg_time;
-  constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
-  const auto result = std::div(rcl_time_.nanoseconds, kRemainder);
-  if (result.rem >= 0) {
-    msg_time.sec = static_cast<std::int32_t>(result.quot);
-    msg_time.nanosec = static_cast<std::uint32_t>(result.rem);
-  } else {
-    msg_time.sec = static_cast<std::int32_t>(result.quot - 1);
-    msg_time.nanosec = static_cast<std::uint32_t>(kRemainder + result.rem);
-  }
+  auto sec_nanos = convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds);
+  msg_time.sec = sec_nanos.first;
+  msg_time.nanosec = sec_nanos.second;
   return msg_time;
 }
 

rclcpp/src/rclcpp/utilities.cpp

@@ -26,6 +26,7 @@
 
 #include "rcl/error_handling.h"
 #include "rcl/rcl.h"
+#include "rcl/time.h"
 
 namespace rclcpp
 {
@@ -227,4 +228,21 @@ get_c_vector_string(const std::vector<std::string> & strings_in)
   return cstrings;
 }
 
+std::pair<int32_t, uint32_t>
+convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point)
+{
+  int32_t seconds;
+  uint32_t nanoseconds;
+  constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
+  const auto result = std::div(time_point, kRemainder);
+  if (result.rem >= 0) {
+    seconds = static_cast<std::int32_t>(result.quot);
+    nanoseconds = static_cast<std::uint32_t>(result.rem);
+  } else {
+    seconds = static_cast<std::int32_t>(result.quot - 1);
+    nanoseconds = static_cast<std::uint32_t>(kRemainder + result.rem);
+  }
+  return {seconds, nanoseconds};
+}
+
 }  // 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);