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

added name to strategy registration

Browse source
This commit is contained in:
Niklas Halle 2025-06-23 08:30:37 +02:00
parent 23c3cd5ceb
commit 95177d5524
1 changed files with 27 additions and 22 deletions
Download patch file Download diff file Expand all files Collapse all files

49
src/full_topology/src/full_topology.cpp
View file

@ -65,13 +65,13 @@
#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); \
strategy->set_executable_deadline(sub->get_subscription_handle(), CHAIN_PERIOD, priority_executor::ExecutableType::SUBSCRIPTION, CHAIN_ID, STR(NAME)); \
} \
strategy->set_executable_deadline(NAME->timer_->get_timer_handle(), CHAIN_PERIOD, priority_executor::ExecutableType::TIMER, CHAIN_ID);
strategy->set_executable_deadline(NAME->timer_->get_timer_handle(), CHAIN_PERIOD, priority_executor::ExecutableType::TIMER, CHAIN_ID, STR(NAME));
#else
#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); \
strategy->set_executable_deadline(sub->get_subscription_handle(), CHAIN_PERIOD, priority_executor::ExecutableType::SUBSCRIPTION, CHAIN_ID, STR(NAME)); \
}
#endif
@ -164,33 +164,36 @@ int main(int argc, char **argv)
{
// 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);
strategy->set_executable_deadline(cameraA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "cameraA");
// 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);
strategy->set_executable_deadline(debayerA->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "debayerA");
strategy->set_executable_deadline(radiometricA->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "radiometricA");
strategy->set_executable_deadline(geometric->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "geometric");
strategy->set_executable_deadline(mapping->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "mapping");
// 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());
strategy->get_priority_settings(mapping->subscription_->get_subscription_handle())->timer_handle = cameraA->timer_;
}
// ---
chain_id = 1;
chain_period = BOOSTED;
{
strategy->set_executable_deadline(cameraB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
std::cout << "chain " << chain_id << " chain_period: " << chain_period << std::endl;
strategy->set_executable_deadline(cameraB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "cameraB");
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_executable_deadline(debayerB->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "debayerB");
strategy->set_executable_deadline(radiometricB->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "radiometricB");
strategy->set_executable_deadline(smoke_classifier->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "smoke_classifier");
strategy->set_last_in_chain(smoke_classifier->subscription_->get_subscription_handle());
strategy->get_priority_settings(smoke_classifier->subscription_->get_subscription_handle())->timer_handle = cameraB->timer_;
#if defined(SEMANTIC_EXECUTOR)
smoke_classifier->chain_id_ = chain_id;
smoke_classifier->strategy_ = strategy;
@ -206,32 +209,34 @@ int main(int argc, char **argv)
chain_id = 2;
chain_period = 1000;
{
strategy->set_executable_deadline(cmd->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->set_executable_deadline(cmd->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "cmd");
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);
strategy->set_executable_deadline(baroA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "baroA");
strategy->set_executable_deadline(imuA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "imuA");
strategy->set_executable_deadline(gpsA->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "gpsA");
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);
strategy->set_executable_deadline(lidar->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "lidar");
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_executable_deadline(control->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "control");
strategy->set_last_in_chain(control->subscription_->get_subscription_handle());
strategy->get_priority_settings(control->subscription_->get_subscription_handle())->timer_handle = cmd->timer_;
}
// ---
chain_id = 3;
chain_period = 1000;
{
strategy->set_executable_deadline(baroB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id);
strategy->set_executable_deadline(baroB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "baroB");
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);
strategy->set_executable_deadline(imuB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "imuB");
strategy->set_executable_deadline(gpsB->timer_->get_timer_handle(), chain_period, priority_executor::ExecutableType::TIMER, chain_id, "gpsB");
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_executable_deadline(radio->subscription_->get_subscription_handle(), chain_period, priority_executor::ExecutableType::SUBSCRIPTION, chain_id, "radio");
strategy->set_last_in_chain(radio->subscription_->get_subscription_handle());
strategy->get_priority_settings(radio->subscription_->get_subscription_handle())->timer_handle = baroB->timer_;
}
#endif