misc fixed & improvements. auto-set chain deadline queue. update usage example

src/priority_executor/CMakeLists.txt

@@ -105,4 +105,5 @@ ament_export_include_directories(include)
 ament_export_libraries(priority_executor)
 ament_export_libraries(multithread_priority_executor)
 ament_export_libraries(default_executor)
+ament_export_dependencies(rclcpp rcl simple_timer)
 ament_package()

src/priority_executor/include/priority_executor/priority_memory_strategy.hpp

@@ -15,11 +15,13 @@
 #ifndef RTIS_PRIORITY_STRATEGY
 #define RTIS_PRIORITY_STRATEGY
 
-#include <memory>
+#include <cstdint>
-#include <vector>
-#include <time.h>
-#include <set>
 #include <deque>
+#include <memory>
+#include <semaphore.h>
+#include <set>
+#include <time.h>
+#include <vector>
 
 #include "rcl/allocator.h"
 
@@ -30,9 +32,9 @@
 
 /// Delegate for handling memory allocations while the Executor is executing.
 /**
- * By default, the memory strategy dynamically allocates memory for structures that come in from
+ * By default, the memory strategy dynamically allocates memory for structures
- * the rmw implementation after the executor waits for work, based on the number of entities that
+ * that come in from the rmw implementation after the executor waits for work,
- * come through.
+ * based on the number of entities that come through.
  */
 
 enum ExecutableType
@@ -61,7 +63,7 @@ public:
   std::shared_ptr<rclcpp::Waitable> waitable;
   ExecutableScheduleType sched_type;
 
-    int priority;
+  int priority = 0;
   long period = 1000; // milliseconds
 
   bool is_first_in_chain = false;
@@ -75,7 +77,9 @@ public:
   // chain aware priority
   int counter = 0;
 
-    PriorityExecutable(std::shared_ptr<const void> h, int p, ExecutableType t, ExecutableScheduleType sched_type = CHAIN_INDEPENDENT_PRIORITY);
+  PriorityExecutable(
+      std::shared_ptr<const void> h, int p, ExecutableType t,
+      ExecutableScheduleType sched_type = CHAIN_INDEPENDENT_PRIORITY);
 
   void dont_run();
 
@@ -91,6 +95,24 @@ public:
   int executable_id = 0;
 
   std::string name = "";
+
+  // stream operator for debug
+  friend std::ostream &operator<<(std::ostream &os,
+                                  const PriorityExecutable &pe)
+  {
+    os << "sched_type: " << pe.sched_type << ", ";
+    if (pe.sched_type == DEADLINE)
+    {
+      os << "period: " << pe.period << ", ";
+    }
+    os << "priority: " << pe.priority << ", ";
+    os << "executable_id: " << pe.executable_id << ", ";
+    os << "chain_id: " << pe.chain_id << ", ";
+    os << "is_first_in_chain: " << pe.is_first_in_chain << ", ";
+    os << "is_last_in_chain: " << pe.is_last_in_chain << ", ";
+
+    return os;
+  }
 };
 
 class PriorityExecutableComparator
@@ -105,7 +127,8 @@ class PriorityMemoryStrategy : public rclcpp::memory_strategy::MemoryStrategy
 public:
   RCLCPP_SMART_PTR_DEFINITIONS(PriorityMemoryStrategy<Alloc>)
 
-    using VoidAllocTraits = typename rclcpp::allocator::AllocRebind<void *, Alloc>;
+  using VoidAllocTraits =
+      typename rclcpp::allocator::AllocRebind<void *, Alloc>;
   using VoidAlloc = typename VoidAllocTraits::allocator_type;
 
   explicit PriorityMemoryStrategy(std::shared_ptr<Alloc> allocator)
@@ -121,9 +144,11 @@ public:
   }
   node_time_logger logger_;
 
-    void add_guard_condition(const rcl_guard_condition_t *guard_condition) override;
+  void
+  add_guard_condition(const rcl_guard_condition_t *guard_condition) override;
 
-    void remove_guard_condition(const rcl_guard_condition_t *guard_condition) override;
+  void
+  remove_guard_condition(const rcl_guard_condition_t *guard_condition) override;
 
   void clear_handles() override;
 
@@ -131,13 +156,12 @@ public:
 
   bool collect_entities(const WeakNodeList &weak_nodes) override;
 
-    void add_waitable_handle(const rclcpp::Waitable::SharedPtr &waitable) override;
+  void
+  add_waitable_handle(const rclcpp::Waitable::SharedPtr &waitable) override;
 
   bool add_handles_to_wait_set(rcl_wait_set_t *wait_set) override;
 
-    void
+  void get_next_executable(rclcpp::AnyExecutable &any_exec,
-    get_next_executable(
-        rclcpp::AnyExecutable &any_exec,
                            const WeakNodeList &weak_nodes);
 
   /**
@@ -145,28 +169,23 @@ public:
    */
   void post_execute(rclcpp::AnyExecutable any_exec, int thread_id = -1);
 
-    void
+  void get_next_subscription(rclcpp::AnyExecutable &any_exec,
-    get_next_subscription(
-        rclcpp::AnyExecutable &any_exec,
                              const WeakNodeList &weak_nodes) override;
 
-    void
+  void get_next_service(rclcpp::AnyExecutable &any_exec,
-    get_next_service(
-        rclcpp::AnyExecutable &any_exec,
                         const WeakNodeList &weak_nodes) override;
 
-    void
+  void get_next_client(rclcpp::AnyExecutable &any_exec,
-    get_next_client(rclcpp::AnyExecutable &any_exec, const WeakNodeList &weak_nodes) override;
-
-    void
-    get_next_timer(
-        rclcpp::AnyExecutable &any_exec,
                        const WeakNodeList &weak_nodes) override;
 
-    void
+  void get_next_timer(rclcpp::AnyExecutable &any_exec,
-    get_next_waitable(rclcpp::AnyExecutable &any_exec, const WeakNodeList &weak_nodes) override;
+                      const WeakNodeList &weak_nodes) override;
 
-    std::shared_ptr<PriorityExecutable> get_priority_settings(std::shared_ptr<const void> executable)
+  void get_next_waitable(rclcpp::AnyExecutable &any_exec,
+                         const WeakNodeList &weak_nodes) override;
+
+  std::shared_ptr<PriorityExecutable>
+  get_priority_settings(std::shared_ptr<const void> executable)
   {
     auto search = priority_map.find(executable);
     if (search != priority_map.end())
@@ -181,7 +200,8 @@ public:
 
   rcl_allocator_t get_allocator() override
   {
-        return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(*allocator_.get());
+    return rclcpp::allocator::get_rcl_allocator<void *, VoidAlloc>(
+        *allocator_.get());
   }
 
   size_t number_of_ready_subscriptions() const override
@@ -230,7 +250,8 @@ public:
     size_t number_of_guard_conditions = guard_conditions_.size();
     for (auto waitable : waitable_handles_)
     {
-            number_of_guard_conditions += waitable->get_number_of_ready_guard_conditions();
+      number_of_guard_conditions +=
+          waitable->get_number_of_ready_guard_conditions();
     }
     return number_of_guard_conditions;
   }
@@ -250,24 +271,39 @@ public:
     return waitable_handles_.size();
   }
 
-    void set_executable_priority(std::shared_ptr<const void> handle, int priority, ExecutableType t)
+  void set_executable_priority(std::shared_ptr<const void> handle, int priority,
+                               ExecutableType t)
   {
     // TODO: any sanity checks should go here
-        priority_map[handle] = std::make_shared<PriorityExecutable>(handle, priority, t);
+    priority_map[handle] =
+        std::make_shared<PriorityExecutable>(handle, priority, t);
   }
-    void set_executable_priority(std::shared_ptr<const void> handle, int priority, ExecutableType t, ExecutableScheduleType sc, int chain_index)
+  void set_executable_priority(std::shared_ptr<const void> handle, int priority,
+                               ExecutableType t, ExecutableScheduleType sc,
+                               int chain_index)
   {
     // TODO: any sanity checks should go here
-        priority_map[handle] = std::make_shared<PriorityExecutable>(handle, priority, t, sc);
+    priority_map[handle] =
+        std::make_shared<PriorityExecutable>(handle, priority, t, sc);
     priority_map[handle]->chain_id = chain_index;
   }
 
-    void set_executable_deadline(std::shared_ptr<const void> handle, int period, ExecutableType t, int chain_id = 0, std::string name = "")
+  void set_executable_deadline(std::shared_ptr<const void> handle, int period,
+                               ExecutableType t, int chain_id = 0,
+                               std::string name = "")
   {
     // TODO: any sanity checks should go here
-        priority_map[handle] = std::make_shared<PriorityExecutable>(handle, period, t, DEADLINE);
+    priority_map[handle] =
+        std::make_shared<PriorityExecutable>(handle, period, t, DEADLINE);
     priority_map[handle]->chain_id = chain_id;
     priority_map[handle]->name = name;
+    // is there a deadline queue for this chain id?
+    auto search = chain_deadlines.find(chain_id);
+    if (search == chain_deadlines.end())
+    {
+      chain_deadlines[chain_id] = std::make_shared<std::deque<uint64_t>>();
+    }
+    priority_map[handle]->deadlines = chain_deadlines[chain_id].get();
   }
 
   int get_priority(std::shared_ptr<const void> executable)
@@ -283,7 +319,6 @@ public:
     }
   }
 
-
   void set_first_in_chain(std::shared_ptr<const void> exec_handle)
   {
     get_priority_settings(exec_handle)->is_first_in_chain = true;
@@ -294,20 +329,37 @@ public:
     get_priority_settings(exec_handle)->is_last_in_chain = true;
   }
 
-    void assign_deadlines_queue(std::shared_ptr<const void> exec_handle, std::deque<uint64_t> *deadlines)
+  void assign_deadlines_queue(std::shared_ptr<const void> exec_handle,
+                              std::deque<uint64_t> *deadlines)
   {
     get_priority_settings(exec_handle)->deadlines = deadlines;
   }
 
+  std::shared_ptr<std::deque<uint64_t>> get_chain_deadlines(int chain_id)
+  {
+    auto search = chain_deadlines.find(chain_id);
+    if (search != chain_deadlines.end())
+    {
+      return search->second;
+    }
+    else
+    {
+      return nullptr;
+    }
+  }
+
 private:
-    std::shared_ptr<PriorityExecutable> get_and_reset_priority(std::shared_ptr<const void> executable, ExecutableType t)
+  std::shared_ptr<PriorityExecutable>
+  get_and_reset_priority(std::shared_ptr<const void> executable,
+                         ExecutableType t)
   {
     // PriorityExecutable *p = get_priority_settings(executable);
     std::shared_ptr<PriorityExecutable> p = get_priority_settings(executable);
     if (p == nullptr)
     {
       // priority_map[executable] = PriorityExecutable(executable, 0, t);
-            priority_map[executable] = std::make_shared<PriorityExecutable>(executable, 0, t);
+      priority_map[executable] =
+          std::make_shared<PriorityExecutable>(executable, 0, t);
       p = priority_map[executable];
     }
     // p->can_be_run = true;
@@ -315,8 +367,8 @@ private:
   }
 
   template <typename T>
-    using VectorRebind =
+  using VectorRebind = std::vector<
-        std::vector<T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
+      T, typename std::allocator_traits<Alloc>::template rebind_alloc<T>>;
 
   VectorRebind<const rcl_guard_condition_t *> guard_conditions_;
 
@@ -331,15 +383,23 @@ private:
   // TODO: evaluate using node/subscription namespaced strings as keys
 
   // holds *all* handle->priority mappings
-    std::map<std::shared_ptr<const void>, std::shared_ptr<PriorityExecutable>> priority_map;
+  std::map<std::shared_ptr<const void>, std::shared_ptr<PriorityExecutable>>
+      priority_map;
+
+  std::map<int, std::shared_ptr<std::deque<uint64_t>>> chain_deadlines;
 
   // hold *only valid* executable+priorities
-    // std::priority_queue<const PriorityExecutable *, std::vector<const PriorityExecutable *>, PriorityExecutableComparator> all_executables_;
+  // std::priority_queue<const PriorityExecutable *, std::vector<const
-    std::set<const PriorityExecutable *, PriorityExecutableComparator> all_executables_;
+  // PriorityExecutable *>, PriorityExecutableComparator> all_executables_;
+  std::set<const PriorityExecutable *, PriorityExecutableComparator>
+      all_executables_;
 
-    // priority queue doesn't allow iteration. fortunately, std::map is sorted by key, so we can replace the priority queue with a map
+  // priority queue doesn't allow iteration. fortunately, std::map is sorted by
-    // the key will be the priority. the value doesn't matter.
+  // key, so we can replace the priority queue with a map the key will be the
-    // std::map<const PriorityExecutable *, int, PriorityExecutableComparator> all_executables_ = std::map<const PriorityExecutable *, int, PriorityExecutableComparator>(PriorityExecutableComparator());
+  // priority. the value doesn't matter. std::map<const PriorityExecutable *,
+  // int, PriorityExecutableComparator> all_executables_ = std::map<const
+  // PriorityExecutable *, int,
+  // PriorityExecutableComparator>(PriorityExecutableComparator());
   void add_executable_to_queue(std::shared_ptr<PriorityExecutable> p);
 };
 

src/priority_executor/src/priority_executor.cpp

@@ -59,7 +59,7 @@ namespace timed_executor
         }
       }
     }
-    std::cout << "shutdown" << std::endl;
+    RCLCPP_INFO(rclcpp::get_logger("priority_executor"), "priority executor shutdown");
   }
 
   bool TimedExecutor::get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout)

src/priority_executor/src/usage_example.cpp

@@ -1,10 +1,10 @@
-#include "rclcpp/rclcpp.hpp"
 #include "priority_executor/priority_executor.hpp"
 #include "priority_executor/priority_memory_strategy.hpp"
-#include <string>
+#include "rclcpp/rclcpp.hpp"
-#include <fstream>
-#include <unistd.h>
 #include "std_msgs/msg/string.hpp"
+#include <fstream>
+#include <string>
+#include <unistd.h>
 
 // re-create the classic talker-listener example with two listeners
 class Talker : public rclcpp::Node
@@ -15,7 +15,8 @@ public:
     // Create a publisher on the "chatter" topic with 10 msg queue size.
     pub_ = this->create_publisher<std_msgs::msg::String>("chatter", 10);
     // Create a timer of period 1s that calls our callback member function.
-        timer_ = this->create_wall_timer(std::chrono::seconds(1), std::bind(&Talker::timer_callback, this));
+    timer_ = this->create_wall_timer(std::chrono::seconds(1),
+                                     std::bind(&Talker::timer_callback, this));
   }
   // the timer must be public
   rclcpp::TimerBase::SharedPtr timer_;
@@ -23,15 +24,11 @@ public:
 private:
   void timer_callback()
   {
-        // Create a message and publish it 10 times.
     std_msgs::msg::String msg;
     msg.data = "Hello World!";
-        for (int i = 0; i < 10; ++i)
-        {
     RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg.data.c_str());
     pub_->publish(msg);
   }
-    }
   rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
 };
 
@@ -40,8 +37,11 @@ class Listener : public rclcpp::Node
 public:
   Listener(std::string name) : Node(name)
   {
-        // Create a subscription on the "chatter" topic with the default callback method.
+    // Create a subscription on the "chatter" topic with the default callback
-        sub_ = this->create_subscription<std_msgs::msg::String>("chatter", 10, std::bind(&Listener::callback, this, std::placeholders::_1));
+    // method.
+    sub_ = this->create_subscription<std_msgs::msg::String>(
+        "chatter", 10,
+        std::bind(&Listener::callback, this, std::placeholders::_1));
   }
   // the publisher must be public
   rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
@@ -66,14 +66,33 @@ int main(int argc, char **argv)
   auto executor = new timed_executor::TimedExecutor(options);
   // replace the above line with the following line to use the default executor
   // which will intermix the execution of listener1 and listener2
-    // auto executor = std::make_shared<rclcpp::executors::SingleThreadedExecutor>(options);
+  // auto executor =
+  // std::make_shared<rclcpp::executors::SingleThreadedExecutor>(options);
 
-    strategy->set_executable_deadline(talker->timer_->get_timer_handle(), 100, TIMER);
+  strategy->set_executable_deadline(talker->timer_->get_timer_handle(), 1000,
-    strategy->set_executable_deadline(listener1->sub_->get_subscription_handle(), 100, SUBSCRIPTION);
+                                    TIMER);
-    strategy->set_executable_deadline(listener2->sub_->get_subscription_handle(), 50, SUBSCRIPTION);
+  strategy->get_priority_settings(talker->timer_->get_timer_handle())
+      ->timer_handle = talker->timer_;
+  strategy->set_first_in_chain(talker->timer_->get_timer_handle());
+  strategy->get_priority_settings(talker->timer_->get_timer_handle());
+  strategy->set_executable_deadline(listener1->sub_->get_subscription_handle(),
+                                    1000, SUBSCRIPTION);
+  strategy->set_executable_deadline(listener2->sub_->get_subscription_handle(),
+                                    1000, SUBSCRIPTION);
+  strategy->set_last_in_chain(listener2->sub_->get_subscription_handle());
   executor->add_node(talker);
   executor->add_node(listener1);
   executor->add_node(listener2);
 
+  std::cout << *strategy->get_priority_settings(
+                   talker->timer_->get_timer_handle())
+            << std::endl;
+  std::cout << *strategy->get_priority_settings(
+                   listener1->sub_->get_subscription_handle())
+            << std::endl;
+  std::cout << *strategy->get_priority_settings(
+                   listener2->sub_->get_subscription_handle())
+            << std::endl;
+
   executor->spin();
 }

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;
+}