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

Signed-off-by: HuaTsai <huatsai.eed07g@nctu.edu.tw>
ros2 · Jan 30, 2024 · 7c57ad5 · 7c57ad5
1 parent 265f5ec
commit 7c57ad5
Show file tree

Hide file tree

Showing 2 changed files with 72 additions and 0 deletions.
diff --git 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
@@ -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