From 7c00d4514c630312d022657c7e0b809f84dd9752 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Thu, 3 Sep 2026 15:41:44 +0200 Subject: [PATCH] run_dls2: changes towards robust and slow startup --- .../core_framework/src/console_layer.cpp.in | 4 +- modules/main/include/dls2/main/run_dls2.hpp | 4 +- modules/main/src/run_dls2.cpp.in | 142 +++++++++++++----- .../state_machine/state_machine_watcher.hpp | 8 +- .../src/state_machine_watcher.cpp | 16 +- 5 files changed, 123 insertions(+), 51 deletions(-) diff --git a/modules/core_framework/src/console_layer.cpp.in b/modules/core_framework/src/console_layer.cpp.in index ef125ff7..b5757176 100644 --- a/modules/core_framework/src/console_layer.cpp.in +++ b/modules/core_framework/src/console_layer.cpp.in @@ -229,7 +229,7 @@ namespace dls } // namespace readline_completion // dls::ConsoleLayer implementation - ConsoleLayer::ConsoleLayer(std::string ID) : Layer(ID, 50), + ConsoleLayer::ConsoleLayer(std::string ID) : Layer(ID, 1000), load_layers_paths_{{"loadController", "${DLS_INSTALL_CONTROLLER_DIR}"}, {"loadGenerator", "${DLS_INSTALL_MOTION_GENERATOR_DIR}"}, {"loadEstimator", "${DLS_INSTALL_ESTIMATOR_DIR}"}, @@ -440,4 +440,4 @@ namespace dls free(line); } } -} // namespace dls \ No newline at end of file +} // namespace dls diff --git a/modules/main/include/dls2/main/run_dls2.hpp b/modules/main/include/dls2/main/run_dls2.hpp index 57f254fe..6ca8ad29 100644 --- a/modules/main/include/dls2/main/run_dls2.hpp +++ b/modules/main/include/dls2/main/run_dls2.hpp @@ -19,8 +19,8 @@ namespace dls private: void change_process_name(char **argv, const std::string &name); void launchSupervisor(); - void launchLayers(); - void runStartup(const std::string &); + bool launchLayers(); + bool runStartup(const std::string &); bool runLayer(const std::string &, const std::string &); void launchServers(); void launchSingleServer(const std::string& ip, int port); diff --git a/modules/main/src/run_dls2.cpp.in b/modules/main/src/run_dls2.cpp.in index bc61fdc2..bdac23a9 100644 --- a/modules/main/src/run_dls2.cpp.in +++ b/modules/main/src/run_dls2.cpp.in @@ -2,12 +2,18 @@ #include "dls2/core_framework/options.hpp" #include +#include +#include #include #include #include namespace dls { + constexpr int STARTUP_DISCOVERY_TIMEOUT_MS = 60000; + constexpr int STARTUP_STATE_TIMEOUT_MS = 60000; + constexpr int STARTUP_RETRY_INTERVAL_MS = 1000; + CommandManager RunDLS2::command_manager("dls_framework"); bool RunDLS2::should_quit(false); @@ -86,7 +92,8 @@ namespace dls // Run startup procedure if requested by the user if (Options::run_startup) { - runStartup(Options::startup_file); + if (!runStartup(Options::startup_file)) + should_quit = true; } else { // the user is selecting what to run @@ -97,10 +104,12 @@ namespace dls // Spawn supervisor (if passed as argument) if (Options::launch_supervisor) { - runLayer("supervisor", "Supervisor"); + if (!runLayer("supervisor", "Supervisor")) + should_quit = true; } // Spawn layers (if passed as argument) - launchLayers(); + if (!launchLayers()) + should_quit = true; } // Hanging on this executable to intercept CTRL+C. If no layers are running, the executable will exit @@ -163,26 +172,52 @@ namespace dls pData->proc->detach(); layers.emplace(pData->getID(), pData); - // activate layer - sm_watcher.waitState(pData->getID(), "idle", should_quit); - if(command_manager.waitCommand(pData->getID(), "activate", should_quit)) - command_manager.callCommand("activate", {}, pData->getID()); - sm_watcher.waitState(pData->getID(), "run", should_quit); - return true; + auto waitForState = [&](const std::string& state) { + for (int elapsed_ms = 0; elapsed_ms < STARTUP_STATE_TIMEOUT_MS && !should_quit; + elapsed_ms += 5000) + { + if (sm_watcher.waitState(pData->getID(), state, should_quit, false)) + return true; + } + std::cerr << "Layer " << pData->getID() << " did not reach " << state + << " within " << STARTUP_STATE_TIMEOUT_MS / 1000 << " seconds" << std::endl; + return false; + }; + + if (!waitForState("idle")) + return false; + + for (int elapsed_ms = 0; elapsed_ms < STARTUP_DISCOVERY_TIMEOUT_MS && !should_quit; + elapsed_ms += STARTUP_RETRY_INTERVAL_MS) + { + if (command_manager.find(pData->getID(), "activate").size() != 1) + { + std::this_thread::sleep_for(std::chrono::milliseconds(STARTUP_RETRY_INTERVAL_MS)); + continue; + } + if (command_manager.callCommand("activate", {}, pData->getID()) == 1) + return waitForState("run"); + } + + std::cerr << "Layer " << pData->getID() + << " did not expose a unique activate command within " + << STARTUP_DISCOVERY_TIMEOUT_MS / 1000 << " seconds" << std::endl; + return false; } - void RunDLS2::launchLayers() + bool RunDLS2::launchLayers() { if (Options::launch_log && !should_quit) - runLayer("log", "LogLayer"); + if (!runLayer("log", "LogLayer")) return false; if (Options::launch_hardware && !should_quit) - runLayer("hardware", "HardwareLayer"); + if (!runLayer("hardware", "HardwareLayer")) return false; if (Options::launch_control && !should_quit) - runLayer("control", "ControlLayer"); + if (!runLayer("control", "ControlLayer")) return false; if (Options::launch_console && !should_quit) - runLayer("console", "ConsoleLayer"); + if (!runLayer("console", "ConsoleLayer")) return false; if (Options::launch_estimation && !should_quit) - runLayer("estimation", Options::estimation_layer_name); + if (!runLayer("estimation", Options::estimation_layer_name)) return false; + return !should_quit; } void RunDLS2::launchServers() @@ -205,7 +240,7 @@ namespace dls } } - void RunDLS2::runStartup(const std::string &startup_file) + bool RunDLS2::runStartup(const std::string &startup_file) { YAML::Node config = YAML::LoadFile(startup_file); @@ -256,7 +291,8 @@ namespace dls launchServers(); // launch supervisor - runLayer("supervisor", "Supervisor"); + if (!runLayer("supervisor", "Supervisor")) + return false; // launch layers for (auto l : layers) { @@ -271,27 +307,64 @@ namespace dls if (l == "estimation") Options::launch_estimation = true; } - launchLayers(); + if (!launchLayers()) + return false; - // launch hardware first so dependent apps do not race missing inputs - for (auto hardware : applications["hardwares"]) - { - command_manager.callCommand(app_to_loading_command["hardwares"], {hardware}, app_to_layer["hardwares"]); - sm_watcher.waitState(hardware, "idle", should_quit); + auto waitForState = [&](const std::string& app, const std::string& state) { + for (int elapsed_ms = 0; elapsed_ms < STARTUP_STATE_TIMEOUT_MS && !should_quit; + elapsed_ms += 5000) + { + if (sm_watcher.waitState(app, state, should_quit, false)) + return true; + } + std::cerr << "Startup failed: " << app << " did not reach " << state + << " within " << STARTUP_STATE_TIMEOUT_MS / 1000 << " seconds" << std::endl; + return false; + }; - if(std::find(active_apps.begin(), active_apps.end(), hardware) != active_apps.end()) + auto callRequiredCommand = [&](const std::string& owner, const std::string& command, + const std::vector& args) { + for (int elapsed_ms = 0; elapsed_ms < STARTUP_DISCOVERY_TIMEOUT_MS && !should_quit; + elapsed_ms += STARTUP_RETRY_INTERVAL_MS) { - if(command_manager.waitCommand(hardware, "activate", should_quit, 10000)) + if (command_manager.find(owner, command).size() != 1) { - command_manager.callCommand("activate", {}, hardware); - sm_watcher.waitState(hardware, "run", should_quit); + std::this_thread::sleep_for(std::chrono::milliseconds(STARTUP_RETRY_INTERVAL_MS)); + continue; } + + const int matches = command_manager.callCommand(command, args, owner); + if (matches == 1) + return true; } + std::cerr << "Startup failed: command " << owner << "::" << command + << " was not available within " << STARTUP_DISCOVERY_TIMEOUT_MS / 1000 + << " seconds" << std::endl; + return false; + }; + + auto startApplication = [&](const std::string& name, const std::string& load_command, + const std::string& layer) { + if (!callRequiredCommand(layer, load_command, {name}) || !waitForState(name, "idle")) + return false; + + if (std::find(active_apps.begin(), active_apps.end(), name) == active_apps.end()) + return true; + + return callRequiredCommand(name, "activate", {}) && waitForState(name, "run"); + }; + + // launch hardware first so dependent apps do not race missing inputs + for (auto hardware : applications["hardwares"]) + { + if (!startApplication(hardware, app_to_loading_command["hardwares"], app_to_layer["hardwares"])) + return false; // loadModel does not have effect on the real robot because the HAL is directly loaded there. sleep(1); // if the model is spawned too fast (in gazebo) the simulation breaks - if(command_manager.waitCommand(hardware, "loadModel", should_quit, 10000)) - command_manager.callCommand("loadModel", {Options::robot_name, std::to_string(Options::robot_spawning_height)}, hardware); + if (!callRequiredCommand(hardware, "loadModel", + {Options::robot_name, std::to_string(Options::robot_spawning_height)})) + return false; } // launch apps @@ -302,15 +375,10 @@ namespace dls for (auto name : app_names) { - command_manager.callCommand(app_to_loading_command[app_type], {name}, app_to_layer[app_type]); - sm_watcher.waitState(name, "idle", should_quit); - if(std::find(active_apps.begin(), active_apps.end(), name) != active_apps.end()) - { - // Python apps can reach IDLE before DDS command discovery catches up. - if(command_manager.waitCommand(name, "activate", should_quit, 10000)) - command_manager.callCommand("activate", {}, name); - } + if (!startApplication(name, app_to_loading_command[app_type], app_to_layer[app_type])) + return false; } } + return true; } } \ No newline at end of file diff --git a/modules/state_machine/include/dls2/state_machine/state_machine_watcher.hpp b/modules/state_machine/include/dls2/state_machine/state_machine_watcher.hpp index c833cfbb..89b5de24 100644 --- a/modules/state_machine/include/dls2/state_machine/state_machine_watcher.hpp +++ b/modules/state_machine/include/dls2/state_machine/state_machine_watcher.hpp @@ -19,12 +19,14 @@ namespace state_machine /*! @brief Wait the state of an application until the stop_wait variable becomes true or the state is found */ - bool waitState(const std::string &app_name, const std::string &state, bool& stop_wait) const; + bool waitState(const std::string &app_name, const std::string &state, bool& stop_wait, + bool log_timeout = true) const; /*! @brief Wait the state of an application until the stop_wait variable becomes true or the state is found * @details Using atomic_bool instead of bool */ - bool waitState(const std::string &app_name, const std::string &state, std::atomic_bool& stop_wait) const; + bool waitState(const std::string &app_name, const std::string &state, std::atomic_bool& stop_wait, + bool log_timeout = true) const; /*! @brief Wait the state of an application until the stop_wait variable becomes true or the state is found */ @@ -46,4 +48,4 @@ namespace state_machine }; } -#endif /* end of include guard: STATE_MACHINE_WATCHER_HPP */ \ No newline at end of file +#endif /* end of include guard: STATE_MACHINE_WATCHER_HPP */ diff --git a/modules/state_machine/src/state_machine_watcher.cpp b/modules/state_machine/src/state_machine_watcher.cpp index 695aaf08..533f0a2e 100644 --- a/modules/state_machine/src/state_machine_watcher.cpp +++ b/modules/state_machine/src/state_machine_watcher.cpp @@ -37,7 +37,8 @@ namespace state_machine } StateMachineWatcher::~StateMachineWatcher() {} - bool StateMachineWatcher::waitState(const std::string &app_name, const std::string &state, bool& stop_wait) const + bool StateMachineWatcher::waitState(const std::string &app_name, const std::string &state, bool& stop_wait, + bool log_timeout) const { // wait app if(!dls::utils::wait(std::function([&](){ @@ -46,7 +47,7 @@ namespace state_machine } return true; }), 5000, 2, stop_wait)){ - if(!stop_wait){ + if(!stop_wait && log_timeout){ std::cerr << app_name << " not found" << std::endl; return false; } @@ -59,7 +60,7 @@ namespace state_machine } return true; }), 5000, 2, stop_wait)){ - if(!stop_wait){ + if(!stop_wait && log_timeout){ std::cerr << app_name << " not found in state " << state << std::endl; return false;} } @@ -68,7 +69,8 @@ namespace state_machine } - bool StateMachineWatcher::waitState(const std::string &app_name, const std::string &state, std::atomic_bool& stop_wait) const + bool StateMachineWatcher::waitState(const std::string &app_name, const std::string &state, + std::atomic_bool& stop_wait, bool log_timeout) const { // wait app if(!dls::utils::wait(std::function([&](){ @@ -77,7 +79,7 @@ namespace state_machine } return true; }), 5000, 2, stop_wait)){ - if(!stop_wait.load()){ + if(!stop_wait.load() && log_timeout){ std::cerr << app_name << " not found" << std::endl; return false;} } @@ -89,7 +91,7 @@ namespace state_machine } return true; }), 5000, 2, stop_wait)){ - if(!stop_wait.load()){ + if(!stop_wait.load() && log_timeout){ std::cerr << app_name << " not found in state " << state << std::endl; return false;} } @@ -146,4 +148,4 @@ namespace state_machine return false; return true; } -} \ No newline at end of file +}