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

got ros and edf to work, built startup script for reliable experimenting

Browse source
This commit is contained in:
Niklas Halle 2025-06-09 15:24:34 +02:00
parent bf3f7ff485
commit 8cd5b03b03
9 changed files with 745 additions and 214 deletions
Download patch file Download diff file Expand all files Collapse all files

6
build.sh
View file

@ -1,5 +1,7 @@
#!/usr/bin/env bash #!/usr/bin/env bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
colcon build --packages-select tracetools --allow-overriding tracetools && \ colcon build --packages-select tracetools --allow-overriding tracetools && \
source install/setup.bash && \ source install/setup.bash && \
colcon build --packages-select cyclonedds --allow-overriding cyclonedds && \ colcon build --packages-select cyclonedds --allow-overriding cyclonedds && \
@ -10,7 +12,9 @@ colcon build --packages-select tracetools --allow-overriding tracetools && \
source install/setup.bash && \ source install/setup.bash && \
colcon build --packages-select rclcpp --allow-overriding rclcpp && \ colcon build --packages-select rclcpp --allow-overriding rclcpp && \
source install/setup.bash && \ source install/setup.bash && \
colcon build --packages-up-to full_topology && \ colcon build --packages-select simple_timer priority_executor && \
source install/setup.bash && \
colcon build --packages-up-to ros2trace ros2trace_analysis full_topology && \
source install/setup.bash source install/setup.bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp

71
build_run_copy.sh
View file

@ -1,9 +1,66 @@
#!/usr/bin/env bash #!/usr/bin/env bash
colcon build --packages-select full_topology && \ set -e
source install/setup.bash && \
TRACE_ID=$(taskset -c 2,3 ros2 launch full_topology trace_full_topology.launch.py length:=100 | \ # ----------- CONFIGURABLES -----------
grep -oP '(?<=Writing tracing session to: /home/niklas/ROS-Dynamic-Executor-Experiments/analysis/tracing/full_topology_tracing-)\d+') && \ REMOTE_USER="niklas"
ros2 trace-analysis convert "/home/niklas/ROS-Dynamic-Executor-Experiments/analysis/tracing/full_topology_tracing-${TRACE_ID}/ust" && \ REMOTE_HOST="192.168.162.249"
scp -r "analysis/tracing/full_topology_tracing-${TRACE_ID}/" 192.168.162.249:/home/niklas/dataflow-analysis/traces/ && \ REMOTE_PATH="/home/niklas/dataflow-analysis/traces"
echo "Trace analysis \"${TRACE_ID}\" complete and copied to remote server." TRACING_BASE="analysis/tracing"
LOCAL_PROJECT_DIR="/home/niklas/ROS-Dynamic-Executor-Experiments"
# --------------------------------------
usage() {
echo "Usage: $0 <ros|edf|sem> <single|multi> <timed|direct> [time_in_seconds]"
exit 1
}
if [[ $# -lt 3 ]]; then
usage
fi
SCHEDULER_TYPE="$1"
THREADING_TYPE="$2"
FUSION_TYPE="$3"
TRACE_LENGTH="${4:-10}"
# Determine defines based on parameters
CMAKE_ARGS=()
[[ "$SCHEDULER_TYPE" == "ros" ]] && CMAKE_ARGS+=("-DROS_DEFAULT_EXECUTOR=ON")
[[ "$SCHEDULER_TYPE" == "edf" ]] && CMAKE_ARGS+=("-DEDF_PRIORITY_EXECUTOR=ON")
[[ "$SCHEDULER_TYPE" == "sem" ]] && CMAKE_ARGS+=("-DSEMANTIC_EXECUTOR=ON")
[[ "$THREADING_TYPE" == "multi" ]] && CMAKE_ARGS+=("-DMULTI_THREADED=ON")
[[ "$FUSION_TYPE" == "timed" ]] && CMAKE_ARGS+=("-DUSE_TIMER_IN_FUSION_NODES=ON")
TRACE_NAME="${SCHEDULER_TYPE}_${THREADING_TYPE}_${FUSION_TYPE}_${TRACE_LENGTH}"
cd "$LOCAL_PROJECT_DIR"
colcon build --packages-select full_topology --cmake-args "${CMAKE_ARGS[@]}"
source install/setup.bash
TRACE_ID=$(taskset -c 2,3 ros2 launch full_topology trace_full_topology.launch.py "length:=${TRACE_LENGTH}" | \
grep -oP '(?<=Writing tracing session to: /home/niklas/ROS-Dynamic-Executor-Experiments/analysis/tracing/)[^\s]+')
if [[ -z "$TRACE_ID" ]]; then
echo "[ERROR] Failed to get tracing session directory. Aborting."
exit 2
fi
TIMESTAMP_SUFFIX="${TRACE_ID#trace_full_topology}"
NEW_TRACE_ID="${TRACE_NAME}${TIMESTAMP_SUFFIX}"
TRACE_ORIG_PATH="${TRACING_BASE}/${TRACE_ID}"
TRACE_NEW_PATH="${TRACING_BASE}/${NEW_TRACE_ID}"
ros2 trace-analysis convert "${TRACE_ORIG_PATH}/ust"
if [[ -e "$TRACE_NEW_PATH" ]]; then
rm -rf "$TRACE_NEW_PATH"
fi
mv "$TRACE_ORIG_PATH" "$TRACE_NEW_PATH"
scp -r "$TRACE_NEW_PATH/" "$REMOTE_USER@$REMOTE_HOST:$REMOTE_PATH/"
echo "[SUCCESS] Trace analysis '$NEW_TRACE_ID' complete and copied to remote server."

32
src/full_topology/CMakeLists.txt
View file

@ -12,14 +12,43 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
endif() endif()
# find dependencies # find dependencies
find_package(priority_executor REQUIRED)
find_package(ament_cmake REQUIRED) find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED) find_package(rclcpp REQUIRED)
find_package(std_msgs REQUIRED) find_package(std_msgs REQUIRED)
# Of the following three, only one should be one at any given time. If none is set, ros default will be used
option(ROS_DEFAULT_EXECUTOR "Use ROS default executor" OFF)
option(EDF_PRIORITY_EXECUTOR "Use EDF priority executor" OFF)
option(SEMANTIC_EXECUTOR "Use semantic executor" OFF)
option(MULTI_THREADED "Use multi-threading" OFF)
option(USE_TIMER_IN_FUSION_NODES "Use timers in fusion nodes" OFF)
if(ROS_DEFAULT_EXECUTOR)
add_compile_definitions(ROS_DEFAULT_EXECUTOR)
endif()
if(EDF_PRIORITY_EXECUTOR)
add_compile_definitions(EDF_PRIORITY_EXECUTOR)
endif()
if(SEMANTIC_EXECUTOR)
add_compile_definitions(SEMANTIC_EXECUTOR)
endif()
if(MULTI_THREADED)
add_compile_definitions(MULTI_THREADED)
endif()
if(USE_TIMER_IN_FUSION_NODES)
add_compile_definitions(USE_TIMER_IN_FUSION_NODES)
endif()
add_compile_definitions(RCLCPP_ENABLE_TRACEPOINTS) add_compile_definitions(RCLCPP_ENABLE_TRACEPOINTS)
add_executable(${PROJECT_NAME} src/full_topology.cpp) add_executable(${PROJECT_NAME} src/full_topology.cpp)
#add_executable(${PROJECT_NAME} src/test.cpp) #add_executable(${PROJECT_NAME} src/simple.cpp)
target_include_directories(${PROJECT_NAME} PUBLIC target_include_directories(${PROJECT_NAME} PUBLIC
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include> $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include> $<INSTALL_INTERFACE:include>
@ -27,6 +56,7 @@ target_include_directories(${PROJECT_NAME} PUBLIC
# Add dependencies with ament # Add dependencies with ament
ament_target_dependencies(${PROJECT_NAME} ament_target_dependencies(${PROJECT_NAME}
priority_executor
rclcpp rclcpp
std_msgs std_msgs
tracetools tracetools

19
src/full_topology/launch/trace_full_topology.launch.py
View file

@ -17,7 +17,7 @@ def generate_launch_description():
return launch.LaunchDescription([ return launch.LaunchDescription([
length_arg, length_arg,
Trace( Trace(
session_name='full_topology_tracing', session_name='trace_full_topology',
append_timestamp=True, append_timestamp=True,
base_path=target_path, base_path=target_path,
events_ust=[ events_ust=[
@ -29,23 +29,8 @@ def generate_launch_description():
'rclcpp:*callback*', 'rclcpp:*callback*',
'rclcpp:*executor*', 'rclcpp:*executor*',
'rmw_cyclonedds:*', 'rmw_cyclonedds:*',
# explicit list of message‑flow events
'ros2:rclcpp_publish',
'ros2:rclcpp_subscription_callback',
'ros2:rclcpp_timer_callback',
'ros2:rclcpp_executor_execute',
'ros2:rclcpp_intra_publish',
'ros2:rmw_take',
'ros2:callback_start',
'ros2:callback_end',
# ---- (optional) add whatever else you like ----
# scheduling / executor / memory events:
'ros2:rclcpp_executor_*',
'ros2:*callback_added',
],
events_kernel=[
'sched_switch', 'sched_wakeup'
], ],
events_kernel=[],
), ),
Node( Node(
package='full_topology', package='full_topology',

1
src/full_topology/package.xml
View file

@ -15,6 +15,7 @@
<depend>tracetools_launch</depend> <depend>tracetools_launch</depend>
<!-- Build Dependencies --> <!-- Build Dependencies -->
<depend>priority_executor</depend>
<depend>rclcpp</depend> <depend>rclcpp</depend>
<depend>std_msgs</depend> <depend>std_msgs</depend>
<depend>tracetools</depend> <depend>tracetools</depend>

504
src/full_topology/src/full_topology.cpp
View file

@ -9,222 +9,244 @@
// the order here matters // the order here matters
#include "tracetools/utils.hpp" #include "tracetools/utils.hpp"
#include <rclcpp/rclcpp.hpp> #include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp" #include "std_msgs/msg/string.hpp"
/** Whether to use timers in fusion nodes */ #include "nodes.hpp"
//#define USE_TIMER_IN_FUSION_NODES
using namespace std::chrono_literals; #include "priority_executor/priority_executor.hpp"
#include "priority_executor/multithread_priority_executor.hpp"
#include "priority_executor/priority_memory_strategy.hpp"
constexpr bool DEBUG = false; #define PROCENTUAL_INPUT_JITTER 0.05
#define PROCENTUAL_PROCESSING_JITTER 0.1
// A generic sensor node that publishes at a given rate + jitter #define STR_HELPER(x) #x
#define DEFINE_SENSOR_NODE_CLASS(NAME) \ #define STR(x) STR_HELPER(x)
class NAME : public rclcpp::Node \
{ \ #ifndef DEBUG
public: \ #define PRINT_BEFORE(CHAIN_ID, NAME) \
NAME(std::string const &name, \ std::cout << "Chain " << CHAIN_ID << " before: " << CHAIN_DURATION(CHAIN_ID) << " ms" << std::endl;
std::string const &topic, \ #define PRINT_AFTER(CHAIN_ID, NAME) \
int rate_hz, \ std::cout << "Chain " << CHAIN_ID << " after: " << CHAIN_DURATION(CHAIN_ID) << " ms" << std::endl;
double jitter_ms_ = 0.0) \ #else
: Node(name) \ #define PRINT_BEFORE(CHAIN_ID, NAME)
{ \ #define PRINT_AFTER(CHAIN_ID, NAME)
publisher_ = create_publisher<std_msgs::msg::String>(topic, 10); \ #endif
auto period = std::chrono::milliseconds(1000 / rate_hz); \
timer_ = create_wall_timer(period, [this, name, jitter_ms_]() \ #ifdef EDF_PRIORITY_EXECUTOR
{ \ #define CHAIN_DURATION(CHAIN_ID) chain_duration_##CHAIN_ID
double jitter = 0.0; \ #define DECLARE_CHAIN(CHAIN_ID) double CHAIN_DURATION(CHAIN_ID) = 0.0;
if (jitter_ms_ > 0.0) { \ #define SUM_CHAIN_DURATION(CHAIN_ID, NAME) \
static thread_local std::mt19937 gen(std::random_device{}()); \ PRINT_BEFORE(CHAIN_ID, NAME); \
std::normal_distribution<> d(0.0, jitter_ms_); \ CHAIN_DURATION(CHAIN_ID) += NAME##_duration + NAME##_jitter; \
jitter = d(gen); \ PRINT_AFTER(CHAIN_ID, NAME);
#else
#define DECLARE_CHAIN(CHAIN_ID)
#define SUM_CHAIN_DURATION(CHAIN_ID, NAME)
#endif
#define MAKE_INPUT_NODE(CHAIN_ID, NAME, CLASS, TOPIC, PERIOD) \
auto NAME##_duration = (PERIOD); \
auto NAME##_jitter = PROCENTUAL_INPUT_JITTER * (PERIOD); \
auto NAME = std::make_shared<CLASS>(std::string("input_" STR(NAME) "_node"), TOPIC, NAME##_duration, NAME##_jitter); \
SUM_CHAIN_DURATION(CHAIN_ID, NAME)
#define MAKE_PROCESSING_NODE(CHAIN_ID, NAME, CLASS, INPUT_TOPIC, OUTPUT_TOPIC, DURATION) \
auto NAME##_duration = (DURATION); \
auto NAME##_jitter = PROCENTUAL_PROCESSING_JITTER * (DURATION); \
auto NAME = std::make_shared<CLASS>(std::string(STR(NAME) "_node"), INPUT_TOPIC, OUTPUT_TOPIC, NAME##_duration, NAME##_jitter); \
SUM_CHAIN_DURATION(CHAIN_ID, NAME)
#define MAKE_OUTPUT_NODE(CHAIN_ID, NAME, CLASS, INPUT_TOPIC, OUTPUT_TOPIC, DURATION) \
auto NAME##_duration = (DURATION); \
auto NAME##_jitter = PROCENTUAL_PROCESSING_JITTER * (DURATION); \
auto NAME = std::make_shared<CLASS>(std::string("output_" STR(NAME) "_node"), INPUT_TOPIC, OUTPUT_TOPIC, NAME##_duration, NAME##_jitter); \
SUM_CHAIN_DURATION(CHAIN_ID, NAME)
#ifdef USE_TIMER_IN_FUSION_NODES
#define REGISTER_MULTI_PROCESSING_NODE(NAME, CHAIN_PERIOD, CHAIN_ID) \
for (auto const &sub : NAME->subscriptions_) { \
strategy->set_executable_deadline(sub->get_subscription_handle(), CHAIN_PERIOD, priority_executor::ExecutableType::SUBSCRIPTION, CHAIN_ID); \
} \ } \
/* Ensure total delay is non-negative */ \ strategy->set_executable_deadline(NAME->timer_->get_timer_handle(), CHAIN_PERIOD, priority_executor::ExecutableType::TIMER, CHAIN_ID);
int total_delay = std::max(0, static_cast<int>(std::round(jitter))); \ #else
std::this_thread::sleep_for(std::chrono::milliseconds(total_delay)); \ #define REGISTER_MULTI_PROCESSING_NODE(NAME, CHAIN_PERIOD, CHAIN_ID) \
auto msg = std_msgs::msg::String(); \ for (auto const &sub : NAME->subscriptions_) { \
msg.data = name + std::string(" publishes"); \ strategy->set_executable_deadline(sub->get_subscription_handle(), CHAIN_PERIOD, priority_executor::ExecutableType::SUBSCRIPTION, CHAIN_ID); \
if (DEBUG) { \ }
RCLCPP_INFO(get_logger(), "%s", msg.data.c_str()); \ #endif
} \
publisher_->publish(msg); }); \
} \
\
private: \
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; \
rclcpp::TimerBase::SharedPtr timer_; \
};
// A generic single-input processing node with a fixed delay +/- jitter int edf_usage_example(int argc, char **argv);
#define DEFINE_PROCESSING_NODE_CLASS(NAME) \
class NAME : public rclcpp::Node \
{ \
public: \
NAME(const std::string & name, \
const std::string & input_topic, \
const std::string & output_topic, \
int delay_ms, \
double jitter_ms = 0.0) \
: Node(name), delay_ms_(delay_ms), jitter_ms_(jitter_ms) \
{ \
subscription_ = create_subscription<std_msgs::msg::String>( \
input_topic, 10, \
[this, name](std_msgs::msg::String::SharedPtr msg) \
{ \
if (DEBUG) { \
RCLCPP_INFO(get_logger(), "%s received: %s", name.c_str(), msg->data.c_str()); \
} \
/* Simulate processing delay */ \
double jitter = 0.0; \
if (jitter_ms_ > 0.0) { \
static thread_local std::mt19937 gen(std::random_device{}());\
std::normal_distribution<> d(0.0, jitter_ms_); \
jitter = d(gen); \
} \
std::this_thread::sleep_for(std::chrono::milliseconds((int)(delay_ms_ + jitter))); \
auto out = std_msgs::msg::String(); \
out.data = msg->data + " -> " + name; \
publisher_->publish(out); \
}); \
publisher_ = create_publisher<std_msgs::msg::String>(output_topic, 10); \
} \
protected: \
int delay_ms_; \
double jitter_ms_; \
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_; \
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; \
};
// A generic single-input processing node with a fixed delay +/- jitter
#define DEFINE_MULTI_PROCESSING_NODE_CLASS(NAME) \
class NAME : public rclcpp::Node \
{ \
public: \
NAME(std::string const &name, \
std::vector<std::string> const &input_topics, \
std::string const &output_topic, \
int delay_ms, \
double jitter_ms = 0.0) \
: Node(name), delay_ms_(delay_ms), jitter_ms_(jitter_ms) \
{ \
for (auto &topic : input_topics) \
{ \
auto sub = create_subscription<std_msgs::msg::String>( \
topic, 10, \
[this, name](std_msgs::msg::String::SharedPtr msg) \
{ \
/* simple fusion: log each arrival */ \
if (DEBUG) \
{ \
RCLCPP_INFO(get_logger(), "%s fusing input: %s", name.c_str(), msg->data.c_str()); \
} \
/* Simulate processing delay */ \
double jitter = 0.0; \
if (jitter_ms_ > 0.0) { \
static thread_local std::mt19937 gen(std::random_device{}()); \
std::normal_distribution<> d(0.0, jitter_ms_); \
jitter = d(gen); \
} \
std::this_thread::sleep_for(std::chrono::milliseconds((int)(delay_ms_ + jitter))); \
auto out = std_msgs::msg::String(); \
out.data = msg->data + " -> " + name; \
if (DEBUG) { \
RCLCPP_INFO(get_logger(), "%s publishes fused message", name.c_str()); \
} \
publisher_->publish(out); \
}); \
subscriptions_.emplace_back(sub); \
} \
publisher_ = create_publisher<std_msgs::msg::String>(output_topic, 10); \
\
} \
protected: \
std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> subscriptions_; \
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; \
int delay_ms_; \
double jitter_ms_; \
};
DEFINE_SENSOR_NODE_CLASS(CameraNode)
DEFINE_PROCESSING_NODE_CLASS(DebayerNode)
DEFINE_PROCESSING_NODE_CLASS(RadiometricNode)
DEFINE_PROCESSING_NODE_CLASS(GeometricNode)
DEFINE_PROCESSING_NODE_CLASS(MappingNode)
DEFINE_PROCESSING_NODE_CLASS(SmokeClassifierNode)
DEFINE_SENSOR_NODE_CLASS(GPSNode)
DEFINE_SENSOR_NODE_CLASS(IMUNode)
DEFINE_SENSOR_NODE_CLASS(BaroNode)
DEFINE_PROCESSING_NODE_CLASS(FusionNodeS)
DEFINE_MULTI_PROCESSING_NODE_CLASS(FusionNode)
DEFINE_SENSOR_NODE_CLASS(LidarNode)
DEFINE_SENSOR_NODE_CLASS(CommandNode)
DEFINE_PROCESSING_NODE_CLASS(FlightManagementNodeS)
DEFINE_MULTI_PROCESSING_NODE_CLASS(FlightManagementNode)
DEFINE_PROCESSING_NODE_CLASS(ControlNode)
DEFINE_PROCESSING_NODE_CLASS(TelemetryNodeS)
DEFINE_MULTI_PROCESSING_NODE_CLASS(TelemetryNode)
DEFINE_PROCESSING_NODE_CLASS(RadioNode)
#define PROCENTUAL_JITTER 0.1
#define DELAY_JITTER_PAIR(DELAY) DELAY, PROCENTUAL_JITTER * DELAY
int main(int argc, char **argv) int main(int argc, char **argv)
{ {
rclcpp::init(argc, argv); rclcpp::init(argc, argv);
DECLARE_CHAIN(0); // Chain 1: Image2Ground Mapping
DECLARE_CHAIN(1); // Chain 2: Semantic Scheduler Adaptation
DECLARE_CHAIN(2); // Chain 3: Flight Control
DECLARE_CHAIN(3); // Chain 4: Data Storage/Streaming
#if defined(SEMANTIC_EXECUTOR)
std::cerr << "Not yet implemented" << std::endl;
return -1;
#elif defined(EDF_PRIORITY_EXECUTOR)
// Create a priority executor with a custom memory strategy
auto strategy = std::make_shared<priority_executor::PriorityMemoryStrategy<>>();
rclcpp::ExecutorOptions options;
options.memory_strategy = strategy;
#ifdef MULTI_THREADED
auto executor = priority_executor::MultithreadTimedExecutor(options);
#else
auto executor = priority_executor::TimedExecutor(options);
#endif
// Must be set to post_execute can set new deadlines
executor.prio_memory_strategy_ = strategy;
#else // default: ROS
#ifdef MULTI_THREADED
// Create a multi-threaded executor to handle all nodes
rclcpp::executors::MultiThreadedExecutor executor; rclcpp::executors::MultiThreadedExecutor executor;
#else
// Create a single-threaded executor to handle all nodes
rclcpp::executors::SingleThreadedExecutor executor;
#endif
#endif
// Chain 1: Image2Ground Mapping // Chain 1: Image2Ground Mapping
auto camera = std::make_shared<CameraNode>("input_camera_node", "/input/camera/raw", DELAY_JITTER_PAIR(5)); // Sensor read, low latency MAKE_INPUT_NODE(0, cameraA, CameraNode, "/input/cameraA/raw", 200); // 60 FPS camera, fast
auto debayer = std::make_shared<DebayerNode>("debayer_node", "/input/camera/raw", "/camera/debayered", DELAY_JITTER_PAIR(18)); // Moderate, image conversion MAKE_PROCESSING_NODE(0, debayerA, DebayerNode, "/input/cameraA/raw", "/cameraA/debayered", 18); // Moderate, image conversion
auto radiometric = std::make_shared<RadiometricNode>("radiometric_node", "/camera/debayered", "/camera/radiometric", DELAY_JITTER_PAIR(22)); // Heavier image math MAKE_PROCESSING_NODE(0, radiometricA, RadiometricNode, "/cameraA/debayered", "/cameraA/radiometric", 22); // Heavier image math
auto geometric = std::make_shared<GeometricNode>("geometric_node", "/camera/radiometric", "/camera/geometric", DELAY_JITTER_PAIR(12)); // Perspective transform, medium MAKE_PROCESSING_NODE(0, geometric, GeometricNode, "/cameraA/radiometric", "/cameraA/geometric", 12); // Perspective transform, medium
auto mapping = std::make_shared<MappingNode>("output_mapping_node", "/camera/geometric", "/output/camera/mapped", DELAY_JITTER_PAIR(16)); // Warping/stitching, moderate MAKE_OUTPUT_NODE(0, mapping, MappingNode, "/cameraA/geometric", "/output/cameraA/mapped", 16); // Warping/stitching, moderate
// Chain 2: Semantic Scheduler Adaptation // Chain 2: Semantic Scheduler Adaptation
auto smoke = std::make_shared<SmokeClassifierNode>("output_smoke_classifier_node", "/camera/radiometric", "/output/classifier/classification", DELAY_JITTER_PAIR(35)); // CNN/ML classifier, can be high MAKE_INPUT_NODE(1, cameraB, CameraNode, "/input/cameraB/raw", 150); // 60 FPS camera, fast
MAKE_PROCESSING_NODE(1, debayerB, DebayerNode, "/input/cameraB/raw", "/cameraB/debayered", 18); // Moderate, image conversion
MAKE_PROCESSING_NODE(1, radiometricB, RadiometricNode, "/cameraB/debayered", "/cameraB/radiometric", 22); // Heavier image math
MAKE_OUTPUT_NODE(1, smoke_classifier, SmokeClassifierNode, "/cameraB/radiometric", "/output/classifier/classification", 35); // CNN/ML classifier, can be high
// Chain 3: Flight Control // Chain 3: Flight Control
auto gps = std::make_shared<GPSNode>("input_gps_node", "/input/gps/fix", DELAY_JITTER_PAIR(7)); // Fast, low latency sensor std::vector<std::string> fusionA_input_topics = {"/input/gpsA/fix", "/input/imuA/data", "/input/baroA/alt"};
auto imu = std::make_shared<IMUNode>("input_imu_node", "/input/imu/data", DELAY_JITTER_PAIR(5)); // Fast, low latency sensor MAKE_INPUT_NODE(2, gpsA, GPSNode, "/input/gpsA/fix", 100); // Fast, low latency sensor
auto baro = std::make_shared<BaroNode>("input_baro_node", "/input/baro/alt", DELAY_JITTER_PAIR(6)); // Fast, low latency sensor MAKE_INPUT_NODE(2, imuA, IMUNode, "/input/imuA/data", 100); // Fast, low latency sensor
auto fusion = std::make_shared<FusionNode>( MAKE_INPUT_NODE(2, baroA, BaroNode, "/input/baroA/alt", 100); // Fast, low latency sensor
"sensor_fusion_node", std::vector<std::string>{"/input/gps/fix", "/input/imu/data", "/input/baro/alt"}, "/sensors/fused", DELAY_JITTER_PAIR(14)); // Sensor fusion, not trivial but not heavy MAKE_PROCESSING_NODE(2, fusionA, FusionNode, fusionA_input_topics, "/sensorsA/fused", 14); // Sensor fusion, not trivial but not heavy
auto lidar = std::make_shared<LidarNode>("input_lidar_node", "/input/lidar/scan", DELAY_JITTER_PAIR(10)); // LIDAR driver, can be fast MAKE_INPUT_NODE(2, lidar, LidarNode, "/input/lidar/scan", 100); // LIDAR driver, can be fast
auto cmd = std::make_shared<CommandNode>("input_operator_cmd_node", "/input/operator/commands", DELAY_JITTER_PAIR(8)); // Operator commands, fast MAKE_INPUT_NODE(2, cmd, CommandNode, "/input/operator/commands", 100); // Operator commands, fast
auto mgmt = std::make_shared<FlightManagementNode>( std::vector<std::string> mgmt_input_topics = {"/sensorsA/fused", "/input/lidar/scan", "/input/operator/commands"};
"flight_mgmt_node", std::vector<std::string>{"/sensors/fused", "/input/lidar/scan", "/input/operator/commands"}, "/flight/plan", DELAY_JITTER_PAIR(18)); // Path planning, moderate MAKE_PROCESSING_NODE(2, mgmt, FlightManagementNode, mgmt_input_topics, "/flight/plan", 18); // Path planning, moderate
auto control = std::make_shared<ControlNode>("output_flight_control_node", "/flight/plan", "/output/flight/cmd", DELAY_JITTER_PAIR(12)); // PID controller, fast but non-zero //MAKE_PROCESSING_NODE(2, mgmt, FlightManagementNodeS, "/input/operator/commands", "/flight/plan", 18); // Path planning, moderate
MAKE_OUTPUT_NODE(2, control, ControlNode, "/flight/plan", "/output/flight/cmd", 12); // PID controller, fast but non-zero
// Chain 4: Data Storage/Streaming // Chain 4: Data Storage/Streaming
auto telemetry = std::make_shared<TelemetryNode>( std::vector<std::string> fusionB_input_topics = {"/input/gpsB/fix", "/input/imuB/data", "/input/baroB/alt"};
"telemetry_node", std::vector<std::string>{"/sensors/fused", "/output/camera/mapped"}, "/telemetry/data", DELAY_JITTER_PAIR(6)); // Quick serialization MAKE_INPUT_NODE(3, gpsB, GPSNode, "/input/gpsB/fix", 100); // Fast, low latency sensor
auto radio = std::make_shared<RadioNode>("output_radio_tx_node", "/telemetry/data", "/output/telemetry/radio", DELAY_JITTER_PAIR(3)); // Output, almost instant MAKE_INPUT_NODE(3, imuB, IMUNode, "/input/imuB/data", 100); // Fast, low latency sensor
MAKE_INPUT_NODE(3, baroB, BaroNode, "/input/baroB/alt", 100); // Fast, low latency sensor
MAKE_PROCESSING_NODE(3, fusionB, FusionNode, fusionB_input_topics, "/sensorsB/fused", 14); // Sensor fusion, not trivial but not heavy
//MAKE_PROCESSING_NODE(3, fusionB, FusionNodeS, "/input/baroB/alt", "/sensorsB/fused", 14); // Sensor fusion, not trivial but not heavy
// Add all to executor std::vector<std::string> telemetry_input_topics = {"/sensorsB/fused", "/output/cameraA/mapped"};
executor.add_node(camera); MAKE_PROCESSING_NODE(3, telemetry, TelemetryNode, telemetry_input_topics, "/telemetry/data", 6); // Quick serialization, low latency
executor.add_node(debayer); //MAKE_PROCESSING_NODE(3, telemetry, TelemetryNodeS, "/sensorsB/fused", "/telemetry/data", 6); // Quick serialization, low latency
executor.add_node(radiometric); MAKE_OUTPUT_NODE(3, radio, RadioNode, "/telemetry/data", "/output/telemetry/radio", 3); // Output, almost instant
#if defined(EDF_PRIORITY_EXECUTOR)
int chain_period;
int chain_id;
// ---
chain_id = 0;
chain_period = 1000;
{
// the new funcitons in PriorityMemoryStrategy accept the handle of the
// timer/subscription as the first argument
strategy->set_executable_deadline(cameraA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
// you _must_ set the timer_handle for each chain
strategy->get_priority_settings(cameraA->timer_->get_timer_handle())->timer_handle = cameraA->timer_;
// you _must_ mark the first executable in the chain
strategy->set_first_in_chain(cameraA->timer_->get_timer_handle());
// Set the executable deadlines for each node
strategy->set_executable_deadline(debayerA->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_executable_deadline(radiometricA->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_executable_deadline(geometric->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_executable_deadline(mapping->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
// you _must_ mark the last executable in the chain (used to keep track of different instances of the same chain)
strategy->set_last_in_chain(mapping->subscription_->get_subscription_handle());
}
// ---
chain_id = 1;
chain_period = 1000;
{
strategy->set_executable_deadline(cameraB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->get_priority_settings(cameraB->timer_->get_timer_handle())->timer_handle = cameraB->timer_;
strategy->set_first_in_chain(cameraB->timer_->get_timer_handle());
strategy->set_executable_deadline(debayerB->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_executable_deadline(radiometricB->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_executable_deadline(smoke_classifier->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_last_in_chain(smoke_classifier->subscription_->get_subscription_handle());
}
// ---
chain_id = 2;
chain_period = 1000;
{
strategy->set_executable_deadline(cmd->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->get_priority_settings(cmd->timer_->get_timer_handle())->timer_handle = cmd->timer_;
strategy->set_first_in_chain(cmd->timer_->get_timer_handle());
strategy->set_executable_deadline(baroA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->set_executable_deadline(imuA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->set_executable_deadline(gpsA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
REGISTER_MULTI_PROCESSING_NODE(fusionA, chain_period, chain_id);
strategy->set_executable_deadline(lidar->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
REGISTER_MULTI_PROCESSING_NODE(mgmt, chain_period, chain_id);
strategy->set_executable_deadline(control->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_last_in_chain(control->subscription_->get_subscription_handle());
}
// ---
chain_id = 3;
chain_period = 1000;
{
strategy->set_executable_deadline(baroB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->get_priority_settings(baroB->timer_->get_timer_handle())->timer_handle = baroB->timer_;
strategy->set_first_in_chain(baroB->timer_->get_timer_handle());
strategy->set_executable_deadline(imuB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->set_executable_deadline(gpsB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
REGISTER_MULTI_PROCESSING_NODE(fusionB, chain_period, chain_id);
REGISTER_MULTI_PROCESSING_NODE(telemetry, chain_period, chain_id);
strategy->set_executable_deadline(radio->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id);
strategy->set_last_in_chain(radio->subscription_->get_subscription_handle());
}
#endif
executor.add_node(cameraA);
executor.add_node(debayerA);
executor.add_node(radiometricA);
executor.add_node(geometric); executor.add_node(geometric);
executor.add_node(mapping); executor.add_node(mapping);
executor.add_node(smoke); executor.add_node(cameraB);
executor.add_node(debayerB);
executor.add_node(radiometricB);
executor.add_node(smoke_classifier);
executor.add_node(gps); executor.add_node(gpsA);
executor.add_node(imu); executor.add_node(imuA);
executor.add_node(baro); executor.add_node(baroA);
executor.add_node(fusion); executor.add_node(fusionA);
executor.add_node(gpsB);
executor.add_node(imuB);
executor.add_node(baroB);
executor.add_node(fusionB);
executor.add_node(telemetry); executor.add_node(telemetry);
executor.add_node(radio); executor.add_node(radio);
@ -239,3 +261,107 @@ int main(int argc, char **argv)
rclcpp::shutdown(); rclcpp::shutdown();
return 0; return 0;
} }
// -----------------------------------------------
// re-create the classic talker-listener example with two listeners
class Talker : public rclcpp::Node
{
public:
Talker() : Node("talker")
{
// Create a publisher on the "chatter" topic with 10 msg queue size.
pub_ = this->create_publisher<std_msgs::msg::String>("chatter", 10);
// Create a timer of period 1s that calls our callback member function.
timer_ = this->create_wall_timer(std::chrono::seconds(1),
std::bind(&Talker::timer_callback, this));
}
// the timer must be public
rclcpp::TimerBase::SharedPtr timer_;
private:
void timer_callback()
{
std_msgs::msg::String msg;
msg.data = "Hello World!";
RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", msg.data.c_str());
pub_->publish(msg);
}
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
};
class Listener : public rclcpp::Node
{
public:
Listener(std::string name) : Node(name)
{
// Create a subscription on the "chatter" topic with the default callback
// method.
pub_ = this->create_publisher<std_msgs::msg::String>("/out", 10);
sub_ = this->create_subscription<std_msgs::msg::String>(
"chatter", 10,
std::bind(&Listener::callback, this, std::placeholders::_1));
}
// the publisher must be public
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr sub_;
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr pub_;
private:
void callback(const std_msgs::msg::String::SharedPtr msg)
{
RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str());
std_msgs::msg::String msgn;
msgn.data = "Hello World!";
RCLCPP_INFO(this->get_logger(), "Publishing: 'I heard: %s'", msgn.data.c_str());
pub_->publish(msgn);
}
};
int edf_usage_example(int argc, char **argv) {
rclcpp::init(argc, argv);
auto talker = std::make_shared<Talker>();
auto listener1 = std::make_shared<Listener>("listener1");
//auto listener2 = std::make_shared<Listener>("listener2");
rclcpp::ExecutorOptions options;
auto strategy = std::make_shared<priority_executor::PriorityMemoryStrategy<>>();
options.memory_strategy = strategy;
auto executor = new priority_executor::TimedExecutor(options);
// must be set to post_execute can set new deadlines
executor->prio_memory_strategy_ = strategy;
// the new funcitons in PriorityMemoryStrategy accept the handle of the
// timer/subscription as the first argument
strategy->set_executable_deadline(talker->timer_->get_timer_handle(), 1000,
priority_executor::ExecutableType::TIMER, 0);
// you _must_ set the timer_handle for each chain
strategy->get_priority_settings(talker->timer_->get_timer_handle())->timer_handle = talker->timer_;
// you _must_ mark the first executable in the chain
strategy->set_first_in_chain(talker->timer_->get_timer_handle());
// set the same period and chain_id for each callback in the chain
strategy->set_executable_deadline(listener1->sub_->get_subscription_handle(),
1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
//strategy->set_executable_deadline(listener2->sub_->get_subscription_handle(), 1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
// you _must_ mark the last executable in the chain (used to keep track of different instances of the same chain)
strategy->set_last_in_chain(listener1->sub_->get_subscription_handle());
// add all the nodes to the executor
executor->add_node(talker);
executor->add_node(listener1);
//executor->add_node(listener2);
// if the executor behaves unexpectedly, you can print the priority settings to make sure they are correct
std::cout << *strategy->get_priority_settings(
talker->timer_->get_timer_handle())
<< std::endl;
std::cout << *strategy->get_priority_settings(
listener1->sub_->get_subscription_handle())
<< std::endl;
//std::cout << *strategy->get_priority_settings(listener2->sub_->get_subscription_handle()) << std::endl;
executor->spin();
rclcpp::shutdown();
return 0;
}

203
src/full_topology/src/nodes.hpp Normal file
View file

@ -0,0 +1,203 @@
#pragma once
#include <chrono>
#include <memory>
#include <random>
#include <string>
#include <thread>
#include <vector>
using namespace std::chrono_literals;
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
// A generic sensor node that publishes at a given rate + jitter
#define DEFINE_SENSOR_NODE_CLASS(NAME) \
class NAME : public rclcpp::Node \
{ \
public: \
NAME(std::string const &name, \
std::string const &topic, \
int period_ms, \
double jitter_ms_ = 0.0) \
: Node(name) \
{ \
publisher_ = create_publisher<std_msgs::msg::String>(topic, 10); \
timer_ = create_wall_timer(std::chrono::milliseconds(period_ms), [this, name, jitter_ms_]() \
{ \
double jitter = 0.0; \
if (jitter_ms_ > 0.0) { \
static thread_local std::mt19937 gen(std::random_device{}()); \
std::normal_distribution<> d(0.0, jitter_ms_); \
jitter = d(gen); \
} \
/* Ensure total delay is non-negative */ \
int total_delay = std::max(0, static_cast<int>(std::round(jitter))); \
std::this_thread::sleep_for(std::chrono::milliseconds(total_delay)); \
auto msg = std_msgs::msg::String(); \
msg.data = name + std::string(" publishes"); \
publisher_->publish(msg); }); \
} \
\
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; \
rclcpp::TimerBase::SharedPtr timer_; \
};
// A generic single-input processing node with a fixed delay +/- jitter
#define DEFINE_PROCESSING_NODE_CLASS(NAME) \
class NAME : public rclcpp::Node \
{ \
public: \
NAME(const std::string & name, \
const std::string & input_topic, \
const std::string & output_topic, \
int delay_ms, \
double jitter_ms = 0.0) \
: Node(name), delay_ms_(delay_ms), jitter_ms_(jitter_ms) \
{ \
subscription_ = create_subscription<std_msgs::msg::String>( \
input_topic, 10, \
[this, name](std_msgs::msg::String::SharedPtr msg) \
{ \
/* Simulate processing delay */ \
double jitter = 0.0; \
if (jitter_ms_ > 0.0) { \
static thread_local std::mt19937 gen(std::random_device{}());\
std::normal_distribution<> d(0.0, jitter_ms_); \
jitter = d(gen); \
} \
std::this_thread::sleep_for(std::chrono::milliseconds((int)(delay_ms_ + jitter))); \
auto out = std_msgs::msg::String(); \
out.data = msg->data + " -> " + name; \
publisher_->publish(out); \
}); \
publisher_ = create_publisher<std_msgs::msg::String>(output_topic, 10); \
} \
\
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr subscription_; \
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; \
protected: \
int delay_ms_; \
double jitter_ms_; \
};
// A generic single-input processing node with a fixed delay +/- jitter
#ifdef USE_TIMER_IN_FUSION_NODES
#define DEFINE_MULTI_PROCESSING_NODE_CLASS(NAME) \
class NAME : public rclcpp::Node \
{ \
public: \
NAME(std::string const &name, \
std::vector<std::string> const &input_topics, \
std::string const &output_topic, \
int delay_ms, \
double jitter_ms = 0.0) \
: Node(name), delay_ms_(delay_ms), jitter_ms_(jitter_ms) \
{ \
for (auto &topic : input_topics) \
{ \
auto sub = create_subscription<std_msgs::msg::String>( \
topic, 10, \
[this, name](std_msgs::msg::String::SharedPtr msg) \
{ \
}); \
subscriptions_.emplace_back(sub); \
} \
publisher_ = create_publisher<std_msgs::msg::String>(output_topic, 10); \
/* Also publish fused result periodically */ \
timer_ = create_wall_timer(std::chrono::milliseconds(delay_ms_), [this, name]() \
{ \
/* Simulate processing delay */ \
double jitter = 0.0; \
if (jitter_ms_ > 0.0) { \
static thread_local std::mt19937 gen(std::random_device{}()); \
std::normal_distribution<> d(0.0, jitter_ms_); \
jitter = d(gen); \
} \
std::this_thread::sleep_for(std::chrono::milliseconds((int)jitter)); \
auto out = std_msgs::msg::String(); \
out.data = name + std::string(" fused"); \
publisher_->publish(out); \
}); \
} \
\
std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> subscriptions_; \
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; \
rclcpp::TimerBase::SharedPtr timer_; \
protected: \
int delay_ms_; \
double jitter_ms_; \
};
#else
#define DEFINE_MULTI_PROCESSING_NODE_CLASS(NAME) \
class NAME : public rclcpp::Node \
{ \
public: \
NAME(std::string const &name, \
std::vector<std::string> const &input_topics, \
std::string const &output_topic, \
int delay_ms, \
double jitter_ms = 0.0) \
: Node(name), delay_ms_(delay_ms), jitter_ms_(jitter_ms) \
{ \
for (auto &topic : input_topics) \
{ \
auto sub = create_subscription<std_msgs::msg::String>( \
topic, 10, \
[this, name](std_msgs::msg::String::SharedPtr msg) \
{ \
/* Simulate processing delay */ \
double jitter = 0.0; \
if (jitter_ms_ > 0.0) { \
static thread_local std::mt19937 gen(std::random_device{}()); \
std::normal_distribution<> d(0.0, jitter_ms_); \
jitter = d(gen); \
} \
std::this_thread::sleep_for(std::chrono::milliseconds((int)(delay_ms_ + jitter))); \
auto out = std_msgs::msg::String(); \
out.data = msg->data + " -> " + name; \
publisher_->publish(out); \
}); \
subscriptions_.emplace_back(sub); \
} \
publisher_ = create_publisher<std_msgs::msg::String>(output_topic, 10); \
\
} \
\
std::vector<rclcpp::Subscription<std_msgs::msg::String>::SharedPtr> subscriptions_; \
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr publisher_; \
protected: \
int delay_ms_; \
double jitter_ms_; \
};
#endif
DEFINE_SENSOR_NODE_CLASS(CameraNode)
DEFINE_PROCESSING_NODE_CLASS(DebayerNode)
DEFINE_PROCESSING_NODE_CLASS(RadiometricNode)
DEFINE_PROCESSING_NODE_CLASS(GeometricNode)
DEFINE_PROCESSING_NODE_CLASS(MappingNode)
DEFINE_PROCESSING_NODE_CLASS(SmokeClassifierNode)
DEFINE_SENSOR_NODE_CLASS(GPSNode)
DEFINE_SENSOR_NODE_CLASS(IMUNode)
DEFINE_SENSOR_NODE_CLASS(BaroNode)
DEFINE_PROCESSING_NODE_CLASS(FusionNodeS)
DEFINE_MULTI_PROCESSING_NODE_CLASS(FusionNode)
DEFINE_SENSOR_NODE_CLASS(LidarNode)
DEFINE_SENSOR_NODE_CLASS(CommandNode)
DEFINE_PROCESSING_NODE_CLASS(FlightManagementNodeS)
DEFINE_MULTI_PROCESSING_NODE_CLASS(FlightManagementNode)
DEFINE_PROCESSING_NODE_CLASS(ControlNode)
DEFINE_PROCESSING_NODE_CLASS(TelemetryNodeS)
DEFINE_MULTI_PROCESSING_NODE_CLASS(TelemetryNode)
DEFINE_PROCESSING_NODE_CLASS(RadioNode)

125
src/full_topology/src/simple.cpp Normal file
View file

@ -0,0 +1,125 @@
// full_topology.cpp
#include <chrono>
#include <memory>
#include <random>
#include <string>
#include <thread>
#include <vector>
// the order here matters
#include "tracetools/utils.hpp"
#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/string.hpp"
#include "config.hpp"
#include "nodes.hpp"
#include "priority_executor/priority_executor.hpp"
#include "priority_executor/multithread_priority_executor.hpp"
#include "priority_executor/priority_memory_strategy.hpp"
#define PROCENTUAL_INPUT_JITTER 0.05
#define PROCENTUAL_PROCESSING_JITTER 0.1
#define STR_HELPER(x) #x
#define STR(x) STR_HELPER(x)
#ifndef DEBUG
#define PRINT_BEFORE(CHAIN_ID, NAME) \
std::cout << "Chain " << CHAIN_ID << " before: " << CHAIN_DURATION(CHAIN_ID) << " ms" << std::endl;
#define PRINT_AFTER(CHAIN_ID, NAME) \
std::cout << "Chain " << CHAIN_ID << " after: " << CHAIN_DURATION(CHAIN_ID) << " ms" << std::endl;
#else
#define PRINT_BEFORE(CHAIN_ID, NAME)
#define PRINT_AFTER(CHAIN_ID, NAME)
#endif
#define CHAIN_DURATION(CHAIN_ID) chain_duration_##CHAIN_ID
#define DECLARE_CHAIN(CHAIN_ID) double CHAIN_DURATION(CHAIN_ID) = 0.0;
#define SUM_CHAIN_DURATION(CHAIN_ID, NAME) \
PRINT_BEFORE(CHAIN_ID, NAME); \
CHAIN_DURATION(CHAIN_ID) += NAME##_duration + NAME##_jitter; \
PRINT_AFTER(CHAIN_ID, NAME);
#define MAKE_INPUT_NODE(CHAIN_ID, NAME, CLASS, TOPIC, PERIOD) \
auto NAME##_duration = (PERIOD); \
auto NAME##_jitter = PROCENTUAL_INPUT_JITTER * (PERIOD); \
auto NAME = std::make_shared<CLASS>(std::string("input_" STR(NAME) "_node"), TOPIC, NAME##_duration, NAME##_jitter); \
SUM_CHAIN_DURATION(CHAIN_ID, NAME)
#define MAKE_PROCESSING_NODE(CHAIN_ID, NAME, CLASS, INPUT_TOPIC, OUTPUT_TOPIC, DURATION) \
auto NAME##_duration = (DURATION); \
auto NAME##_jitter = PROCENTUAL_PROCESSING_JITTER * (DURATION); \
auto NAME = std::make_shared<CLASS>(std::string(STR(NAME) "_node"), INPUT_TOPIC, OUTPUT_TOPIC, NAME##_duration, NAME##_jitter); \
SUM_CHAIN_DURATION(CHAIN_ID, NAME)
#define MAKE_OUTPUT_NODE(CHAIN_ID, NAME, CLASS, INPUT_TOPIC, OUTPUT_TOPIC, DURATION) \
auto NAME##_duration = (DURATION); \
auto NAME##_jitter = PROCENTUAL_PROCESSING_JITTER * (DURATION); \
auto NAME = std::make_shared<CLASS>(std::string("output_" STR(NAME) "_node"), INPUT_TOPIC, OUTPUT_TOPIC, NAME##_duration, NAME##_jitter); \
SUM_CHAIN_DURATION(CHAIN_ID, NAME)
int main(int argc, char **argv)
{
DECLARE_CHAIN(0); // Chain 1: Image2Ground Mapping
rclcpp::init(argc, argv);
// Create a priority executor with a custom memory strategy
auto strategy = std::make_shared<priority_executor::PriorityMemoryStrategy<>>();
rclcpp::ExecutorOptions options;
options.memory_strategy = strategy;
auto executor = priority_executor::MultithreadTimedExecutor(options);
// Must be set to post_execute can set new deadlines
executor.prio_memory_strategy_ = strategy;
// chain id, node name, node class, input topic, output topic, duration in ms
MAKE_INPUT_NODE( 0, cameraA , CameraNode , /*has no input topic*/ "/input/cameraA/raw" , 1000.0/2.0); // 60 FPS camera, fast
MAKE_PROCESSING_NODE(0, debayerA , DebayerNode , "/input/cameraA/raw" , "/cameraA/debayered" , 18); // Moderate, image conversion
MAKE_PROCESSING_NODE(0, radiometricA, RadiometricNode, "/cameraA/debayered" , "/cameraA/radiometric" , 22); // Heavier image math
MAKE_PROCESSING_NODE(0, geometric , GeometricNode , "/cameraA/radiometric", "/cameraA/geometric" , 12); // Perspective transform, medium
MAKE_OUTPUT_NODE( 0, mapping , MappingNode , "/cameraA/geometric" , "/output/cameraA/mapped", 16); // Warping/stitching, moderate
// the new funcitons in PriorityMemoryStrategy accept the handle of the
// timer/subscription as the first argument
strategy->set_executable_deadline(cameraA->timer_->get_timer_handle() , 1000, priority_executor::ExecutableType::TIMER , 0);
// you _must_ set the timer_handle for each chain
strategy->get_priority_settings(cameraA->timer_->get_timer_handle())->timer_handle = cameraA->timer_;
// you _must_ mark the first executable in the chain
strategy->set_first_in_chain(cameraA->timer_->get_timer_handle());
// Set the executable deadlines for each node
strategy->set_executable_deadline(debayerA->subscription_->get_subscription_handle() , 1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
strategy->set_executable_deadline(radiometricA->subscription_->get_subscription_handle(), 1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
strategy->set_executable_deadline(geometric->subscription_->get_subscription_handle() , 1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
strategy->set_executable_deadline(mapping->subscription_->get_subscription_handle() , 1000, priority_executor::ExecutableType::SUBSCRIPTION, 0);
// you _must_ mark the last executable in the chain (used to keep track of different instances of the same chain)
strategy->set_last_in_chain(mapping->subscription_->get_subscription_handle());
executor.add_node(cameraA);
executor.add_node(debayerA);
executor.add_node(radiometricA);
executor.add_node(geometric);
executor.add_node(mapping);
std::cout << *strategy->get_priority_settings(cameraA->timer_->get_timer_handle())
<< std::endl;
std::cout << *strategy->get_priority_settings(debayerA->subscription_->get_subscription_handle())
<< std::endl;
std::cout << *strategy->get_priority_settings(radiometricA->subscription_->get_subscription_handle())
<< std::endl;
std::cout << *strategy->get_priority_settings(geometric->subscription_->get_subscription_handle())
<< std::endl;
std::cout << *strategy->get_priority_settings(mapping->subscription_->get_subscription_handle())
<< std::endl;
executor.spin();
rclcpp::shutdown();
return 0;
}

2
src/ros_edf

@ -1 +1 @@
Subproject commit c44b7b8c3c527a8d16739349b50340a7de9b6f51 Subproject commit 5f73198adbabb3a34985fb5e5c7f84b5e4149a86