From e8d3d0b88394ee87e2cd8d4c73b92e8030422412 Mon Sep 17 00:00:00 2001 From: Kurt Wilson Date: Sat, 2 Apr 2022 18:13:46 -0400 Subject: [PATCH] add a basic usage example --- README.md | 3 + src/priority_executor/CMakeLists.txt | 18 ++++- .../priority_memory_strategy.hpp | 3 +- .../src/default_executor.cpp | 1 + src/priority_executor/src/usage_example.cpp | 81 +++++++++++++++++++ 5 files changed, 104 insertions(+), 2 deletions(-) create mode 100644 src/priority_executor/src/usage_example.cpp diff --git a/README.md b/README.md index 56202d7..969eeac 100644 --- a/README.md +++ b/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 + diff --git a/src/priority_executor/CMakeLists.txt b/src/priority_executor/CMakeLists.txt index 42275de..bbd0114 100644 --- a/src/priority_executor/CMakeLists.txt +++ b/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 + $ + $) +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) diff --git a/src/priority_executor/include/priority_executor/priority_memory_strategy.hpp b/src/priority_executor/include/priority_executor/priority_memory_strategy.hpp index e2e26b2..3bb0950 100644 --- a/src/priority_executor/include/priority_executor/priority_memory_strategy.hpp +++ b/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 handle, int period, ExecutableType t, int chain_id) + void set_executable_deadline(std::shared_ptr handle, int period, ExecutableType t, int chain_id = 0) { // TODO: any sanity checks should go here // priority_map.insert(executable, priority); diff --git a/src/priority_executor/src/default_executor.cpp b/src/priority_executor/src/default_executor.cpp index 4caaa22..324077f 100644 --- a/src/priority_executor/src/default_executor.cpp +++ b/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))); } } diff --git a/src/priority_executor/src/usage_example.cpp b/src/priority_executor/src/usage_example.cpp new file mode 100644 index 0000000..3c319b8 --- /dev/null +++ b/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 +#include +#include "simple_timer/rt-sched.hpp" +#include "priority_executor/default_executor.hpp" +#include + +// 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("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::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("chatter", 10, std::bind(&Listener::callback, this, std::placeholders::_1)); + } + // the publisher must be public + rclcpp::Subscription::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(); + auto listener1 = std::make_shared("listener1"); + auto listener2 = std::make_shared("listener2"); + rclcpp::ExecutorOptions options; + + auto strategy = std::make_shared>(); + 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(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(); +}