From 42fe666407e23f1d37741e768e7492dc25fef34b Mon Sep 17 00:00:00 2001
From: Barry Xu <barry.xu@sony.com>
Date: Thu, 16 Sep 2021 14:46:35 +0800
Subject: [PATCH] Add new interface for time conversion

Signed-off-by: Barry Xu <barry.xu@sony.com>
---
 CMakeLists.txt             |  3 ++
 include/rcpputils/time.hpp | 62 ++++++++++++++++++++++++++++++++++++++
 test/test_time.cpp         | 28 +++++++++++++++++
 3 files changed, 93 insertions(+)
 create mode 100644 include/rcpputils/time.hpp
 create mode 100644 test/test_time.cpp

diff --git a/CMakeLists.txt b/CMakeLists.txt
index b22d03b..ff55fb5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -69,6 +69,9 @@ if(BUILD_TESTING)
 
   ament_add_gtest(test_join test/test_join.cpp)
 
+  ament_add_gtest(test_time test/test_time.cpp)
+  ament_target_dependencies(test_time rcutils)
+
   ament_add_gtest(test_get_env test/test_get_env.cpp
     ENV
       EMPTY_TEST=
diff --git a/include/rcpputils/time.hpp b/include/rcpputils/time.hpp
new file mode 100644
index 0000000..bae36ff
--- /dev/null
+++ b/include/rcpputils/time.hpp
@@ -0,0 +1,62 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RCPPUTILS__TIME_HPP_
+#define RCPPUTILS__TIME_HPP_
+
+#include <chrono>
+
+#include "rcutils/time.h"
+
+#include "rcpputils/visibility_control.hpp"
+
+namespace rcpputils
+{
+
+/// Convert to rcutils duration value.
+/*
+ * \param[in] time The time to be converted to rcutils duration.
+ * \return rcutils duration value
+ * \throws std::invalid_argument if time is bigger than std::chrono::nanoseconds::max().
+ */
+template<typename DurationRepT = int64_t, typename DurationT = std::milli>
+RCPPUTILS_PUBLIC
+rcutils_duration_value_t convert_to_rcutils_duration(
+  const std::chrono::duration<DurationRepT, DurationT> & time)
+{
+  // Casting to a double representation might lose precision and allow the check below to succeed
+  // but the actual cast to nanoseconds fail. Using 1 DurationT worth of nanoseconds less than max
+  constexpr auto maximum_safe_cast_ns =
+    std::chrono::nanoseconds::max() - std::chrono::duration<DurationRepT, DurationT>(1);
+  // If period is greater than nanoseconds::max(), the duration_cast to nanoseconds will overflow
+  // a signed integer, which is undefined behavior. Checking whether any std::chrono::duration is
+  // greater than nanoseconds::max() is a difficult general problem. This is a more conservative
+  // version of Howard Hinnant's (the <chrono> guy>) response here:
+  // https://stackoverflow.com/a/44637334/2089061
+  // However, this doesn't solve the issue for all possible duration types of period.
+  // Follow-up issue: https://github.com/ros2/rclcpp/issues/1177
+  constexpr auto ns_max_as_double =
+    std::chrono::duration_cast<std::chrono::duration<double, std::chrono::nanoseconds::period>>(
+    maximum_safe_cast_ns);
+  if (time > ns_max_as_double) {
+    throw std::invalid_argument{
+            "time must be less than std::chrono::nanoseconds::max()"};
+  }
+
+  return (std::chrono::duration_cast<std::chrono::nanoseconds>(time)).count();
+}
+
+}  // namespace rcpputils
+
+#endif  // RCPPUTILS__TIME_HPP_
diff --git a/test/test_time.cpp b/test/test_time.cpp
new file mode 100644
index 0000000..c76f1d9
--- /dev/null
+++ b/test/test_time.cpp
@@ -0,0 +1,28 @@
+// Copyright 2021 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+
+#include <rcpputils/time.hpp>
+
+TEST(test_time, test_convert_to_rcutils_duration) {
+  rcutils_duration_value_t expect_value = RCUTILS_S_TO_NS(5 * 60);  // 5 minutes
+  rcutils_duration_value_t cast_val;
+  EXPECT_NO_THROW(cast_val = rcpputils::convert_to_rcutils_duration(std::chrono::minutes(5)));
+  EXPECT_EQ(cast_val, expect_value);
+
+  EXPECT_THROW(
+    rcpputils::convert_to_rcutils_duration(std::chrono::hours(10000000)),
+    std::invalid_argument);
+}