initial commit

experiments/picas_priority_long.yaml

@@ -0,0 +1,5 @@
+experiment_parameters:
+  ros__parameters:
+    experiment_name: "picas_priority_long"
+    schedule_type: "chain_priority"
+    count_max: 500

experiments/picas_priority_short.yaml

@@ -0,0 +1,5 @@
+experiment_parameters:
+  ros__parameters:
+    experiment_name: "picas_priority_short"
+    schedule_type: "chain_priority"
+    count_max: 20

experiments/ros_default_long.yaml

@@ -0,0 +1,5 @@
+experiment_parameters:
+  ros__parameters:
+    experiment_name: "ros_default_long"
+    schedule_type: "default"
+    count_max: 500

experiments/ros_default_short.yaml

@@ -0,0 +1,5 @@
+experiment_parameters:
+  ros__parameters:
+    experiment_name: "ros_default_short"
+    schedule_type: "default"
+    count_max: 20

experiments/rtis_deadline_long.yaml

@@ -0,0 +1,5 @@
+experiment_parameters:
+  ros__parameters:
+    experiment_name: "rtis_deadline_long"
+    schedule_type: "deadline"
+    count_max: 500

experiments/rtis_deadline_short.yaml

@@ -0,0 +1,7 @@
+experiment_parameters:
+  ros__parameters:
+    experiment_name: "rtis_deadline_short"
+    schedule_type: "deadline"
+    count_max: 20
+    chain_deadlines: [1000, 1000]
+    chain_periods: [1000, 1000]

src/priority_executor/CMakeLists.txt

@@ -0,0 +1,126 @@
+cmake_minimum_required(VERSION 3.5)
+project(priority_executor)
+
+# 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)
+find_package(rclcpp REQUIRED)
+find_package(rcl REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(std_srvs REQUIRED)
+find_package(simple_timer REQUIRED)
+
+add_library(priority_executor src/priority_executor.cpp)
+target_include_directories(priority_executor PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(priority_executor
+  rmw
+  rclcpp
+  rcl
+  simple_timer 
+)
+
+add_library(test_nodes src/test_nodes.cpp)
+target_include_directories(test_nodes PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(test_nodes
+  rmw
+  rclcpp
+  rcl
+  simple_timer
+  std_msgs
+)
+target_link_libraries(test_nodes
+  primes_workload
+)
+
+add_library(primes_workload src/primes_workload.cpp)
+target_include_directories(primes_workload PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+ament_target_dependencies(primes_workload
+  simple_timer
+)
+
+
+add_executable(test_publisher src/test_publisher.cpp)
+target_include_directories(test_publisher PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_link_libraries(test_publisher
+  priority_executor
+  test_nodes
+  default_executor
+)
+ament_target_dependencies(
+  test_publisher
+  rclcpp
+  std_msgs
+  std_srvs
+  simple_timer 
+)
+
+install(TARGETS test_publisher priority_executor
+  DESTINATION lib/${PROJECT_NAME})
+
+add_library(default_executor src/default_executor.cpp)
+target_include_directories(default_executor PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(default_executor
+  rclcpp
+  simple_timer
+)
+target_link_libraries(default_executor
+  primes_workload
+)
+
+add_executable(f1tenth_publisher src/f1tenth_test.cpp)
+target_include_directories(test_publisher PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_link_libraries(f1tenth_publisher
+  priority_executor
+  test_nodes
+  default_executor
+)
+ament_target_dependencies(f1tenth_publisher
+  rclcpp
+  std_msgs
+  std_srvs
+  simple_timer 
+)
+
+install(TARGETS f1tenth_publisher priority_executor
+  DESTINATION lib/${PROJECT_NAME})
+
+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()
+
+ament_package()

src/priority_executor/include/priority_executor/default_executor.hpp

@@ -0,0 +1,71 @@
+// Copyright 2014 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 RTIS_DEFAULT_EXECUTOR
+#define RTIS_DEFAULT_EXECUTOR
+
+#include <rmw/rmw.h>
+
+#include <cassert>
+#include <cstdlib>
+#include <memory>
+#include <vector>
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/visibility_control.hpp"
+#include "priority_executor/priority_memory_strategy.hpp"
+
+/// Single-threaded executor implementation.
+/**
+ * This is the default executor created by rclcpp::spin.
+ */
+class ROSDefaultExecutor : public rclcpp::Executor
+{
+public:
+  RCLCPP_SMART_PTR_DEFINITIONS(ROSDefaultExecutor)
+  std::unordered_map<std::shared_ptr<const void>, PriorityExecutable> priority_map;
+  node_time_logger logger;
+
+  /// Default constructor. See the default constructor for Executor.
+  RCLCPP_PUBLIC
+  explicit ROSDefaultExecutor(
+      const rclcpp::ExecutorOptions &options = rclcpp::ExecutorOptions());
+
+  /// Default destructor.
+  RCLCPP_PUBLIC
+  virtual ~ROSDefaultExecutor();
+
+  /// Single-threaded implementation of spin.
+  /**
+   * This function will block until work comes in, execute it, and then repeat
+   * the process until canceled.
+   * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c
+   * if the associated context is configured to shutdown on SIGINT.
+   * \throws std::runtime_error when spin() called while already spinning
+   */
+  RCLCPP_PUBLIC
+  void
+  spin() override;
+  bool get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+
+private:
+  RCLCPP_DISABLE_COPY(ROSDefaultExecutor)
+};
+
+#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_

src/priority_executor/include/priority_executor/primes_workload.hpp

@@ -0,0 +1,14 @@
+#ifndef RTIS_PRIMES_WORKLOAD
+#define RTIS_PRIMES_WORKLOAD
+#include <time.h>
+#include <sys/time.h>
+#include <thread>
+#include <cstdio>
+typedef double ktimeunit;
+ktimeunit nth_prime_silly(int n, double millis = 100);
+ktimeunit get_thread_time(struct timespec *currTime);
+// int main()
+// {
+//   printf("%lf\n", nth_prime_silly(100000, 750));
+// }
+#endif

src/priority_executor/include/priority_executor/priority_executor.hpp

@@ -0,0 +1,130 @@
+// Copyright 2014 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 RTIS_TIMED_EXECUTOR
+#define RTIS_TIMED_EXECUTOR
+
+#include <rmw/rmw.h>
+
+#include <cassert>
+#include <cstdlib>
+#include <memory>
+#include <vector>
+#include <time.h>
+
+#include "rclcpp/executor.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp/memory_strategies.hpp"
+#include "rclcpp/node.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/rate.hpp"
+#include "rclcpp/visibility_control.hpp"
+namespace timed_executor
+{
+
+  /// Single-threaded executor implementation.
+  /**
+ * This is the default executor created by rclcpp::spin.
+ */
+  class TimedExecutor : public rclcpp::Executor
+  {
+  public:
+    RCLCPP_SMART_PTR_DEFINITIONS(TimedExecutor)
+
+    /// Default constructor. See the default constructor for Executor.
+    RCLCPP_PUBLIC
+    explicit TimedExecutor(
+        const rclcpp::ExecutorOptions &options = rclcpp::ExecutorOptions(), std::string name = "unnamed executor");
+
+    /// Default destructor.
+    RCLCPP_PUBLIC
+    virtual ~TimedExecutor();
+
+    /// Single-threaded implementation of spin.
+    /**
+   * This function will block until work comes in, execute it, and then repeat
+   * the process until canceled.
+   * It may be interrupt by a call to rclcpp::Executor::cancel() or by ctrl-c
+   * if the associated context is configured to shutdown on SIGINT.
+   * \throws std::runtime_error when spin() called while already spinning
+   */
+    RCLCPP_PUBLIC
+    void
+    spin() override;
+    unsigned long long get_max_runtime(void);
+    std::string name;
+
+    void set_use_priorities(bool use_prio);
+
+  private:
+    RCLCPP_DISABLE_COPY(TimedExecutor)
+    // TODO: remove these
+    unsigned long long maxRuntime = 0;
+    unsigned long long start_time = 0;
+    int recording = 0;
+    void execute_subscription(rclcpp::AnyExecutable subscription);
+    bool
+    get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout = std::chrono::nanoseconds(-1));
+    void
+    wait_for_work(std::chrono::nanoseconds timeout);
+
+    bool
+    get_next_ready_executable(rclcpp::AnyExecutable &any_executable);
+
+    bool use_priorities = true;
+  };
+
+} // namespace timed_executor
+
+static void
+take_and_do_error_handling(
+    const char *action_description,
+    const char *topic_or_service_name,
+    std::function<bool()> take_action,
+    std::function<void()> handle_action)
+{
+  bool taken = false;
+  try
+  {
+    taken = take_action();
+  }
+  catch (const rclcpp::exceptions::RCLError &rcl_error)
+  {
+    RCLCPP_ERROR(
+        rclcpp::get_logger("rclcpp"),
+        "executor %s '%s' unexpectedly failed: %s",
+        action_description,
+        topic_or_service_name,
+        rcl_error.what());
+  }
+  if (taken)
+  {
+    handle_action();
+  }
+  else
+  {
+    // Message or Service was not taken for some reason.
+    // Note that this can be normal, if the underlying middleware needs to
+    // interrupt wait spuriously it is allowed.
+    // So in that case the executor cannot tell the difference in a
+    // spurious wake up and an entity actually having data until trying
+    // to take the data.
+    RCLCPP_DEBUG(
+        rclcpp::get_logger("rclcpp"),
+        "executor %s '%s' failed to take anything",
+        action_description,
+        topic_or_service_name);
+  }
+}
+#endif // RCLCPP__EXECUTORS__SINGLE_THREADED_EXECUTOR_HPP_

src/priority_executor/include/priority_executor/test_nodes.hpp

@@ -0,0 +1,39 @@
+#include "rclcpp/rclcpp.hpp"
+#include "simple_timer/rt-sched.hpp"
+#include "std_msgs/msg/string.hpp"
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+
+class PublisherNode : public rclcpp::Node
+{
+public:
+  PublisherNode(std::string publish_topic, int chain, int period, double runtime);
+
+  rclcpp::TimerBase::SharedPtr timer_;
+  uint count_max = 20;
+  node_time_logger logger_;
+
+private:
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  uint count_;
+  int chain;
+  double runtime;
+  int period;
+};
+class DummyWorker : public rclcpp::Node
+{
+public:
+  DummyWorker(const std::string &name, double runtime, int chain, int number, bool is_multichain = false);
+  node_time_logger logger_;
+
+private:
+  double runtime;
+  int number;
+  int chain;
+  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_;
+  void topic_callback(const std_msgs::msg::String::SharedPtr msg) const;
+
+public:
+  rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_;
+};

src/priority_executor/package.xml

@@ -0,0 +1,24 @@
+<?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>priority_executor</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="kurt4wilson@gmail.com">kurt</maintainer>
+  <license>TODO: License declaration</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rcl</depend>
+  <depend>std_msgs</depend>
+  <depend>std_srvs</depend>
+  <depend>simple_timer</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/priority_executor/src/default_executor.cpp

@@ -0,0 +1,66 @@
+// Copyright 2015 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 "rcpputils/scope_exit.hpp"
+
+#include "rclcpp/any_executable.hpp"
+#include "priority_executor/priority_memory_strategy.hpp"
+#include "priority_executor/default_executor.hpp"
+#include "priority_executor/primes_workload.hpp"
+
+ROSDefaultExecutor::ROSDefaultExecutor(const rclcpp::ExecutorOptions &options)
+    : rclcpp::Executor(options) {}
+
+ROSDefaultExecutor::~ROSDefaultExecutor() {}
+
+bool ROSDefaultExecutor::get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout)
+{
+  bool success = false;
+  // Check to see if there are any subscriptions or timers needing service
+  // TODO(wjwwood): improve run to run efficiency of this function
+  // sched_yield();
+  wait_for_work(std::chrono::milliseconds(1));
+  success = get_next_ready_executable(any_executable);
+  return success;
+}
+
+void ROSDefaultExecutor::spin()
+{
+  if (spinning.exchange(true))
+  {
+    throw std::runtime_error("spin() called while already spinning");
+  }
+  RCPPUTILS_SCOPE_EXIT(this->spinning.store(false););
+  while (rclcpp::ok(this->context_) && spinning.load())
+  {
+    rclcpp::AnyExecutable any_executable;
+    if (get_next_executable(any_executable))
+    {
+      if (any_executable.timer)
+      {
+        if (priority_map.find(any_executable.timer->get_timer_handle()) != priority_map.end())
+        {
+          timespec current_time;
+          clock_gettime(CLOCK_MONOTONIC_RAW, &current_time);
+          uint64_t millis = (current_time.tv_sec * (uint64_t)1000) + (current_time.tv_nsec / 1000000);
+          PriorityExecutable next_exec = priority_map[any_executable.timer->get_timer_handle()];
+
+          auto timer = next_exec.timer_handle;
+          log_entry(logger, "timer_" + std::to_string(next_exec.chain_id) + "_release_" + std::to_string(millis + (timer->time_until_trigger().count() / 1000000)));
+        }
+      }
+      execute_any_executable(any_executable);
+    }
+  }
+}

src/priority_executor/src/f1tenth_test.cpp

@@ -0,0 +1,330 @@
+#include "rclcpp/rclcpp.hpp"
+#include "simple_timer/rt-sched.hpp"
+#include "priority_executor/priority_memory_strategy.hpp"
+#include "priority_executor/priority_executor.hpp"
+#include "priority_executor/test_nodes.hpp"
+#include "priority_executor/default_executor.hpp"
+#include <vector>
+#include <fstream>
+#include <unistd.h>
+
+typedef struct
+{
+  std::shared_ptr<timed_executor::TimedExecutor> executor;
+  std::shared_ptr<PriorityMemoryStrategy<>> strat;
+
+  std::shared_ptr<ROSDefaultExecutor> default_executor;
+} executor_strat;
+
+void spin_exec(executor_strat strat, int id, int index)
+{
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  CPU_SET(id, &cpuset);
+
+  pthread_t current_thread = pthread_self();
+  int result;
+  if (result = pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &cpuset))
+  {
+    std::cout << "problem setting cpu core" << std::endl;
+    std::cout << strerror(result) << std::endl;
+  }
+  sched_param sch_params;
+
+  // experiment: RT threads have priority 99, all others 98
+  if (index < 4)
+  {
+    sch_params.sched_priority = 99;
+  }
+  else
+  {
+    sch_params.sched_priority = 98;
+  }
+  // sch_params.sched_priority = 99 - index;
+  if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch_params))
+  {
+    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "spin_rt thread has an error.");
+  }
+  if (strat.executor != nullptr)
+  {
+    strat.executor->spin();
+  }
+  else if (strat.default_executor != nullptr)
+  {
+    strat.default_executor->spin();
+  }
+  else
+  {
+    std::cout << "spin_exec got a executor_strat with null values!" << std::endl;
+  }
+}
+
+int main(int argc, char **argv)
+{
+  // read parameters
+  rclcpp::init(argc, argv);
+  std::cout << "starting..." << std::endl;
+  auto node = rclcpp::Node::make_shared("experiment_parameters");
+  node->declare_parameter("experiment_name");
+  node->declare_parameter("count_max");
+  node->declare_parameter("schedule_type");
+
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+  // parameters_client->wait_for_service();
+  const std::string schedule_type_str = parameters_client->get_parameter("schedule_type", std::string("deadline"));
+  std::cout << schedule_type_str << std::endl;
+  int COUNT_MAX = parameters_client->get_parameter("count_max", 500);
+  ExecutableScheduleType schedule_type = DEFAULT;
+  if (schedule_type_str == "deadline")
+  {
+    schedule_type = DEADLINE;
+  }
+  else if (schedule_type_str == "chain_priority")
+  {
+    schedule_type = CHAIN_AWARE_PRIORITY;
+  }
+  else
+  {
+    schedule_type = DEFAULT;
+  }
+
+  // create executors
+  std::vector<executor_strat> executors;
+  const int NUM_EXECUTORS = 8;
+  std::cout << "creating executors" << std::endl;
+  for (int i = 0; i < NUM_EXECUTORS; i++)
+  {
+    executor_strat executor;
+    if (schedule_type == DEFAULT)
+    {
+      executor.default_executor = std::make_shared<ROSDefaultExecutor>();
+      executor.default_executor->logger = create_logger();
+    }
+    else
+    {
+
+      executor.strat = std::make_shared<PriorityMemoryStrategy<>>();
+      rclcpp::ExecutorOptions options;
+      options.memory_strategy = executor.strat;
+      executor.strat->logger = create_logger();
+      executor.strat->is_f1tenth = true;
+
+      executor.executor = std::make_shared<timed_executor::TimedExecutor>(options);
+      executor.executor->set_use_priorities(true);
+    }
+    executors.push_back(executor);
+  }
+  std::cout << "executors created" << std::endl;
+
+  std::vector<uint64_t> chain_lengths = {2, 4, 4, 3, 4, 2, 2, 2, 2, 2, 2, 2};
+  std::vector<std::vector<uint64_t>> chain_member_ids = {{1, 2}, {1, 3, 4, 5}, {6, 7, 8, 9}, {10, 11, 12}, {13, 14, 15, 16}, {17, 18}, {19, 20}, {21, 22}, {23, 24}, {25, 26}, {27, 28}, {29, 30}};
+  std::vector<std::vector<uint64_t>> chain_priorities = {{1, 0}, {5, 4, 3, 2, 1}, {9, 8, 7, 6}, {12, 11, 10}, {16, 15, 14, 13}, {18, 17}, {20, 19}, {22, 21}, {24, 23}, {26, 25}, {28, 27}, {30, 29}};
+  // assignments for ROS and EDF
+  std::vector<std::vector<uint64_t>> node_executor_assignment = {{0, 0}, {0, 1, 1, 1}, {2, 2, 2, 2}, {3, 3, 3}, {0, 0, 0, 0}, {1, 1}, {4, 4}, {5, 5}, {6, 6}, {7, 7}, {4, 4}, {5, 5}};
+  std::vector<uint64_t> executor_cpu_assignment = {0, 1, 2, 3, 0, 1, 2, 3};
+  std::vector<double_t> node_runtimes = {2.3, 16.1, 2.3, 2.2, 18.4, 9.1, 23.1, 7.9, 14.2, 17.9, 20.6, 17.9, 6.6, 1.7, 11.0, 6.6, 7.9, 1.7, 195.9, 33.2, 2.2, 33.2, 2.2, 33.2, 2.2, 33.2, 2.2, 33.2, 2.2, 33.2, 2.2};
+  std::cout << std::to_string(node_runtimes.size()) << std::endl;
+  std::vector<uint64_t> chain_periods = {80, 80, 100, 100, 160, 1000, 120, 120, 120, 120, 120, 120};
+  std::vector<uint64_t> chain_deadlines = {80, 80, 100, 100, 160, 1000, 120, 120, 120, 120, 120, 120};
+
+  std::vector<std::vector<std::shared_ptr<rclcpp::Node>>> nodes;
+  std::vector<std::shared_ptr<PublisherNode>> publishers;
+  std::vector<std::shared_ptr<DummyWorker>> workers;
+  std::vector<std::deque<uint> *> chain_deadlines_deque;
+  // create nodes and assign to executors
+  uint64_t current_node_id = 0;
+  for (uint chain_index = 0; chain_index < chain_lengths.size(); chain_index++)
+  {
+    std::cout << "making chain " << std::to_string(chain_index) << std::endl;
+    std::shared_ptr<rclcpp::TimerBase> this_chain_timer_handle;
+    std::deque<uint> *this_chain_deadlines_deque = new std::deque<uint>();
+    nodes.push_back(std::vector<std::shared_ptr<rclcpp::Node>>());
+    for (uint cb_index = 0; cb_index < chain_lengths[chain_index]; cb_index++)
+    {
+      std::cout << "making node " << std::to_string(current_node_id) << " with runtime " << node_runtimes[current_node_id] << std::endl;
+
+      executor_strat this_executor = executors[node_executor_assignment[chain_index][cb_index] % NUM_EXECUTORS];
+      if (cb_index == 0)
+      {
+        // this should be a timer
+        std::shared_ptr<PublisherNode> publisher_node;
+        if (chain_index == 1)
+        {
+          // special case, re-use timer from index 0
+          publisher_node = std::static_pointer_cast<PublisherNode>(nodes[0][0]);
+          this_chain_timer_handle = publisher_node->timer_;
+          // current_node_id--;
+        }
+        else
+        {
+          publisher_node = std::make_shared<PublisherNode>("topic_" + std::to_string(chain_index), chain_index, chain_periods[chain_index], node_runtimes[current_node_id]);
+          publishers.push_back(publisher_node);
+          publisher_node->count_max = COUNT_MAX;
+          if (schedule_type == DEADLINE)
+          {
+            assert(this_executor.strat != nullptr);
+            auto timer_handle = publisher_node->timer_->get_timer_handle();
+            assert(timer_handle != nullptr);
+            this_executor.strat->set_executable_deadline(publisher_node->timer_->get_timer_handle(), chain_deadlines[chain_index], TIMER, chain_index);
+          }
+          else if (schedule_type == CHAIN_AWARE_PRIORITY)
+          {
+            this_executor.strat->set_executable_priority(publisher_node->timer_->get_timer_handle(), chain_priorities[chain_index][cb_index], TIMER, CHAIN_AWARE_PRIORITY, chain_index);
+          }
+
+          if (schedule_type == DEFAULT)
+          {
+            this_executor.default_executor->add_node(publisher_node);
+            PriorityExecutable e;
+            e.chain_id = chain_index;
+            e.timer_handle = publisher_node->timer_;
+            this_executor.default_executor->priority_map[publisher_node->timer_->get_timer_handle()] = e;
+          }
+          else
+          {
+            this_executor.strat->set_first_in_chain(publisher_node->timer_->get_timer_handle());
+            this_executor.strat->assign_deadlines_queue(publisher_node->timer_->get_timer_handle(), this_chain_deadlines_deque);
+            this_chain_timer_handle = publisher_node->timer_;
+            this_executor.strat->get_priority_settings(publisher_node->timer_->get_timer_handle())->timer_handle = this_chain_timer_handle;
+            this_executor.executor->add_node(publisher_node);
+          }
+        }
+        nodes[chain_index].push_back(std::static_pointer_cast<rclcpp::Node>(publisher_node));
+      }
+      else
+      {
+        // this is a worker node
+        std::shared_ptr<DummyWorker> sub_node;
+
+        if (chain_index == 1 && cb_index == 1)
+        {
+          sub_node = std::make_shared<DummyWorker>("chain_" + std::to_string(chain_index) + "_worker_" + std::to_string(cb_index), node_runtimes[current_node_id], chain_index, cb_index, true);
+        }
+        else
+        {
+          sub_node = std::make_shared<DummyWorker>("chain_" + std::to_string(chain_index) + "_worker_" + std::to_string(cb_index), node_runtimes[current_node_id], chain_index, cb_index);
+        }
+        workers.push_back(sub_node);
+        if (schedule_type == DEADLINE)
+        {
+          this_executor.strat->set_executable_deadline(sub_node->subscription_->get_subscription_handle(), chain_deadlines[chain_index], SUBSCRIPTION, chain_index);
+        }
+        else if (schedule_type == CHAIN_AWARE_PRIORITY)
+        {
+          this_executor.strat->set_executable_priority(sub_node->subscription_->get_subscription_handle(), chain_priorities[chain_index][cb_index], SUBSCRIPTION, CHAIN_AWARE_PRIORITY, chain_index);
+        }
+
+        if (schedule_type == DEFAULT)
+        {
+          this_executor.default_executor->add_node(sub_node);
+        }
+        else
+        {
+          this_executor.executor->add_node(sub_node);
+          this_executor.strat->assign_deadlines_queue(sub_node->subscription_->get_subscription_handle(), this_chain_deadlines_deque);
+          if (cb_index == chain_lengths[chain_index] - 1)
+          {
+            this_executor.strat->set_last_in_chain(sub_node->subscription_->get_subscription_handle());
+            this_executor.strat->get_priority_settings(sub_node->subscription_->get_subscription_handle())->timer_handle = this_chain_timer_handle;
+          }
+        }
+        nodes[chain_index].push_back(std::static_pointer_cast<rclcpp::Node>(sub_node));
+      }
+      current_node_id++;
+    }
+    chain_deadlines_deque.push_back(this_chain_deadlines_deque);
+  }
+  std::cout << "initialized nodes" << std::endl;
+  node_time_logger logger = create_logger();
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC_RAW, &current_time);
+  uint64_t millis = (current_time.tv_sec * (uint64_t)1000) + (current_time.tv_nsec / 1000000);
+  for (uint chain_index = 0; chain_index < chain_lengths.size(); chain_index++)
+  {
+    if (schedule_type == DEADLINE)
+    {
+
+      log_entry(logger, "deadline_" + std::to_string(chain_index) + "_" + std::to_string(millis + chain_deadlines[chain_index]));
+    }
+    log_entry(logger, "timer_" + std::to_string(chain_index) + "_release_" + std::to_string(millis));
+    chain_deadlines_deque[chain_index]->push_back(millis + chain_deadlines[chain_index]);
+    // chain_deadlines_deque[chain_index]->push_back(0);
+  }
+
+  std::vector<std::thread> threads;
+  // start each executor on it's own thread
+  for (int i = 0; i < NUM_EXECUTORS; i++)
+  {
+    executor_strat strat = executors[i];
+    auto func = std::bind(spin_exec, strat, executor_cpu_assignment[i], i);
+    threads.emplace_back(func);
+  }
+  for (auto &thread : threads)
+  {
+    thread.join();
+  }
+  rclcpp::shutdown();
+
+  std::ofstream output_file;
+  std::string suffix = "_rtis_alloc";
+  if (schedule_type == DEADLINE)
+  {
+    output_file.open("experiments/results/f1tenth_full" + std::to_string(NUM_EXECUTORS) + "c" + suffix + ".txt");
+  }
+  else if (schedule_type == CHAIN_AWARE_PRIORITY)
+  {
+    output_file.open("experiments/results/f1tenth_full_chain" + std::to_string(NUM_EXECUTORS) + "c" + suffix + ".txt");
+  }
+  else
+  {
+    output_file.open("experiments/results/f1tenth_default" + std::to_string(NUM_EXECUTORS) + "c" + suffix + ".txt");
+  }
+
+  std::vector<std::pair<std::string, u64>> combined_logs;
+  for (auto &publisher : publishers)
+  {
+    for (auto &log : *(publisher->logger_.recorded_times))
+    {
+      combined_logs.push_back(log);
+    }
+  }
+
+  for (auto &worker : workers)
+  {
+    for (auto &log : *(worker->logger_.recorded_times))
+    {
+      combined_logs.push_back(log);
+    }
+  }
+
+  if (schedule_type == DEFAULT)
+  {
+    for (auto &executor : executors)
+    {
+      for (auto &log : *(executor.default_executor->logger.recorded_times))
+      {
+        combined_logs.push_back(log);
+      }
+    }
+  }
+  else
+  {
+    for (auto &executor : executors)
+    {
+      for (auto &log : *(executor.strat->logger.recorded_times))
+      {
+        combined_logs.push_back(log);
+      }
+    }
+  }
+
+  std::sort(combined_logs.begin(), combined_logs.end(), [](const std::pair<std::string, u64> &a, const std::pair<std::string, u64> &b)
+            { return a.second < b.second; });
+
+  for (auto p : combined_logs)
+  {
+    output_file << p.second << " " << p.first << std::endl;
+  }
+  output_file.close();
+  std::cout<<"data written"<<std::endl;
+}

src/priority_executor/src/primes_workload.cpp

@@ -0,0 +1,35 @@
+#include "priority_executor/primes_workload.hpp"
+ktimeunit nth_prime_silly(int n, double millis)
+{
+  // struct tms this_thread_times;
+  struct timespec currTime;
+  int sum = 0;
+  int i;
+  int j;
+  ktimeunit const start_cpu_time = get_thread_time(&currTime);
+  ktimeunit last_iter_time = 0;
+  ktimeunit last_iter_start_time = start_cpu_time;
+  for (i = 2; i < 4294967296 - 1; i++)
+  {
+    // times(&this_thread_times);
+    ktimeunit cum_time = get_thread_time(&currTime);
+    last_iter_time = cum_time - last_iter_start_time;
+    last_iter_start_time = cum_time;
+    if ((cum_time - start_cpu_time + last_iter_time) > millis)
+    {
+      break;
+    }
+    for (j = 2; j < i; j++)
+    {
+      sum += j;
+    }
+  }
+  return get_thread_time(&currTime) - start_cpu_time;
+}
+ktimeunit get_thread_time(struct timespec *currTime)
+{
+  clockid_t threadClockId;
+  pthread_getcpuclockid(pthread_self(), &threadClockId);
+  clock_gettime(threadClockId, currTime);
+  return currTime->tv_nsec / 1000000.0 + currTime->tv_sec * 1000.0;
+}

src/priority_executor/src/priority_executor.cpp

@@ -0,0 +1,339 @@
+// Copyright 2015 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 "priority_executor/priority_executor.hpp"
+#include "priority_executor/priority_memory_strategy.hpp"
+#include "rclcpp/any_executable.hpp"
+#include "rclcpp/scope_exit.hpp"
+#include "simple_timer/rt-sched.hpp"
+#include "rclcpp/utilities.hpp"
+#include "rclcpp/exceptions.hpp"
+#include <memory>
+#include <sched.h>
+namespace timed_executor
+{
+
+  TimedExecutor::TimedExecutor(const rclcpp::ExecutorOptions &options, std::string name)
+      : rclcpp::Executor(options)
+  {
+    this->name = name;
+  }
+
+  TimedExecutor::~TimedExecutor() {}
+
+  void
+  TimedExecutor::spin()
+  {
+    if (spinning.exchange(true))
+    {
+      throw std::runtime_error("spin() called while already spinning");
+    }
+    RCLCPP_SCOPE_EXIT(this->spinning.store(false););
+    while (rclcpp::ok(this->context_) && spinning.load())
+    {
+      rclcpp::AnyExecutable any_executable;
+      // std::cout<<memory_strategy_->number_of_ready_timers()<<std::endl;
+      // std::cout << "spinning " << this->name << std::endl;
+      // size_t ready = memory_strategy_->number_of_ready_subscriptions();
+      // std::cout << "ready:" << ready << std::endl;
+
+      if (get_next_executable(any_executable))
+      {
+        if (any_executable.subscription)
+        {
+          execute_subscription(any_executable);
+        }
+        else
+        {
+          execute_any_executable(any_executable);
+        }
+      }
+    }
+    std::cout << "shutdown" << std::endl;
+  }
+
+  unsigned long long TimedExecutor::get_max_runtime(void)
+  {
+    return maxRuntime;
+  }
+
+  void
+  TimedExecutor::execute_subscription(rclcpp::AnyExecutable executable)
+  {
+    rclcpp::SubscriptionBase::SharedPtr subscription = executable.subscription;
+
+    rclcpp::MessageInfo message_info;
+    message_info.get_rmw_message_info().from_intra_process = false;
+
+    if (subscription->is_serialized())
+    {
+      // This is the case where a copy of the serialized message is taken from
+      // the middleware via inter-process communication.
+
+      // if this should happen on another thread,  we'd pass it to a thread here
+      std::shared_ptr<rclcpp::SerializedMessage> serialized_msg = subscription->create_serialized_message();
+      take_and_do_error_handling(
+          "taking a serialized message from topic",
+          subscription->get_topic_name(),
+          [&]()
+          {
+            auto result = subscription->take_serialized(*serialized_msg.get(), message_info);
+            // RCLCPP_INFO(rclcpp::get_logger(this->name), "at topic %s, serialized msg sent at %ld, and recieved at %ld", executable.node_base->get_name(), message_info.get_rmw_message_info().source_timestamp, message_info.get_rmw_message_info().received_timestamp);
+            return result;
+          },
+          [&]()
+          {
+            auto void_serialized_msg = std::static_pointer_cast<void>(serialized_msg);
+            subscription->handle_message(void_serialized_msg, message_info);
+          });
+      subscription->return_serialized_message(serialized_msg);
+    }
+    else if (subscription->can_loan_messages())
+    {
+      // This is the case where a loaned message is taken from the middleware via
+      // inter-process communication, given to the user for their callback,
+      // and then returned.
+      void *loaned_msg = nullptr;
+      // TODO(wjwwood): refactor this into methods on subscription when LoanedMessage
+      //   is extened to support subscriptions as well.
+      take_and_do_error_handling(
+          "taking a loaned message from topic",
+          subscription->get_topic_name(),
+          [&]()
+          {
+            rcl_ret_t ret = rcl_take_loaned_message(
+                subscription->get_subscription_handle().get(),
+                &loaned_msg,
+                &message_info.get_rmw_message_info(),
+                nullptr);
+            if (RCL_RET_SUBSCRIPTION_TAKE_FAILED == ret)
+            {
+              return false;
+            }
+            else if (RCL_RET_OK != ret)
+            {
+              rclcpp::exceptions::throw_from_rcl_error(ret);
+            }
+            // RCLCPP_INFO(rclcpp::get_logger(this->name), "at topic %s, loaned msg sent at %ld, and recieved at %ld", executable.node_base->get_name(), message_info.get_rmw_message_info().source_timestamp, message_info.get_rmw_message_info().received_timestamp);
+            return true;
+          },
+          [&]()
+          { subscription->handle_loaned_message(loaned_msg, message_info); });
+      rcl_ret_t ret = rcl_return_loaned_message_from_subscription(
+          subscription->get_subscription_handle().get(),
+          loaned_msg);
+      if (RCL_RET_OK != ret)
+      {
+        RCLCPP_ERROR(
+            rclcpp::get_logger("rclcpp"),
+            "rcl_return_loaned_message_from_subscription() failed for subscription on topic '%s': %s",
+            subscription->get_topic_name(), rcl_get_error_string().str);
+      }
+      loaned_msg = nullptr;
+    }
+    else
+    {
+      // This case is taking a copy of the message data from the middleware via
+      // inter-process communication.
+      std::shared_ptr<void> message = subscription->create_message();
+      take_and_do_error_handling(
+          "taking a message from topic",
+          subscription->get_topic_name(),
+          [&]()
+          {
+            auto result = subscription->take_type_erased(message.get(), message_info);
+            // RCLCPP_INFO(rclcpp::get_logger(this->name), "at topic %s, IPC msg sent at %ld, and recieved at %ld", executable.node_base->get_name(), message_info.get_rmw_message_info().source_timestamp, message_info.get_rmw_message_info().received_timestamp);
+            return result;
+          },
+          [&]()
+          { subscription->handle_message(message, message_info); });
+      // this just deallocates
+      subscription->return_message(message);
+    }
+  }
+  bool TimedExecutor::get_next_executable(rclcpp::AnyExecutable &any_executable, std::chrono::nanoseconds timeout)
+  {
+    bool success = false;
+    // Check to see if there are any subscriptions or timers needing service
+    // TODO(wjwwood): improve run to run efficiency of this function
+    // sched_yield();
+    wait_for_work(std::chrono::milliseconds(1));
+    success = get_next_ready_executable(any_executable);
+    return success;
+  }
+
+  // TODO: since we're calling this more often, clean it up a bit
+  void
+  TimedExecutor::wait_for_work(std::chrono::nanoseconds timeout)
+  {
+    {
+      std::unique_lock<std::mutex> lock(memory_strategy_mutex_);
+
+      // Collect the subscriptions and timers to be waited on
+      memory_strategy_->clear_handles();
+      bool has_invalid_weak_nodes = memory_strategy_->collect_entities(weak_nodes_);
+
+      // Clean up any invalid nodes, if they were detected
+      if (has_invalid_weak_nodes)
+      {
+        auto node_it = weak_nodes_.begin();
+        auto gc_it = guard_conditions_.begin();
+        while (node_it != weak_nodes_.end())
+        {
+          if (node_it->expired())
+          {
+            node_it = weak_nodes_.erase(node_it);
+            memory_strategy_->remove_guard_condition(*gc_it);
+            gc_it = guard_conditions_.erase(gc_it);
+          }
+          else
+          {
+            ++node_it;
+            ++gc_it;
+          }
+        }
+      }
+      // clear wait set
+      rcl_ret_t ret = rcl_wait_set_clear(&wait_set_);
+      if (ret != RCL_RET_OK)
+      {
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't clear wait set");
+      }
+
+      // The size of waitables are accounted for in size of the other entities
+      ret = rcl_wait_set_resize(
+          &wait_set_, memory_strategy_->number_of_ready_subscriptions(),
+          memory_strategy_->number_of_guard_conditions(), memory_strategy_->number_of_ready_timers(),
+          memory_strategy_->number_of_ready_clients(), memory_strategy_->number_of_ready_services(),
+          memory_strategy_->number_of_ready_events());
+      if (RCL_RET_OK != ret)
+      {
+        rclcpp::exceptions::throw_from_rcl_error(ret, "Couldn't resize the wait set");
+      }
+
+      if (!memory_strategy_->add_handles_to_wait_set(&wait_set_))
+      {
+        throw std::runtime_error("Couldn't fill wait set");
+      }
+    }
+    rcl_ret_t status =
+        rcl_wait(&wait_set_, std::chrono::duration_cast<std::chrono::nanoseconds>(timeout).count());
+    if (status == RCL_RET_WAIT_SET_EMPTY)
+    {
+      RCUTILS_LOG_WARN_NAMED(
+          "rclcpp",
+          "empty wait set received in rcl_wait(). This should never happen.");
+    }
+    else if (status != RCL_RET_OK && status != RCL_RET_TIMEOUT)
+    {
+      using rclcpp::exceptions::throw_from_rcl_error;
+      throw_from_rcl_error(status, "rcl_wait() failed");
+    }
+
+    // check the null handles in the wait set and remove them from the handles in memory strategy
+    // for callback-based entities
+    memory_strategy_->remove_null_handles(&wait_set_);
+  }
+  bool
+  TimedExecutor::get_next_ready_executable(rclcpp::AnyExecutable &any_executable)
+  {
+    bool success = false;
+    if (use_priorities)
+    {
+      std::shared_ptr<PriorityMemoryStrategy<>> strat = std::dynamic_pointer_cast<PriorityMemoryStrategy<>>(memory_strategy_);
+      strat->get_next_executable(any_executable, weak_nodes_);
+      if (any_executable.timer || any_executable.subscription || any_executable.service || any_executable.client || any_executable.waitable)
+      {
+        success = true;
+      }
+    }
+    else
+    {
+      // Check the timers to see if there are any that are ready
+      memory_strategy_->get_next_timer(any_executable, weak_nodes_);
+      if (any_executable.timer)
+      {
+        std::cout << "got timer" << std::endl;
+        success = true;
+      }
+      if (!success)
+      {
+        // Check the subscriptions to see if there are any that are ready
+        memory_strategy_->get_next_subscription(any_executable, weak_nodes_);
+        if (any_executable.subscription)
+        {
+          // std::cout << "got subs" << std::endl;
+          success = true;
+        }
+      }
+      if (!success)
+      {
+        // Check the services to see if there are any that are ready
+        memory_strategy_->get_next_service(any_executable, weak_nodes_);
+        if (any_executable.service)
+        {
+          std::cout << "got serv" << std::endl;
+          success = true;
+        }
+      }
+      if (!success)
+      {
+        // Check the clients to see if there are any that are ready
+        memory_strategy_->get_next_client(any_executable, weak_nodes_);
+        if (any_executable.client)
+        {
+          std::cout << "got client" << std::endl;
+          success = true;
+        }
+      }
+      if (!success)
+      {
+        // Check the waitables to see if there are any that are ready
+        memory_strategy_->get_next_waitable(any_executable, weak_nodes_);
+        if (any_executable.waitable)
+        {
+          std::cout << "got wait" << std::endl;
+          success = true;
+        }
+      }
+    }
+    // At this point any_exec should be valid with either a valid subscription
+    // or a valid timer, or it should be a null shared_ptr
+    if (success)
+    {
+      // If it is valid, check to see if the group is mutually exclusive or
+      // not, then mark it accordingly
+      using rclcpp::callback_group::CallbackGroupType;
+      if (
+          any_executable.callback_group &&
+          any_executable.callback_group->type() == rclcpp::CallbackGroupType::MutuallyExclusive)
+      {
+        // It should not have been taken otherwise
+        assert(any_executable.callback_group->can_be_taken_from().load());
+        // Set to false to indicate something is being run from this group
+        // This is reset to true either when the any_exec is executed or when the
+        // any_exec is destructued
+        any_executable.callback_group->can_be_taken_from().store(false);
+      }
+    }
+    // If there is no ready executable, return false
+    return success;
+  }
+
+  void TimedExecutor::set_use_priorities(bool use_prio)
+  {
+    use_priorities = use_prio;
+  }
+
+} // namespace timed_executor

src/priority_executor/src/test_nodes.cpp

@@ -0,0 +1,81 @@
+#include "priority_executor/test_nodes.hpp"
+#include "priority_executor/primes_workload.hpp"
+#include "simple_timer/rt-sched.hpp"
+#include <string>
+#include <vector>
+
+using namespace std::chrono_literals;
+using std::placeholders::_1;
+PublisherNode::PublisherNode(std::string publish_topic, int chain, int period, double runtime)
+    : Node("PublisherNode_" + publish_topic), count_(0)
+{
+  logger_ = create_logger();
+  publisher_ = this->create_publisher<std_msgs::msg::String>(publish_topic, 1);
+  this->chain = chain;
+  this->runtime = runtime;
+  this->period = period;
+  std::cout << "creating timer "
+            << publish_topic << std::endl;
+  auto timer_callback =
+      [this]() -> void
+  {
+    if (this->count_ > this->count_max)
+    {
+      rclcpp::shutdown();
+      return;
+    }
+    this->logger_.recorded_times->push_back(std::make_pair(std::string(this->get_name()) + "_publish_" + std::to_string(this->count_), get_time_us()));
+    this->logger_.recorded_times->push_back(std::make_pair("chain_" + std::to_string(this->chain) + "_worker_0_recv_MESSAGE" + std::to_string(this->count_), get_time_us()));
+
+    double result = nth_prime_silly(100000, this->runtime);
+
+    this->logger_.recorded_times->push_back(std::make_pair("chain_" + std::to_string(this->chain) + "_worker_0_processed_MESSAGE" + std::to_string(this->count_), get_time_us()));
+    auto message = std_msgs::msg::String();
+    message.data = "MESSAGE" + std::to_string(this->count_++);
+    this->publisher_->publish(message);
+
+    // RCLCPP_INFO(this->get_logger(), "I did work on: '%s', taking %lf ms", message.data.c_str(), result);
+    // usleep(600 * 1000);
+  };
+  timer_ = this->create_wall_timer(std::chrono::milliseconds(period), timer_callback);
+}
+
+DummyWorker::DummyWorker(const std::string &name, double runtime, int chain, int number, bool is_multichain)
+    : Node(name)
+{
+  this->runtime = runtime;
+  this->number = number;
+  this->chain = chain;
+  this->logger_ = create_logger();
+  std::cout << "creating dummy worker "
+            << name << std::endl;
+  if (is_multichain)
+  {
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+        "topic_" + std::to_string(chain - 1), 1, std::bind(&DummyWorker::topic_callback, this, _1));
+  }
+  else if (number == 1)
+  {
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+        "topic_" + std::to_string(chain), 1, std::bind(&DummyWorker::topic_callback, this, _1));
+  }
+  else
+  {
+    subscription_ = this->create_subscription<std_msgs::msg::String>(
+        "chain_" + std::to_string(chain) + "_topic_" + std::to_string(number), 1, std::bind(&DummyWorker::topic_callback, this, _1));
+  }
+  this->publisher_ = this->create_publisher<std_msgs::msg::String>("chain_" + std::to_string(chain) + "_topic_" + std::to_string(number + 1), 1);
+}
+void DummyWorker::topic_callback(const std_msgs::msg::String::SharedPtr msg) const
+{
+  this->logger_.recorded_times->push_back(std::make_pair(std::string(this->get_name()) + "_recv_" + msg->data, get_time_us()));
+
+  double result = nth_prime_silly(100000, runtime);
+
+  this->logger_.recorded_times->push_back(std::make_pair(std::string(this->get_name()) + "_processed_" + msg->data, get_time_us()));
+  // RCLCPP_INFO(this->get_logger(), "I did work on: '%s', taking %lf ms", msg->data.c_str(), result);
+  auto message = std_msgs::msg::String();
+  message.data = msg->data;
+  this->publisher_->publish(message);
+  // usleep(600 * 1000);
+}

src/priority_executor/src/test_publisher.cpp

@@ -0,0 +1,298 @@
+#include "rclcpp/rclcpp.hpp"
+#include "priority_executor/priority_executor.hpp"
+#include "priority_executor/priority_memory_strategy.hpp"
+#include "priority_executor/test_nodes.hpp"
+#include <string>
+#include <fstream>
+#include "simple_timer/rt-sched.hpp"
+#include "priority_executor/default_executor.hpp"
+#include <unistd.h>
+
+// clock_t times(struct tms *buf);
+
+std::vector<int64_t> get_parameter_array(std::shared_ptr<rclcpp::Node> node, std::string name, std::vector<int64_t> default_val)
+{
+  rclcpp::Parameter param_result(name, default_val);
+  node->get_parameter_or(name, param_result, param_result);
+  return param_result.as_integer_array();
+}
+
+int main(int argc, char **argv)
+{
+  cpu_set_t cpuset;
+  CPU_ZERO(&cpuset);
+  CPU_SET(0, &cpuset);
+
+  pthread_t current_thread = pthread_self();
+  if (pthread_setaffinity_np(current_thread, sizeof(cpu_set_t), &cpuset))
+  {
+    std::cout << "problem setting cpu core" << std::endl;
+  }
+  sched_param sch_params;
+  sch_params.sched_priority = 98;
+  if (pthread_setschedparam(pthread_self(), SCHED_FIFO, &sch_params))
+  {
+    RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "spin_rt thread has an error.");
+  }
+  rclcpp::init(argc, argv);
+
+  // https://design.ros2.org/articles/ros_command_line_arguments.html#multiple-parameter-assignments
+  auto node = rclcpp::Node::make_shared("experiment_parameters");
+  node->declare_parameter("experiment_name");
+  node->declare_parameter("count_max");
+  node->declare_parameter("schedule_type");
+
+  node->declare_parameter("chain_lengths");
+  node->declare_parameter("chain_periods");
+  node->declare_parameter("chain_deadlines");
+  node->declare_parameter("chain_runtimes");
+  node->declare_parameter("chain_priorities");
+  node->declare_parameter("chain_timer_runtimes");
+
+  auto parameters_client = std::make_shared<rclcpp::SyncParametersClient>(node);
+  parameters_client->wait_for_service();
+  const std::string schedule_type_str = parameters_client->get_parameter("schedule_type", std::string("deadline"));
+  std::cout << schedule_type_str << std::endl;
+  ExecutableScheduleType schedule_type = DEFAULT;
+  if (schedule_type_str == "deadline")
+  {
+    schedule_type = DEADLINE;
+  }
+  else if (schedule_type_str == "chain_priority")
+  {
+    schedule_type = CHAIN_AWARE_PRIORITY;
+  }
+  else
+  {
+    schedule_type = DEFAULT;
+  }
+
+  const std::vector<int64_t> chain_lengths = get_parameter_array(node, "chain_lengths", std::vector<int64_t>({3, 7}));
+
+  const std::vector<int64_t> chain_periods = get_parameter_array(node, "chain_periods", std::vector<int64_t>({1000, 1000}));
+  const std::vector<int64_t> chain_deadlines = get_parameter_array(node, "chain_deadlines", std::vector<int64_t>({1000, 1000}));
+  const std::vector<int64_t> chain_runtimes = get_parameter_array(node, "chain_runtimes", std::vector<int64_t>({131, 131}));
+  const std::vector<int64_t> chain_timer_runtimes = get_parameter_array(node, "chain_timer_runtimes", std::vector<int64_t>({109, 109}));
+
+  const std::vector<int64_t> chain_priorities = get_parameter_array(node, "chain_priorities", std::vector<int64_t>({1, 2}));
+  const uint NUM_CHAINS = chain_lengths.size();
+  if (chain_lengths.size() > chain_periods.size())
+  {
+    std::cout << "chain_periods shorter than chain_lengths" << std::endl;
+    exit(-1);
+  }
+
+  if (chain_lengths.size() > chain_runtimes.size())
+  {
+    std::cout << "chain_runtimes shorter than chain_lengths" << std::endl;
+    exit(-1);
+  }
+  if (chain_lengths.size() > chain_timer_runtimes.size())
+  {
+    std::cout << "chain_timer_runtimes shorter than chain_lengths" << std::endl;
+    exit(-1);
+  }
+  if (schedule_type == DEADLINE)
+  {
+    if (chain_lengths.size() > chain_deadlines.size())
+    {
+      std::cout << "chain_deadlines shorter than chain_lengths" << std::endl;
+      exit(-1);
+    }
+  }
+  else if (schedule_type == CHAIN_AWARE_PRIORITY)
+  {
+    if (chain_lengths.size() > chain_priorities.size())
+    {
+      std::cout << "chain_priorities shorter than chain_lengths" << std::endl;
+      exit(-1);
+    }
+  }
+
+  const uint COUNT_MAX = parameters_client->get_parameter("count_max", 20);
+  const std::string experiment_name = parameters_client->get_parameter("experiment_name", std::string("unnamed_experiment"));
+
+  rclcpp::ExecutorOptions options;
+  // use a modified memorystrategy
+  std::shared_ptr<PriorityMemoryStrategy<>> strat = std::make_shared<PriorityMemoryStrategy<>>();
+  strat->logger = create_logger();
+  // publisher
+  options.memory_strategy = strat;
+  rclcpp::Executor *sub1_executor = nullptr;
+
+  ROSDefaultExecutor *default_executor = nullptr;
+  if (schedule_type != DEFAULT)
+  {
+    timed_executor::TimedExecutor *rtis_executor = new timed_executor::TimedExecutor(options, "short_executor");
+    rtis_executor->set_use_priorities(true);
+    sub1_executor = rtis_executor;
+  }
+  else if (schedule_type == DEFAULT)
+  {
+    default_executor = new ROSDefaultExecutor();
+    default_executor->logger = create_logger();
+  }
+  // stock ROS executor
+  // rclcpp::executors::SingleThreadedExecutor sub1_executor;
+  std::vector<std::shared_ptr<DummyWorker>> workers;
+  std::vector<std::deque<uint> *> chain_deadlines_deque;
+  std::vector<std::shared_ptr<PublisherNode>> timers;
+  // TODO: make the chain layout configurable via rosparam
+  for (uint chain_index = 0; chain_index < NUM_CHAINS; chain_index++)
+  {
+    std::shared_ptr<rclcpp::TimerBase> this_chain_timer_handle;
+    std::deque<uint> *this_chain_deadlines_deque = new std::deque<uint>();
+    for (int cb_index = 0; cb_index < chain_lengths[chain_index]; cb_index++)
+    {
+      int total_prio = 0;
+      int this_chain_prio = 0;
+      if (schedule_type == CHAIN_AWARE_PRIORITY)
+      {
+        this_chain_prio = chain_priorities[chain_index];
+        total_prio = chain_lengths[chain_index];
+        for (uint eval_chain = 0; eval_chain < NUM_CHAINS; eval_chain++)
+        {
+          if (eval_chain == chain_index)
+          {
+            continue;
+          }
+          if (chain_priorities[eval_chain] < this_chain_prio)
+          {
+            total_prio += chain_lengths[eval_chain] * chain_priorities[eval_chain];
+          }
+        }
+      }
+      if (cb_index == 0)
+      {
+        std::shared_ptr<PublisherNode> pubnode = std::make_shared<PublisherNode>("topic_" + std::to_string(chain_index), chain_index, chain_periods[chain_index], chain_timer_runtimes[chain_index]);
+        pubnode->count_max = COUNT_MAX;
+        if (schedule_type == DEADLINE)
+        {
+          strat->set_executable_deadline(pubnode->timer_->get_timer_handle(), chain_deadlines[chain_index], TIMER, chain_index);
+        }
+        else if (schedule_type == CHAIN_AWARE_PRIORITY)
+        {
+          std::cout << "creating prio timer on chain " << std::to_string(chain_index) << " with prio " << std::to_string(chain_index) << std::endl;
+          strat->set_executable_priority(pubnode->timer_->get_timer_handle(), chain_index, TIMER, CHAIN_AWARE_PRIORITY, chain_index);
+        }
+        if (schedule_type != DEFAULT)
+        {
+          strat->set_first_in_chain(pubnode->timer_->get_timer_handle());
+          strat->assign_deadlines_queue(pubnode->timer_->get_timer_handle(), this_chain_deadlines_deque);
+          strat->get_priority_settings(pubnode->timer_->get_timer_handle())->timer_handle = pubnode->timer_;
+          this_chain_timer_handle = pubnode->timer_;
+        }
+        if (schedule_type == DEFAULT)
+        {
+          PriorityExecutable e;
+          e.chain_id = chain_index;
+          e.timer_handle = pubnode->timer_;
+          default_executor->priority_map[pubnode->timer_->get_timer_handle()] = e;
+          default_executor->add_node(pubnode);
+        }
+        else
+        {
+          sub1_executor->add_node(pubnode);
+        }
+        timers.push_back(pubnode);
+      }
+      else
+      {
+        auto sub1node = std::make_shared<DummyWorker>("chain_" + std::to_string(chain_index) + "_worker_" + std::to_string(cb_index), chain_runtimes[chain_index], chain_index, cb_index);
+        if (schedule_type == DEFAULT)
+        {
+          default_executor->add_node(sub1node);
+        }
+        else
+        {
+          sub1_executor->add_node(sub1node);
+        }
+        if (schedule_type == DEADLINE)
+        {
+          strat->set_executable_deadline(sub1node->subscription_->get_subscription_handle(), chain_deadlines[chain_index], SUBSCRIPTION, chain_index);
+        }
+        else if (schedule_type == CHAIN_AWARE_PRIORITY)
+        {
+          std::cout << "creating prio cb with prio " << std::to_string(chain_index) << std::endl;
+          strat->set_executable_priority(sub1node->subscription_->get_subscription_handle(), (chain_index), SUBSCRIPTION, CHAIN_AWARE_PRIORITY, chain_index);
+        }
+        if (schedule_type != DEFAULT)
+        {
+          strat->assign_deadlines_queue(sub1node->subscription_->get_subscription_handle(), this_chain_deadlines_deque);
+          if (cb_index == chain_lengths[chain_index] - 1)
+          {
+            strat->set_last_in_chain(sub1node->subscription_->get_subscription_handle());
+            strat->get_priority_settings(sub1node->subscription_->get_subscription_handle())->timer_handle = this_chain_timer_handle;
+          }
+        }
+        workers.push_back(sub1node);
+      }
+    }
+    chain_deadlines_deque.push_back(this_chain_deadlines_deque);
+  }
+  std::cout << "initialized nodes" << std::endl;
+  node_time_logger logger = create_logger();
+  // initialize first deadlines
+  timespec current_time;
+  clock_gettime(CLOCK_MONOTONIC_RAW, &current_time);
+  uint64_t millis = (current_time.tv_sec * (uint64_t)1000) + (current_time.tv_nsec / 1000000);
+  for (uint chain_index = 0; chain_index < NUM_CHAINS; chain_index++)
+  {
+    log_entry(logger, "deadline_" + std::to_string(chain_index) + "_" + std::to_string(millis + chain_deadlines[chain_index]));
+    log_entry(logger, "timer_" + std::to_string(chain_index) + "_release_" + std::to_string(millis));
+    chain_deadlines_deque[chain_index]->push_back(millis + chain_deadlines[chain_index]);
+    // chain_deadlines_deque[chain_index]->push_back(0);
+  }
+  if (schedule_type == DEFAULT)
+  {
+    default_executor->spin();
+  }
+  else
+  {
+    sub1_executor->spin();
+  }
+  rclcpp::shutdown();
+
+  // combine logs from all chains
+  std::vector<std::pair<std::string, u64>> combined_logs;
+  for (auto &worker : workers)
+  {
+    for (auto &log : *(worker->logger_.recorded_times))
+    {
+      combined_logs.push_back(log);
+    }
+  }
+  for (auto &timer : timers)
+  {
+    for (auto &log : *(timer->logger_.recorded_times))
+    {
+      combined_logs.push_back(log);
+    }
+  }
+  // add logs from strategy
+  for (auto &log : *(strat->logger.recorded_times))
+  {
+    combined_logs.push_back(log);
+  }
+
+  // if using the "default" executor, grab those logs too
+  if (schedule_type == DEFAULT)
+  {
+    for (auto &log : *(default_executor->logger.recorded_times))
+    {
+      combined_logs.push_back(log);
+    }
+  }
+
+  // sort logs
+  std::sort(combined_logs.begin(), combined_logs.end(), [](const std::pair<std::string, u64> &a, const std::pair<std::string, u64> &b)
+            { return a.second < b.second; });
+
+  std::ofstream output_file;
+  output_file.open("experiments/results/" + experiment_name + ".txt");
+  for (auto p : combined_logs)
+  {
+    output_file << p.second << " " << p.first << std::endl;
+  }
+  output_file.close();
+}

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 SHARED 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,46 @@
+// 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 "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()));
+    }
+}
+
+node_time_logger create_logger()
+{
+    node_time_logger logger;
+    logger.recorded_times = std::make_shared<std::vector<std::pair<std::string, u64>>>();
+    return logger;
+}