feat: add/minus for msg::Time and rclcpp::Duration (#2419)

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

Signed-off-by: HuaTsai <[email protected]>

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/time.hpp

@@ -222,6 +222,15 @@ RCLCPP_PUBLIC
 Time
 operator+(const rclcpp::Duration & lhs, const rclcpp::Time & rhs);
 
+/// Convert rcl_time_point_value_t to builtin_interfaces::msg::Time
+/**
+ * \param[in] time_point is a rcl_time_point_value_t
+ * \return the builtin_interfaces::msg::Time from the time_point
+ */
+RCLCPP_PUBLIC
+builtin_interfaces::msg::Time
+convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point);
+
 }  // namespace rclcpp
 
 #endif  // RCLCPP__TIME_HPP_

rclcpp/src/rclcpp/duration.cpp

@@ -28,6 +28,8 @@
 
 #include "rcutils/logging_macros.h"
 
+#include "rclcpp/utilities.hpp"
+
 namespace rclcpp
 {
 
@@ -316,4 +318,50 @@ 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();
+
+  return convert_rcl_time_to_sec_nanos(rcl_time);
+}
+
+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();
+
+  return convert_rcl_time_to_sec_nanos(rcl_time);
+}
+
 }  // namespace rclcpp

rclcpp/src/rclcpp/time.cpp

@@ -90,17 +90,7 @@ 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);
-  }
-  return msg_time;
+  return convert_rcl_time_to_sec_nanos(rcl_time_.nanoseconds);
 }
 
 Time &
@@ -281,5 +271,20 @@ Time::max(rcl_clock_type_t clock_type)
   return Time(std::numeric_limits<int32_t>::max(), 999999999, clock_type);
 }
 
+builtin_interfaces::msg::Time
+convert_rcl_time_to_sec_nanos(const rcl_time_point_value_t & time_point)
+{
+  builtin_interfaces::msg::Time ret;
+  constexpr rcl_time_point_value_t kRemainder = RCL_S_TO_NS(1);
+  const auto result = std::div(time_point, 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,35 @@ TEST_F(TestDuration, operators) {
   EXPECT_TRUE(time == assignment_op_duration);
 }
 
+TEST_F(TestDuration, operators_with_message_stamp) {
+  builtin_interfaces::msg::Time time_msg = rclcpp::Time(0, 100000000u);  // 0.1s
+  rclcpp::Duration pos_duration(1, 100000000u);  // 1.1s
+  rclcpp::Duration neg_duration(-2, 900000000u);  // -1.1s
+
+  builtin_interfaces::msg::Time res_addpos = time_msg + pos_duration;
+  EXPECT_EQ(res_addpos.sec, 1);
+  EXPECT_EQ(res_addpos.nanosec, 200000000u);
+
+  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, 200000000u);
+
+  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);