niklas/ROS-Dynamic-Executor
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity Actions

make a slimmed-down branch with just the exector and usage example

Browse source
This commit is contained in:
Kurt Wilson 2022-04-07 20:27:29 -04:00
parent e8d3d0b883
commit 34a7043006
27 changed files with 5 additions and 1445 deletions
Download patch file Download diff file Expand all files Collapse all files

80
src/priority_executor/CMakeLists.txt
View file

@ -32,81 +32,6 @@ 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
)
add_executable(usage_example src/usage_example.cpp)
@ -115,17 +40,14 @@ target_include_directories(usage_example PUBLIC
$<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
install(TARGETS priority_executor usage_example
DESTINATION lib/${PROJECT_NAME})
if(BUILD_TESTING)

71
src/priority_executor/include/priority_executor/default_executor.hpp
View file

@ -1,71 +0,0 @@
// 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_

14
src/priority_executor/include/priority_executor/primes_workload.hpp
View file

@ -1,14 +0,0 @@
#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

15
src/priority_executor/include/priority_executor/priority_memory_strategy.hpp
View file

@ -31,8 +31,6 @@
#include "rmw/types.h"
#include "simple_timer/rt-sched.hpp"
/// Delegate for handling memory allocations while the Executor is executing.
/**
* By default, the memory strategy dynamically allocates memory for structures that come in from
@ -219,9 +217,6 @@ class PriorityMemoryStrategy : public rclcpp::memory_strategy::MemoryStrategy
public:
RCLCPP_SMART_PTR_DEFINITIONS(PriorityMemoryStrategy<Alloc>)
node_time_logger logger;
bool is_f1tenth = false;
using VoidAllocTraits = typename rclcpp::allocator::AllocRebind<void *, Alloc>;
using VoidAlloc = typename VoidAllocTraits::allocator_type;
@ -677,12 +672,6 @@ public:
auto timer = next_exec->timer_handle;
int64_t time_until_next_call = timer->time_until_trigger().count() / 1000000;
// std::cout << "end of chain. time until trigger: " << std::to_string(time_until_next_call) << std::endl;
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));
}
}
if (next_exec->is_first_in_chain && next_exec->sched_type == DEADLINE)
{
@ -700,15 +689,13 @@ public:
}
int64_t time_until_next_call = timer->time_until_trigger().count() / 1000000;
// std::cout << "end of chain. time until trigger: " << std::to_string(time_until_next_call) << std::endl;
log_entry(logger, "timer_" + std::to_string(next_exec->chain_id) + "_release_" + std::to_string(millis + time_until_next_call));
uint64_t next_deadline = millis + time_until_next_call + next_exec->period;
next_exec->deadlines->push_back(next_deadline);
log_entry(logger, "deadline_" + std::to_string(next_exec->chain_id) + "_" + std::to_string(next_deadline));
// std::cout << "deadline set" << std::endl;
}
if (next_exec->is_last_in_chain && next_exec->sched_type == DEADLINE)
{
if (!next_exec->deadlines->empty() && (!is_f1tenth || next_exec->chain_id != 0))
if (!next_exec->deadlines->empty() && next_exec->chain_id != 0)
next_exec->deadlines->pop_front();
}
if (next_exec->sched_type == CHAIN_AWARE_PRIORITY || next_exec->sched_type == DEADLINE)

39
src/priority_executor/include/priority_executor/test_nodes.hpp
View file

@ -1,39 +0,0 @@
#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_;
};

67
src/priority_executor/src/default_executor.cpp
View file

@ -1,67 +0,0 @@
// 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;
// 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)));
}
}
execute_any_executable(any_executable);
}
}
}

331
src/priority_executor/src/f1tenth_test.cpp
View file

@ -1,331 +0,0 @@
#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;
// useful if testing different variations
std::string suffix = "";
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;
}

40
src/priority_executor/src/primes_workload.cpp
View file

@ -1,40 +0,0 @@
#include "priority_executor/primes_workload.hpp"
#include <iostream>
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;
}
if (cum_time - start_cpu_time > millis)
{
std::cout << "Warning: Time limit exceeded" << std::endl;
}
}
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;
}

1
src/priority_executor/src/priority_executor.cpp
View file

@ -16,7 +16,6 @@
#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>

81
src/priority_executor/src/test_nodes.cpp
View file

@ -1,81 +0,0 @@
#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);
}

298
src/priority_executor/src/test_publisher.cpp
View file

@ -1,298 +0,0 @@
#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();
}

4
src/priority_executor/src/usage_example.cpp
View file

@ -1,12 +1,10 @@
#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>
#include "std_msgs/msg/string.hpp"
// re-create the classic talker-listener example with two listeners
class Talker : public rclcpp::Node

61
src/simple_timer/CMakeLists.txt
View file

@ -1,61 +0,0 @@
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()

23
src/simple_timer/include/simple_timer/cycle_timer.hpp
View file

@ -1,23 +0,0 @@
#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

25
src/simple_timer/include/simple_timer/period_timer.hpp
View file

@ -1,25 +0,0 @@
#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

98
src/simple_timer/include/simple_timer/rt-sched.hpp
View file

@ -1,98 +0,0 @@
// 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__ */

18
src/simple_timer/package.xml
View file

@ -1,18 +0,0 @@
<?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>

43
src/simple_timer/src/cycle_timer.cpp
View file

@ -1,43 +0,0 @@
#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

56
src/simple_timer/src/period_timer.cpp
View file

@ -1,56 +0,0 @@
#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

46
src/simple_timer/src/rt-sched.cpp
View file

@ -1,46 +0,0 @@
// 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;
}