Robustifying console

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<std::string, std::string> 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<std::string, std::string> 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;

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}"},

modules/ddscom/include/dls2/util/messaging/dds_listeners.hpp

@@ -7,7 +7,9 @@
 #include <fastdds/dds/subscriber/DataReaderListener.hpp>
 
 #include <string>
+#include <mutex>
 #include <shared_mutex>
+#include <vector>
 
 /// \cond doxygen_namespace_dls
 namespace dls
@@ -28,8 +30,12 @@ namespace dls
 			const eprosima::fastdds::dds::PublicationMatchedStatus &info
 		) override;
 
+		std::vector<eprosima::fastdds::dds::InstanceHandle_t> get_matched_datareaders_instances() const;
+
 		std::atomic_int matched_count;
 
+	private:
+		mutable std::mutex matched_datareaders_mutex_;
 		std::vector<eprosima::fastdds::dds::InstanceHandle_t> matched_datareaders_instances;
 	};
 

modules/ddscom/src/dds_listeners.cpp

@@ -17,6 +17,8 @@ namespace dls
 		const eprosima::fastdds::dds::PublicationMatchedStatus &info
 	)
 	{
+		std::lock_guard<std::mutex> 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<eprosima::fastdds::dds::InstanceHandle_t> DDSPubListener::get_matched_datareaders_instances() const
+	{
+		std::lock_guard<std::mutex> lock(matched_datareaders_mutex_);
+		return matched_datareaders_instances;
+	}
+
 	// =====================================================================
 	// Subscriber Helper Listener Class Implementation
 	// =====================================================================

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<std::string> data_readers;
 		for(auto datareader_instance : matched_datareaders_instances)

modules/state_machine/include/dls2/state_machine/state_machine_watcher.hpp

@@ -8,12 +8,15 @@
 
 #include <map>
 #include <memory>
+#include <mutex>
 
 namespace state_machine
 {
     class StateMachineWatcher
     {
     public:
+        using AppStates = std::map<std::string, std::pair<std::string, bool>>;
+
         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<std::string, std::pair<std::string, bool>> 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 */
+#endif /* end of include guard: STATE_MACHINE_WATCHER_HPP */

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(void *)>{[&](void *msg)
+                                 std::function<void(void *)>{[this](void *msg)
                                                              {
                                                                  auto component = static_cast<dls2_interface::msg::StateMachineMonitor *>(msg);
                                                                  std::string name = component->app_name();
                                                                  std::string state = component->state();
                                                                  bool realtime = component->realtime();
+                                                                 std::lock_guard<std::mutex> 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<bool()>([&](){
-                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<bool()>([&](){
-            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,23 +78,23 @@ namespace state_machine
     {       
         // wait app
         if(!dls::utils::wait(std::function<bool()>([&](){
-                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;}
             }
 
         // wait state
         if(!dls::utils::wait(std::function<bool()>([&](){
-            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<bool()>([&](){
-                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<bool()>([&](){
-                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<std::mutex> 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<std::mutex> lock(app_states_mutex_);
+        auto app = app_states.find(app_name);
+        if(app == app_states.end() || app->second.first != state)
            return false;
         return true;
     }
-}
+
+    StateMachineWatcher::AppStates StateMachineWatcher::getAppStates() const
+    {
+        std::lock_guard<std::mutex> lock(app_states_mutex_);
+        return app_states;
+    }
+}

modules/supervisor/src/supervisor.cpp

@@ -22,7 +22,8 @@ namespace dls
             std::function<bool()>([&]()->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 */
+#endif /* end of include guard: SUPERVISOR_CPP */