make a slimmed-down branch with just the exector and usage example
This commit is contained in:
Kurt Wilson
parent
e8d3d0b883
commit
34a7043006
27 changed files with 5 additions and 1445 deletions
|
|
@ -1,13 +1,10 @@
|
||||||
This contains two packages:
|
This contains the priority_executor package that adds an Executor with deadline and chain-aware priority support.
|
||||||
- `simple_timer` contains timing related functions, and a wrapper for calling some kernel scheduling functions
|
- `simple_timer` contains timing related functions, and a wrapper for calling some kernel scheduling functions
|
||||||
- `priority_executor` contains the modified executor and some nodes to test it
|
- `priority_executor` contains the modified executor and some nodes to test it
|
||||||
|
|
||||||
In `priority_executor`:
|
In `priority_executor`:
|
||||||
- `priority_executor.cpp` subclasses `rclcpp::Executor` to allow for additional customization
|
- `priority_executor.cpp` subclasses `rclcpp::Executor` to allow for additional customization
|
||||||
- `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.
|
- `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
|
- `usage_example.cpp` shows how to set deadlines for callbacks and timers
|
||||||
|
|
||||||
|
the `main` branch of this repository has the test scripts and nodes used in the paper.
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
experiment_parameters:
|
|
||||||
ros__parameters:
|
|
||||||
experiment_name: "picas_priority_long"
|
|
||||||
schedule_type: "chain_priority"
|
|
||||||
count_max: 5000
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
experiment_parameters:
|
|
||||||
ros__parameters:
|
|
||||||
experiment_name: "picas_priority_short"
|
|
||||||
schedule_type: "chain_priority"
|
|
||||||
count_max: 500
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
experiment_parameters:
|
|
||||||
ros__parameters:
|
|
||||||
experiment_name: "ros_default_long"
|
|
||||||
schedule_type: "default"
|
|
||||||
count_max: 5000
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
experiment_parameters:
|
|
||||||
ros__parameters:
|
|
||||||
experiment_name: "ros_default_short"
|
|
||||||
schedule_type: "default"
|
|
||||||
count_max: 500
|
|
||||||
|
|
@ -1,5 +0,0 @@
|
||||||
experiment_parameters:
|
|
||||||
ros__parameters:
|
|
||||||
experiment_name: "rtis_deadline_long"
|
|
||||||
schedule_type: "deadline"
|
|
||||||
count_max: 5000
|
|
||||||
|
|
@ -1,7 +0,0 @@
|
||||||
experiment_parameters:
|
|
||||||
ros__parameters:
|
|
||||||
experiment_name: "rtis_deadline_short"
|
|
||||||
schedule_type: "deadline"
|
|
||||||
count_max: 500
|
|
||||||
chain_deadlines: [1000, 1000]
|
|
||||||
chain_periods: [1000, 1000]
|
|
||||||
|
|
@ -32,81 +32,6 @@ ament_target_dependencies(priority_executor
|
||||||
rmw
|
rmw
|
||||||
rclcpp
|
rclcpp
|
||||||
rcl
|
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)
|
add_executable(usage_example src/usage_example.cpp)
|
||||||
|
|
@ -115,17 +40,14 @@ target_include_directories(usage_example PUBLIC
|
||||||
$<INSTALL_INTERFACE:include>)
|
$<INSTALL_INTERFACE:include>)
|
||||||
target_link_libraries(usage_example
|
target_link_libraries(usage_example
|
||||||
priority_executor
|
priority_executor
|
||||||
test_nodes
|
|
||||||
default_executor
|
|
||||||
)
|
)
|
||||||
ament_target_dependencies(usage_example
|
ament_target_dependencies(usage_example
|
||||||
rclcpp
|
rclcpp
|
||||||
std_msgs
|
std_msgs
|
||||||
std_srvs
|
std_srvs
|
||||||
simple_timer
|
|
||||||
)
|
)
|
||||||
|
|
||||||
install(TARGETS f1tenth_publisher priority_executor usage_example
|
install(TARGETS priority_executor usage_example
|
||||||
DESTINATION lib/${PROJECT_NAME})
|
DESTINATION lib/${PROJECT_NAME})
|
||||||
|
|
||||||
if(BUILD_TESTING)
|
if(BUILD_TESTING)
|
||||||
|
|
|
||||||
|
|
@ -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_
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -31,8 +31,6 @@
|
||||||
|
|
||||||
#include "rmw/types.h"
|
#include "rmw/types.h"
|
||||||
|
|
||||||
#include "simple_timer/rt-sched.hpp"
|
|
||||||
|
|
||||||
/// Delegate for handling memory allocations while the Executor is executing.
|
/// Delegate for handling memory allocations while the Executor is executing.
|
||||||
/**
|
/**
|
||||||
* By default, the memory strategy dynamically allocates memory for structures that come in from
|
* By default, the memory strategy dynamically allocates memory for structures that come in from
|
||||||
|
|
@ -219,9 +217,6 @@ class PriorityMemoryStrategy : public rclcpp::memory_strategy::MemoryStrategy
|
||||||
public:
|
public:
|
||||||
RCLCPP_SMART_PTR_DEFINITIONS(PriorityMemoryStrategy<Alloc>)
|
RCLCPP_SMART_PTR_DEFINITIONS(PriorityMemoryStrategy<Alloc>)
|
||||||
|
|
||||||
node_time_logger logger;
|
|
||||||
bool is_f1tenth = false;
|
|
||||||
|
|
||||||
using VoidAllocTraits = typename rclcpp::allocator::AllocRebind<void *, Alloc>;
|
using VoidAllocTraits = typename rclcpp::allocator::AllocRebind<void *, Alloc>;
|
||||||
using VoidAlloc = typename VoidAllocTraits::allocator_type;
|
using VoidAlloc = typename VoidAllocTraits::allocator_type;
|
||||||
|
|
||||||
|
|
@ -677,12 +672,6 @@ public:
|
||||||
auto timer = next_exec->timer_handle;
|
auto timer = next_exec->timer_handle;
|
||||||
int64_t time_until_next_call = timer->time_until_trigger().count() / 1000000;
|
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;
|
// 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)
|
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;
|
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;
|
// 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;
|
uint64_t next_deadline = millis + time_until_next_call + next_exec->period;
|
||||||
next_exec->deadlines->push_back(next_deadline);
|
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;
|
// std::cout << "deadline set" << std::endl;
|
||||||
}
|
}
|
||||||
if (next_exec->is_last_in_chain && next_exec->sched_type == DEADLINE)
|
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();
|
next_exec->deadlines->pop_front();
|
||||||
}
|
}
|
||||||
if (next_exec->sched_type == CHAIN_AWARE_PRIORITY || next_exec->sched_type == DEADLINE)
|
if (next_exec->sched_type == CHAIN_AWARE_PRIORITY || next_exec->sched_type == DEADLINE)
|
||||||
|
|
|
||||||
|
|
@ -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_;
|
|
||||||
};
|
|
||||||
|
|
@ -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, ¤t_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);
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
@ -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, ¤t_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;
|
|
||||||
}
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
|
|
@ -16,7 +16,6 @@
|
||||||
#include "priority_executor/priority_memory_strategy.hpp"
|
#include "priority_executor/priority_memory_strategy.hpp"
|
||||||
#include "rclcpp/any_executable.hpp"
|
#include "rclcpp/any_executable.hpp"
|
||||||
#include "rclcpp/scope_exit.hpp"
|
#include "rclcpp/scope_exit.hpp"
|
||||||
#include "simple_timer/rt-sched.hpp"
|
|
||||||
#include "rclcpp/utilities.hpp"
|
#include "rclcpp/utilities.hpp"
|
||||||
#include "rclcpp/exceptions.hpp"
|
#include "rclcpp/exceptions.hpp"
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
|
|
||||||
|
|
@ -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);
|
|
||||||
}
|
|
||||||
|
|
@ -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, ¤t_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();
|
|
||||||
}
|
|
||||||
|
|
@ -1,12 +1,10 @@
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "priority_executor/priority_executor.hpp"
|
#include "priority_executor/priority_executor.hpp"
|
||||||
#include "priority_executor/priority_memory_strategy.hpp"
|
#include "priority_executor/priority_memory_strategy.hpp"
|
||||||
#include "priority_executor/test_nodes.hpp"
|
|
||||||
#include <string>
|
#include <string>
|
||||||
#include <fstream>
|
#include <fstream>
|
||||||
#include "simple_timer/rt-sched.hpp"
|
|
||||||
#include "priority_executor/default_executor.hpp"
|
|
||||||
#include <unistd.h>
|
#include <unistd.h>
|
||||||
|
#include "std_msgs/msg/string.hpp"
|
||||||
|
|
||||||
// re-create the classic talker-listener example with two listeners
|
// re-create the classic talker-listener example with two listeners
|
||||||
class Talker : public rclcpp::Node
|
class Talker : public rclcpp::Node
|
||||||
|
|
|
||||||
|
|
@ -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()
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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__ */
|
|
||||||
|
|
@ -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>
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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
|
|
||||||
|
|
@ -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;
|
|
||||||
}
|
|
||||||
Loading…
Add table
Add a link
Reference in a new issue