add simple timer package needed to build

src/simple_timer/CMakeLists.txt

@@ -0,0 +1,61 @@
+cmake_minimum_required(VERSION 3.5)
+project(simple_timer)
+
+# Default to C99
+if(NOT CMAKE_C_STANDARD)
+  set(CMAKE_C_STANDARD 99)
+endif()
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+# uncomment the following section in order to fill in
+# further dependencies manually.
+# find_package(<dependency> REQUIRED)
+
+add_library(simple_timer src/cycle_timer.cpp src/period_timer.cpp)
+target_include_directories(simple_timer PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+add_library(rt-sched src/rt-sched.cpp)
+target_include_directories(rt-sched PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS simple_timer rt-sched
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+ament_export_include_directories(include)
+ament_export_libraries(simple_timer)
+ament_export_libraries(rt-sched)
+ament_package()

src/simple_timer/include/simple_timer/cycle_timer.hpp

@@ -0,0 +1,23 @@
+#ifndef __CYCLE_TIMER__
+#define __CYCLE_TIMER__
+
+#include <memory>
+#include "simple_timer/rt-sched.hpp"
+namespace simple_timer
+{
+    class CycleTimer
+    {
+    public:
+        CycleTimer(long start_delay=0);
+        void tick() ;
+        const u64 start_delay_time;
+        u64 start_time = 0;
+        u64 last_cycle_time = 0;
+        unsigned long max_diff = 0;
+        unsigned long min_diff = 0;
+        unsigned long last_diff = 0;
+        bool recording = false;
+    };
+} // namespace simple_timer
+
+#endif

src/simple_timer/include/simple_timer/period_timer.hpp

@@ -0,0 +1,25 @@
+#ifndef __PERIOD_TIMER__
+#define __PERIOD_TIMER__
+
+#include <memory>
+#include "simple_timer/rt-sched.hpp"
+namespace simple_timer
+{
+    class PeriodTimer
+    {
+    public:
+        PeriodTimer(long start_delay = 0);
+        void start();
+        void stop();
+        const u64 start_delay_time;
+        u64 start_time = 0;
+
+        u64 last_period_time = 0;
+        unsigned long max_period = 0;
+        unsigned long min_period = 0;
+        unsigned long last_period = 0;
+        bool recording = false;
+    };
+} // namespace simple_timer
+
+#endif

src/simple_timer/include/simple_timer/rt-sched.hpp

@@ -0,0 +1,98 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * rt-sched.h - sched_setattr() and sched_getattr() API
+ * (C) Dario Faggioli <raistlin@linux.it>, 2009, 2010
+ * Copyright (C) 2014 BMW Car IT GmbH, Daniel Wagner <daniel.wagner@bmw-carit.de
+ */
+
+/* This file is based on Dario Faggioli's libdl. Eventually it will be
+   replaced by a proper implemenation of this API. */
+
+#ifndef __RT_SCHED_H__
+#define __RT_SCHED_H__
+
+#include <stdint.h>
+#include <sys/types.h>
+#include <mutex>
+#include <vector>
+#include <memory>
+
+#ifndef SCHED_DEADLINE
+#define SCHED_DEADLINE 6
+#endif
+
+#ifdef __x86_64__
+#define __NR_sched_setattr		314
+#define __NR_sched_getattr		315
+#endif
+
+#ifdef __i386__
+#define __NR_sched_setattr		351
+#define __NR_sched_getattr		352
+#endif
+
+#ifdef __arm__
+#ifndef __NR_sched_setattr
+#define __NR_sched_setattr		380
+#endif
+#ifndef __NR_sched_getattr
+#define __NR_sched_getattr		381
+#endif
+#endif
+
+#ifdef __tilegx__
+#define __NR_sched_setattr		274
+#define __NR_sched_getattr		275
+#endif
+
+typedef unsigned long long u64;
+#define NS_TO_MS 1000000
+struct sched_attr {
+	uint32_t size;
+	uint32_t sched_policy;
+	uint64_t sched_flags;
+
+	/* SCHED_NORMAL, SCHED_BATCH */
+	int32_t sched_nice;
+
+	/* SCHED_FIFO, SCHED_RR */
+	uint32_t sched_priority;
+
+	/* SCHED_DEADLINE */
+	uint64_t sched_runtime;
+	uint64_t sched_deadline;
+	uint64_t sched_period;
+};
+
+int sched_setattr(pid_t pid,
+		  const struct sched_attr *attr,
+		  unsigned int flags);
+
+int sched_getattr(pid_t pid,
+		  struct sched_attr *attr,
+		  unsigned int size,
+		  unsigned int flags);
+
+u64 get_time_us(void);
+
+typedef struct node_time_logger
+{
+	std::shared_ptr<std::vector<std::pair<std::string, u64>>> recorded_times;
+} node_time_logger;
+
+void log_entry(node_time_logger logger, std::string text);
+node_time_logger create_logger();
+
+inline u64 get_time_us(void)
+{
+	struct timespec ts;
+	unsigned long long time;
+
+	clock_gettime(CLOCK_MONOTONIC_RAW, &ts);
+	time = ts.tv_sec * 1000000;
+	time += ts.tv_nsec / 1000;
+
+	return time;
+}
+
+#endif /* __RT_SCHED_H__ */

src/simple_timer/package.xml

@@ -0,0 +1,18 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>simple_timer</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="kurt4wilson@gmail.com">nvidia</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/simple_timer/src/cycle_timer.cpp

@@ -0,0 +1,43 @@
+#include "simple_timer/cycle_timer.hpp"
+
+namespace simple_timer
+{
+  CycleTimer::CycleTimer(long start_delay) : start_delay_time(start_delay * 1000)
+  {
+  }
+
+  void CycleTimer::tick()
+  {
+    u64 current_wall_time = get_time_us();
+    u64 time_diff = 0;
+
+    if (!recording)
+    {
+
+      if (start_time == 0)
+      {
+        start_time = current_wall_time;
+      }
+      else if (current_wall_time - start_time > start_delay_time)
+      {
+        recording = true;
+        last_cycle_time = current_wall_time;
+        start_time = current_wall_time;
+      }
+    }
+    else
+    {
+      time_diff = current_wall_time - last_cycle_time;
+      if (time_diff < min_diff || min_diff == 0)
+      {
+        min_diff = time_diff;
+      }
+      if (time_diff > max_diff || max_diff == 0)
+      {
+        max_diff = time_diff;
+      }
+      last_cycle_time = current_wall_time;
+      last_diff = time_diff;
+    }
+  }
+} // namespace simple_timer

src/simple_timer/src/period_timer.cpp

@@ -0,0 +1,56 @@
+
+#include "simple_timer/period_timer.hpp"
+
+namespace simple_timer
+{
+    PeriodTimer::PeriodTimer(long start_delay) : start_delay_time(start_delay * 1000)
+    {
+    }
+
+    void PeriodTimer::start()
+    {
+        u64 current_wall_time = get_time_us();
+
+        if (!recording)
+        {
+
+            if (start_time == 0)
+            {
+                start_time = current_wall_time;
+            }
+            else if (current_wall_time - start_time > start_delay_time)
+            {
+                recording = true;
+                start_time = current_wall_time;
+                last_period_time = current_wall_time;
+            }
+        }
+        else
+        {
+            last_period_time = current_wall_time;
+        }
+    }
+  void PeriodTimer::stop()
+  {
+    u64 current_wall_time = get_time_us();
+    u64 time_diff = 0;
+
+    if (!recording)
+    {
+        return;
+    }
+    else
+    {
+      time_diff = current_wall_time - last_period_time;
+      if (time_diff < min_period || min_period == 0)
+      {
+        min_period = time_diff;
+      }
+      if (time_diff > max_period || max_period == 0)
+      {
+        max_period = time_diff;
+      }
+      last_period = time_diff;
+    }
+  }
+} // namespace simple_timer

src/simple_timer/src/rt-sched.cpp

@@ -0,0 +1,50 @@
+// SPDX-License-Identifier: GPL-2.0-or-later
+/*
+ * rt-sched.h - sched_setattr() and sched_getattr() API
+ *
+ * (C) Dario Faggioli <raistlin@linux.it>, 2009, 2010
+ * Copyright (C) 2014 BMW Car IT GmbH, Daniel Wagner <daniel.wagner@bmw-carit.de
+ */
+
+/* This file is based on Dario Faggioli's libdl. Eventually it will be
+   replaced by a proper implemenation of this API. */
+
+#include <unistd.h>
+#include <sys/syscall.h>
+#include <time.h>
+#include <iostream>
+
+#include "simple_timer/rt-sched.hpp"
+
+int sched_setattr(pid_t pid,
+		  const struct sched_attr *attr,
+		  unsigned int flags)
+{
+	return syscall(__NR_sched_setattr, pid, attr, flags);
+}
+
+int sched_getattr(pid_t pid,
+		  struct sched_attr *attr,
+		  unsigned int size,
+		  unsigned int flags)
+{
+	return syscall(__NR_sched_getattr, pid, attr, size, flags);
+}
+
+void log_entry(node_time_logger logger, std::string text)
+{
+    if (logger.recorded_times != nullptr)
+    {
+        logger.recorded_times->push_back(std::make_pair(text, get_time_us() / 1000));
+        // std::cout<<text<<std::endl;
+    }else{
+        // TODO: report error
+    }
+}
+
+node_time_logger create_logger()
+{
+    node_time_logger logger;
+    logger.recorded_times = std::make_shared<std::vector<std::pair<std::string, u64>>>();
+    return logger;
+}