add a basic usage example

README.md

@@ -7,4 +7,7 @@ In `priority_executor`:
  - `priority_memory_strategy.hpp` is a modified version of `rclcpp`s `allocator_memory_strategy.hpp` that selects callbacks based on either the earliest deadline, or a relative priority. Executor polls this for ready callbacks. 
  - `test_nodes.cpp` adds timer-based publishing nodes and dummy worker nodes that can be arranged in chains. It uses `dummy_workload.hpp` to generate a workload.
  - `f1tenth_test.cpp` sets up a chain of nodes similar to [https://intra.ece.ucr.edu/~hyoseung/pdf/rtas21_picas.pdf](https://intra.ece.ucr.edu/~hyoseung/pdf/rtas21_picas.pdf)
+ - `test_publisher.cpp` creates two small chains on a single executor to demonstrate an overloaded executor.
+ - `usage_example.cpp` shows how to set deadlines for callbacks and timers
+
 

src/priority_executor/CMakeLists.txt

@@ -109,7 +109,23 @@ ament_target_dependencies(f1tenth_publisher
   simple_timer 
 )
 
-install(TARGETS f1tenth_publisher priority_executor
+add_executable(usage_example src/usage_example.cpp)
+target_include_directories(usage_example PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+target_link_libraries(usage_example
+  priority_executor
+  test_nodes
+  default_executor
+)
+ament_target_dependencies(usage_example
+  rclcpp
+  std_msgs
+  std_srvs
+  simple_timer 
+)
+
+install(TARGETS f1tenth_publisher priority_executor usage_example
   DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)

src/priority_executor/include/priority_executor/priority_memory_strategy.hpp

@@ -680,6 +680,7 @@ public:
                 log_entry(logger, "timer_" + std::to_string(next_exec->chain_id) + "_release_" + std::to_string(millis + time_until_next_call));
                 if (next_exec->chain_id == 0 && is_f1tenth)
                 {
+                    // special case for logging the shared timer
                     log_entry(logger, "timer_" + std::to_string(next_exec->chain_id + 1) + "_release_" + std::to_string(millis + time_until_next_call));
                 }
             }
@@ -999,7 +1000,7 @@ public:
         priority_map[handle].chain_id = chain_index;
     }
 
-    void set_executable_deadline(std::shared_ptr<const void> handle, int period, ExecutableType t, int chain_id)
+    void set_executable_deadline(std::shared_ptr<const void> handle, int period, ExecutableType t, int chain_id = 0)
     {
         // TODO: any sanity checks should go here
         // priority_map.insert(executable, priority);

src/priority_executor/src/default_executor.cpp

@@ -57,6 +57,7 @@ void ROSDefaultExecutor::spin()
           PriorityExecutable next_exec = priority_map[any_executable.timer->get_timer_handle()];
 
           auto timer = next_exec.timer_handle;
+          // TODO: this is really a fire
           log_entry(logger, "timer_" + std::to_string(next_exec.chain_id) + "_release_" + std::to_string(millis + (timer->time_until_trigger().count() / 1000000)));
         }
       }

src/priority_executor/src/usage_example.cpp

@@ -0,0 +1,81 @@
+#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>
+
+// re-create the classic talker-listener example with two listeners
+class Talker : public rclcpp::Node
+{
+public:
+    Talker() : Node("talker")
+    {
+        // 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));
+    }
+    // the timer must be public
+    rclcpp::TimerBase::SharedPtr timer_;
+
+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_;
+};
+
+class Listener : public rclcpp::Node
+{
+public:
+    Listener(std::string name) : Node(name)
+    {
+        // Create a subscription on the "chatter" topic with the default callback 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_;
+
+private:
+    void callback(const std_msgs::msg::String::SharedPtr msg)
+    {
+        RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
+    }
+};
+
+int main(int argc, char **argv)
+{
+    rclcpp::init(argc, argv);
+    auto talker = std::make_shared<Talker>();
+    auto listener1 = std::make_shared<Listener>("listener1");
+    auto listener2 = std::make_shared<Listener>("listener2");
+    rclcpp::ExecutorOptions options;
+
+    auto strategy = std::make_shared<PriorityMemoryStrategy<>>();
+    options.memory_strategy = strategy;
+    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);
+
+    strategy->set_executable_deadline(talker->timer_->get_timer_handle(), 100, TIMER);
+    strategy->set_executable_deadline(listener1->sub_->get_subscription_handle(), 100, SUBSCRIPTION);
+    strategy->set_executable_deadline(listener2->sub_->get_subscription_handle(), 50, SUBSCRIPTION);
+    executor->add_node(talker);
+    executor->add_node(listener1);
+    executor->add_node(listener2);
+
+    executor->spin();
+}