feat: Add human readable date to logging formats. (ros2#510)

Backport of ros2#441

Signed-off-by: Tomoya Fujita <Tomoya.Fujita@sony.com>
Co-authored-by: Kaju Bubanja <bubanja.kaju@gmail.com>

Author: fujitatomoya

include/rcutils/time.h

@@ -143,6 +143,37 @@ rcutils_time_point_value_as_nanoseconds_string(
   char * str,
   size_t str_size);
 
+/// Return a time point as an datetime in local time with milliseconds in a string.
+/**
+ *
+ * If the given string is not large enough, the result will be truncated.
+ * If you need a string with variable width, using `snprintf()` directly is
+ * recommended.
+ *
+ * <hr>
+ * Attribute          | Adherence
+ * ------------------ | -------------
+ * Allocates Memory   | No [1]
+ * Thread-Safe        | Yes
+ * Uses Atomics       | No
+ * Lock-Free          | Yes
+ * <i>[1] if `snprintf()` does not allocate additional memory internally</i>
+ *
+ * \param[in] time_point the time to be made into a string
+ * \param[out] str the output string in which it is stored
+ * \param[in] str_size the size of the output string
+ * \return #RCUTILS_RET_OK if successful (even if truncated), or
+ * \return #RCUTILS_RET_INVALID_ARGUMENT if any arguments are invalid, or
+ * \return #RCUTILS_RET_ERROR if an unspecified error occur.
+ */
+RCUTILS_PUBLIC
+RCUTILS_WARN_UNUSED
+rcutils_ret_t
+rcutils_time_point_value_as_date_string(
+  const rcutils_time_point_value_t * time_point,
+  char * str,
+  size_t str_size);
+
 /// Return a time point as floating point seconds in a string.
 /**
  * The number is always fixed width, with left padding zeros up to the maximum

src/logging.c

@@ -593,6 +593,13 @@ const char * expand_time(
   APPEND_AND_RETURN_LOG_OUTPUT(numeric_storage);
 }
 
+const char * expand_time_as_date(
+  const logging_input * logging_input,
+  rcutils_char_array_t * logging_output)
+{
+  return expand_time(logging_input, logging_output, rcutils_time_point_value_as_date_string);
+}
+
 const char * expand_time_as_seconds(
   const logging_input * logging_input,
   rcutils_char_array_t * logging_output)
@@ -686,6 +693,7 @@ static const token_map_entry tokens[] = {
   {.token = "time", .handler = expand_time_as_seconds},
   {.token = "time_as_nanoseconds", .handler = expand_time_as_nanoseconds},
   {.token = "line_number", .handler = expand_line_number},
+  {.token = "date_time_with_ms", .handler = expand_time_as_date},
 };
 
 token_handler find_token_handler(const char * token)

src/time.c

@@ -20,9 +20,9 @@ extern "C"
 #include "rcutils/time.h"
 
 #include <inttypes.h>
-#include <stdint.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <time.h>
 
 #include "rcutils/allocator.h"
 #include "rcutils/error_handling.h"
@@ -46,6 +46,61 @@ rcutils_time_point_value_as_nanoseconds_string(
   return RCUTILS_RET_OK;
 }
 
+rcutils_ret_t
+rcutils_time_point_value_as_date_string(
+  const rcutils_time_point_value_t * time_point,
+  char * str,
+  size_t str_size)
+{
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(time_point, RCUTILS_RET_INVALID_ARGUMENT);
+  RCUTILS_CHECK_ARGUMENT_FOR_NULL(str, RCUTILS_RET_INVALID_ARGUMENT);
+  if (0 == str_size) {
+    return RCUTILS_RET_OK;
+  }
+  // best to abs it to avoid issues with negative values in C89, see:
+  //   https://stackoverflow.com/a/3604984/671658
+  uint64_t abs_time_point = (uint64_t)llabs(*time_point);
+  // break into two parts to avoid floating point error
+  uint64_t seconds = abs_time_point / (1000u * 1000u * 1000u);
+  uint64_t nanoseconds = abs_time_point % (1000u * 1000u * 1000u);
+  // Make sure the buffer is large enough to hold the largest possible uint64_t
+  char nanoseconds_str[21];
+
+  if (rcutils_snprintf(nanoseconds_str, sizeof(nanoseconds_str), "%" PRIu64, nanoseconds) < 0) {
+    RCUTILS_SET_ERROR_MSG("failed to format time point nanoseconds into string");
+    return RCUTILS_RET_ERROR;
+  }
+
+  time_t now_t = (time_t)(seconds);
+  struct tm ptm = {.tm_year = 0, .tm_mday = 0};
+#ifdef _WIN32
+  if (localtime_s(&ptm, &now_t) != 0) {
+    RCUTILS_SET_ERROR_MSG("failed to get localtime");
+    return RCUTILS_RET_ERROR;
+  }
+#else
+  if (localtime_r(&now_t, &ptm) == NULL) {
+    RCUTILS_SET_ERROR_MSG("failed to get localtime");
+    return RCUTILS_RET_ERROR;
+  }
+#endif
+
+  if (str_size < 32 || strftime(str, 32, "%Y-%m-%d %H:%M:%S", &ptm) == 0) {
+    RCUTILS_SET_ERROR_MSG("failed to format time point into string as iso8601_date");
+    return RCUTILS_RET_ERROR;
+  }
+  static const int date_end_position = 19;
+  if (rcutils_snprintf(
+      &str[date_end_position], str_size - date_end_position, ".%.3s",
+      nanoseconds_str) < 0)
+  {
+    RCUTILS_SET_ERROR_MSG("failed to format time point into string as date_time_with_ms");
+    return RCUTILS_RET_ERROR;
+  }
+
+  return RCUTILS_RET_OK;
+}
+
 rcutils_ret_t
 rcutils_time_point_value_as_seconds_string(
   const rcutils_time_point_value_t * time_point,

test/test_time.cpp

@@ -301,6 +301,46 @@ TEST_F(TestTimeFixture, test_rcutils_time_point_value_as_nanoseconds_string) {
   EXPECT_STREQ("-0000000000000000100", buffer);
 }
 
+// Tests the rcutils_time_point_value_as_date_string() function.
+TEST_F(TestTimeFixture, test_rcutils_time_point_value_as_date_string) {
+  rcutils_ret_t ret;
+  rcutils_time_point_value_t timepoint;
+  char buffer[256] = "";
+
+  // Typical use case.
+  timepoint = 100;
+  ret = rcutils_time_point_value_as_date_string(&timepoint, buffer, sizeof(buffer));
+  EXPECT_EQ(RCUTILS_RET_OK, ret) << rcutils_get_error_string().str;
+  std::tm t = {};
+  std::istringstream ss(buffer);
+  // To test that it works we call it once with the correct format string
+  ss >> std::get_time(&t, "%Y-%m-%d %H:%M:%S");
+  ASSERT_FALSE(ss.fail());
+  std::istringstream ss2(buffer);
+  // and once with the false one
+  ss2 >> std::get_time(&t, "%Y-%b-%d %H:%M:%S");
+  ASSERT_TRUE(ss2.fail());
+
+  // nullptr for timepoint
+  ret = rcutils_time_point_value_as_date_string(nullptr, buffer, sizeof(buffer));
+  EXPECT_EQ(RCUTILS_RET_INVALID_ARGUMENT, ret);
+  rcutils_reset_error();
+
+  // nullptr for string
+  timepoint = 100;
+  ret = rcutils_time_point_value_as_date_string(&timepoint, nullptr, 0);
+  EXPECT_EQ(RCUTILS_RET_INVALID_ARGUMENT, ret);
+  rcutils_reset_error();
+
+  const char * test_str = "should not be touched";
+  timepoint = 100;
+  // buffer is of size 256, so it will fit
+  (void)memmove(buffer, test_str, strlen(test_str) + 1);
+  ret = rcutils_time_point_value_as_date_string(&timepoint, buffer, 0);
+  EXPECT_EQ(RCUTILS_RET_OK, ret) << rcutils_get_error_string().str;
+  EXPECT_STREQ(test_str, buffer);
+}
+
 // Tests the rcutils_time_point_value_as_seconds_string() function.
 TEST_F(TestTimeFixture, test_rcutils_time_point_value_as_seconds_string) {
   rcutils_ret_t ret;