From 25d3245824efd971cfbbbd29b48f3fe61643d421 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 7 Sep 2026 11:52:45 +0200 Subject: [PATCH 1/2] command manager: protected matched data readers --- modules/command/src/command_manager.cpp | 11 ++++++++--- modules/core_framework/src/console_layer.cpp.in | 2 +- .../include/dls2/util/messaging/dds_listeners.hpp | 6 ++++++ modules/ddscom/src/dds_listeners.cpp | 8 ++++++++ modules/signal/src/writer_base.cpp | 2 +- 5 files changed, 24 insertions(+), 5 deletions(-) diff --git a/modules/command/src/command_manager.cpp b/modules/command/src/command_manager.cpp index ce6675ef..07533c3b 100644 --- a/modules/command/src/command_manager.cpp +++ b/modules/command/src/command_manager.cpp @@ -6,6 +6,11 @@ using namespace dls; +namespace +{ + constexpr int wait_poll_period_ms = 10; +} + CommandManager::CommandManager(std::string owner_) : commands() , owner(owner_) @@ -126,7 +131,7 @@ std::multimap CommandManager::getCommandsList() if(command_publisher_listener == nullptr) return {}; // Get matched datareaders instances - auto matched_datareaders_instances = command_publisher_listener->matched_datareaders_instances; + auto matched_datareaders_instances = command_publisher_listener->get_matched_datareaders_instances(); // Find the domain participant name associated to each matched data reader, and save the name (corresponding to the command name) std::multimap cmds; for(auto datareader_instance : matched_datareaders_instances) @@ -302,7 +307,7 @@ bool CommandManager::waitCommand(const std::string& owner, const std::string& na return false; } return true; - }), timeout_ms, 2, stop_wait)){ + }), timeout_ms, wait_poll_period_ms, stop_wait)){ if(!stop_wait) std::cerr << "Command " << owner << "::" << name<<" not found" << std::endl; return false; @@ -316,7 +321,7 @@ bool CommandManager::waitCommand(const std::string& owner, const std::string& na return false; } return true; - }), timeout_ms, 2, stop_wait)){ + }), timeout_ms, wait_poll_period_ms, stop_wait)){ if(!stop_wait.load()) std::cerr << "Command " << owner << "::" << name<<" not found" << std::endl; return false; diff --git a/modules/core_framework/src/console_layer.cpp.in b/modules/core_framework/src/console_layer.cpp.in index ef125ff7..300b424d 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, 3000), load_layers_paths_{{"loadController", "${DLS_INSTALL_CONTROLLER_DIR}"}, {"loadGenerator", "${DLS_INSTALL_MOTION_GENERATOR_DIR}"}, {"loadEstimator", "${DLS_INSTALL_ESTIMATOR_DIR}"}, diff --git a/modules/ddscom/include/dls2/util/messaging/dds_listeners.hpp b/modules/ddscom/include/dls2/util/messaging/dds_listeners.hpp index ce59ffb2..2a734d1c 100644 --- a/modules/ddscom/include/dls2/util/messaging/dds_listeners.hpp +++ b/modules/ddscom/include/dls2/util/messaging/dds_listeners.hpp @@ -7,7 +7,9 @@ #include #include +#include #include +#include /// \cond doxygen_namespace_dls namespace dls @@ -28,8 +30,12 @@ namespace dls const eprosima::fastdds::dds::PublicationMatchedStatus &info ) override; + std::vector get_matched_datareaders_instances() const; + std::atomic_int matched_count; + private: + mutable std::mutex matched_datareaders_mutex_; std::vector matched_datareaders_instances; }; diff --git a/modules/ddscom/src/dds_listeners.cpp b/modules/ddscom/src/dds_listeners.cpp index c2e02520..c1880b8f 100644 --- a/modules/ddscom/src/dds_listeners.cpp +++ b/modules/ddscom/src/dds_listeners.cpp @@ -17,6 +17,8 @@ namespace dls const eprosima::fastdds::dds::PublicationMatchedStatus &info ) { + std::lock_guard lock(matched_datareaders_mutex_); + if(info.current_count_change == 1){ // publisher matched this->matched_count = info.current_count; @@ -34,6 +36,12 @@ namespace dls } } + std::vector DDSPubListener::get_matched_datareaders_instances() const + { + std::lock_guard lock(matched_datareaders_mutex_); + return matched_datareaders_instances; + } + // ===================================================================== // Subscriber Helper Listener Class Implementation // ===================================================================== diff --git a/modules/signal/src/writer_base.cpp b/modules/signal/src/writer_base.cpp index 9359249b..802c6ee4 100644 --- a/modules/signal/src/writer_base.cpp +++ b/modules/signal/src/writer_base.cpp @@ -17,7 +17,7 @@ namespace dls if(command_publisher_listener == nullptr) return {}; // Get matched datareaders instances - auto matched_datareaders_instances = command_publisher_listener->matched_datareaders_instances; + auto matched_datareaders_instances = command_publisher_listener->get_matched_datareaders_instances(); // Find the domain participant name associated to each matched data reader, and save the name (corresponding to the command name) std::vector data_readers; for(auto datareader_instance : matched_datareaders_instances) From ae1ffff9c6848c8d1a55c7c8aef5f2d5e6b73bc5 Mon Sep 17 00:00:00 2001 From: mich-pest Date: Mon, 7 Sep 2026 11:54:51 +0200 Subject: [PATCH 2/2] state_machine_watcher: protected app states access --- .../state_machine/state_machine_watcher.hpp | 9 +++- .../src/state_machine_watcher.cpp | 45 ++++++++++++------- modules/supervisor/src/supervisor.cpp | 5 ++- 3 files changed, 40 insertions(+), 19 deletions(-) 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..a5810398 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 @@ -8,12 +8,15 @@ #include #include +#include namespace state_machine { class StateMachineWatcher { public: + using AppStates = std::map>; + StateMachineWatcher(const std::string &name); ~StateMachineWatcher(); @@ -39,11 +42,13 @@ namespace state_machine bool findState(const std::string &app_name, const std::string &state) const; - std::map> app_states; + AppStates getAppStates() const; private: + mutable std::mutex app_states_mutex_; + AppStates app_states; dls::DDSParticipant dds_sm_watcher; }; } -#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..dde2feb9 100644 --- a/modules/state_machine/src/state_machine_watcher.cpp +++ b/modules/state_machine/src/state_machine_watcher.cpp @@ -6,6 +6,11 @@ namespace state_machine { + namespace + { + constexpr int wait_poll_period_ms = 10; + } + StateMachineWatcher::StateMachineWatcher(const std::string &name) : dds_sm_watcher(name, dls::domains::layers, eprosima::fastdds::rtps::DiscoveryProtocol::SUPER_CLIENT) { @@ -16,12 +21,13 @@ namespace state_machine dds_sm_watcher.addReader("state_machine_watcher", dls::topics::state_machine, - std::function{[&](void *msg) + std::function{[this](void *msg) { auto component = static_cast(msg); std::string name = component->app_name(); std::string state = component->state(); bool realtime = component->realtime(); + std::lock_guard lock(app_states_mutex_); if (app_states.find(name) == app_states.end()) { @@ -41,11 +47,11 @@ namespace state_machine { // wait app if(!dls::utils::wait(std::function([&](){ - if(app_states.find(app_name) == app_states.end()){ + if(!findApp(app_name)){ return false; } return true; - }), 5000, 2, stop_wait)){ + }), 5000, wait_poll_period_ms, stop_wait)){ if(!stop_wait){ std::cerr << app_name << " not found" << std::endl; return false; @@ -54,11 +60,11 @@ namespace state_machine // wait state if(!dls::utils::wait(std::function([&](){ - if(app_states.at(app_name).first != state){ + if(!findState(app_name, state)){ return false; } return true; - }), 5000, 2, stop_wait)){ + }), 5000, wait_poll_period_ms, stop_wait)){ if(!stop_wait){ std::cerr << app_name << " not found in state " << state << std::endl; return false;} @@ -72,11 +78,11 @@ namespace state_machine { // wait app if(!dls::utils::wait(std::function([&](){ - if(app_states.find(app_name) == app_states.end()){ + if(!findApp(app_name)){ return false; } return true; - }), 5000, 2, stop_wait)){ + }), 5000, wait_poll_period_ms, stop_wait)){ if(!stop_wait.load()){ std::cerr << app_name << " not found" << std::endl; return false;} @@ -84,11 +90,11 @@ namespace state_machine // wait state if(!dls::utils::wait(std::function([&](){ - if(app_states.at(app_name).first != state){ + if(!findState(app_name, state)){ return false; } return true; - }), 5000, 2, stop_wait)){ + }), 5000, wait_poll_period_ms, stop_wait)){ if(!stop_wait.load()){ std::cerr << app_name << " not found in state " << state << std::endl; return false;} @@ -101,11 +107,11 @@ namespace state_machine { // wait app if(!dls::utils::wait(std::function([&](){ - if(app_states.find(app_name) == app_states.end()){ + if(!findApp(app_name)){ return false; } return true; - }), 5000, 2, stop_wait)){ + }), 5000, wait_poll_period_ms, stop_wait)){ if(!stop_wait){ std::cerr << app_name << " not found" << std::endl; return false; @@ -119,11 +125,11 @@ namespace state_machine { // wait app if(!dls::utils::wait(std::function([&](){ - if(app_states.find(app_name) == app_states.end()){ + if(!findApp(app_name)){ return false; } return true; - }), 5000, 2, stop_wait)){ + }), 5000, wait_poll_period_ms, stop_wait)){ if(!stop_wait.load()){ std::cerr << app_name << " not found" << std::endl; return false;} @@ -134,6 +140,7 @@ namespace state_machine bool StateMachineWatcher::findApp(const std::string &app_name) const { + std::lock_guard lock(app_states_mutex_); if(app_states.find(app_name) == app_states.end()){ return false; } @@ -142,8 +149,16 @@ namespace state_machine bool StateMachineWatcher::findState(const std::string &app_name, const std::string &state) const { - if(!findApp(app_name) || app_states.at(app_name).first != state) + std::lock_guard lock(app_states_mutex_); + auto app = app_states.find(app_name); + if(app == app_states.end() || app->second.first != state) return false; return true; } -} \ No newline at end of file + + StateMachineWatcher::AppStates StateMachineWatcher::getAppStates() const + { + std::lock_guard lock(app_states_mutex_); + return app_states; + } +} diff --git a/modules/supervisor/src/supervisor.cpp b/modules/supervisor/src/supervisor.cpp index 4baa8ec4..3e430256 100644 --- a/modules/supervisor/src/supervisor.cpp +++ b/modules/supervisor/src/supervisor.cpp @@ -22,7 +22,8 @@ namespace dls std::function([&]()->bool { std::string info = "\n"; - for (const auto & [key, value] : state_machine_watcher.app_states){ + auto app_states = state_machine_watcher.getAppStates(); + for (const auto & [key, value] : app_states){ info += key + " " + value.first + " "; if(value.second){ info += "RT"; @@ -342,4 +343,4 @@ namespace dls } } -#endif /* end of include guard: SUPERVISOR_CPP */ \ No newline at end of file +#endif /* end of include guard: SUPERVISOR_CPP */