Skip to content

Robustify dls2 startup #331

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
mich-pest wants to merge 1 commit into
base: main
Choose a base branch
from
+123 −51
Open

Robustify dls2 startup #331

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 2 additions & 2 deletions modules/core_framework/src/console_layer.cpp.in
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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}"},
Expand Down Expand Up @@ -440,4 +440,4 @@ namespace dls
free(line);
}
}
} // namespace dls
} // namespace dls
4 changes: 2 additions & 2 deletions modules/main/include/dls2/main/run_dls2.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
142 changes: 105 additions & 37 deletions modules/main/src/run_dls2.cpp.in
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,18 @@
#include "dls2/core_framework/options.hpp"

#include <filesystem>
#include <chrono>
#include <thread>
#include <sys/prctl.h>
#include <sys/types.h>
#include <sys/stat.h>

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);

Expand Down Expand Up @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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()
Expand All @@ -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);

Expand Down Expand Up @@ -256,7 +291,8 @@ namespace dls
launchServers();

// launch supervisor
runLayer("supervisor", "Supervisor");
if (!runLayer("supervisor", "Supervisor"))
return false;
// launch layers
for (auto l : layers)
{
Expand All @@ -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<std::string>& 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
Expand All @@ -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;
}
}
8 changes: 5 additions & 3 deletions modules/state_machine/include/dls2/state_machine/state_machine_watcher.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
*/
Expand All @@ -46,4 +48,4 @@ namespace state_machine
};
}

#endif /* end of include guard: STATE_MACHINE_WATCHER_HPP */
#endif /* end of include guard: STATE_MACHINE_WATCHER_HPP */
16 changes: 9 additions & 7 deletions modules/state_machine/src/state_machine_watcher.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<bool()>([&](){
Expand All @@ -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;
}
Expand All @@ -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;}
}
Expand All @@ -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<bool()>([&](){
Expand All @@ -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;}
}
Expand All @@ -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;}
}
Expand Down Expand Up @@ -146,4 +148,4 @@ namespace state_machine
return false;
return true;
}
}
}