diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 935927737f7cc543309e9cd7cf47c42504808f5b..e9ebf874f3ead340f86c19d3c66b97a88ae00606 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -148,6 +148,7 @@ add_message_files(
# )
add_service_files(
FILES
+ LoadYamlFiles.srv
Controller.srv
CMRead.srv
CMQuery.srv
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
index 6197ce6d2f72244dfa2c969035bd445a9011baa7..36153c11a6fe4639fd150f7c588c4db3769d16fd 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
@@ -52,6 +52,7 @@
//#include <std_msgs/String.h>
#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/LoadYamlFiles.h"
// Include the shared definitions
#include "nodes/ParameterServiceDefinitions.h"
@@ -119,3 +120,5 @@ ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
// ----------------------------------------------------------------------------------
void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);
+
+bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response);
diff --git a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
index 02e32fa0a52d7c86fbdc85acad6a9a7a52d393d8..19a1a90d349978f5b4c2392b9a5350400dae4305 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
@@ -54,6 +54,7 @@
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/Controller.h"
+#include "d_fall_pps/LoadYamlFiles.h"
#include <std_msgs/Int32.h>
@@ -107,6 +108,9 @@ using namespace d_fall_pps;
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
+// The Load Yaml Files Service
+ros::ServiceClient loadYamlFilesService_own_agent;
+
std::vector<float> ffThrust(4);
std::vector<float> feedforwardMotor(4);
float cf_mass;
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index 6f5467630ff3ecfe88dd6be55f19d3037e5d2a9d..64a69269e43e8f688969c152278ce3aaebc436a9 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -75,6 +75,11 @@
<param name="type" type="str" value="agent" />
<param name="agentID" type="str" value="$(arg agentID)" />
<rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/YamlFileNames.yaml"
+ ns = "YamlFileNames"
+ />
+ <!-- rosparam
command = "load"
file = "$(find d_fall_pps)/param/SafeController.yaml"
ns = "SafeController"
@@ -93,7 +98,7 @@
command = "load"
file = "$(find d_fall_pps)/param/MpcController.yaml"
ns = "MpcController"
- />
+ / -->
</node>
<!-- AGENT GUI (aka. the "student GUI") -->
diff --git a/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml b/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml
new file mode 100644
index 0000000000000000000000000000000000000000..53fd75c2ba6f14e86dfcd8e8809314c35fd9a8af
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/param/YamlFileNames.yaml
@@ -0,0 +1,4 @@
+dictionary : {
+ 'ClientConfig' : 'ClientConfig' ,
+ 'SafeController' : 'SafeController'
+}
diff --git a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
index 825378bfd85b06f1e4df9a992dd31b48b261276a..3b06b35196761b33807fbd4e42f5029a9b7fc1ae 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -986,7 +986,7 @@ int main(int argc, char* argv[]) {
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
// Call the class function that loads the parameters for this class.
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ //fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
// *********************************************************************************
diff --git a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
index bf3aa7d913eb1c36e5f29369843c8e2c21ba6c8a..1e5ba7c3cb0524ffb552a8bbce87c3aac32c3169 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -206,6 +206,46 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
+bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response)
+{
+
+ std::string yamlFileName_toLoad = request.yamlFileNames[0];
+
+ ros::NodeHandle nodeHandle("~");
+
+ std::string yamlFileNamesParamters_basenamespace = "YamlFileNames/dictionary";
+
+ std::string paramterName = yamlFileNamesParamters_basenamespace + "/" + yamlFileName_toLoad;
+
+ std::string yamlFileName_from_dictionary;
+
+ if(!nodeHandle.getParam(paramterName, yamlFileName_from_dictionary))
+ {
+ ROS_ERROR_STREAM("[PARAMETER SERVICE] Missing parameter: '" << paramterName << "'");
+ return false;
+ }
+
+ // Instantiate a local variable for the command string that will be passed to the "system":
+ std::string cmd;
+
+ // Get the abolute path to "d_fall_pps":
+ std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
+
+ // Construct the system command string for (re-)loading the parameters:
+ cmd = "rosparam load " + d_fall_pps_path + "/param" + "/" + yamlFileName_from_dictionary + ".yaml " + m_base_namespace + "/" + yamlFileName_from_dictionary;
+
+ // Let the user know what is about to happen
+ ROS_INFO_STREAM("[PARAMETER SERVICE] The following path will be used for locating the .yaml file: " << d_fall_pps_path << " The comand line string sent to the 'system' is: " << cmd );
+
+ system(cmd.c_str());
+
+ // Pause breifly to ensure that the yaml file is fully loaded
+ ros::Duration(0.5).sleep();
+
+ return true;
+}
+
+
@@ -360,6 +400,16 @@ int main(int argc, char* argv[])
}
+ // Advertise the service for loading Yaml Files
+ ros::ServiceServer service = nodeHandle.advertiseService("LoadYamlFiles", loadYamlFiles);
+
+
+
+ // LOAD THE LIST OF YAML FILE NAMES
+
+
+
+
ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
ros::spin();
diff --git a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
index 1ac866eda0febf9afd488fc3d9032b7fc0ac8c22..c361f4cdcb6daa64158b3a0cc1758bbe0d8b6e3e 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/SafeControllerService.cpp
@@ -490,8 +490,36 @@ int main(int argc, char* argv[]) {
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
// Call the class function that loads the parameters for this class.
+
+ // Sleep for some time (in seconds)
+ ros::Duration(1.0).sleep();
+
+ // Ask the Paramter Service to load the respective YAML file
+ std::string namespace_to_own_agent_loadYamlFiles_service = namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
+ loadYamlFilesService_own_agent = ros::service::createClient<LoadYamlFiles>(namespace_to_own_agent_loadYamlFiles_service, true);
+ ROS_INFO_STREAM("[SAFE CONTROLLER] Loaded service: " << loadYamlFilesService_own_agent.getService());
+
+ LoadYamlFiles loadYamlFilesService;
+ std::vector<std::string> yamlFileNames_to_load = {"SafeController"};
+ loadYamlFilesService.request.yamlFileNames = yamlFileNames_to_load;
+ bool success = loadYamlFilesService_own_agent.call(loadYamlFilesService);
+
+ ROS_INFO_STREAM("[SAFE CONTROLLER] called Laod Yaml File service with success = " << success);
+
+ ros::Duration(2.0).sleep();
+
fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+ // DEBUGGING
+ // Add the "CustomController" namespace to the "nodeHandle"
+ //ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
+ // > The mass of the crazyflie
+ //float temp_mass = getParameterFloat(nodeHandle_for_safeController, "mass");
+ //ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << temp_mass);
+
+
+
// *********************************************************************************
diff --git a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
index 1bfb40a77b0c994e24208d7e8e1488bb7aa4d69d..24623df5a48712ad4de438c997f4e7fbcf8bdc6c 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -805,7 +805,7 @@ int main(int argc, char* argv[]) {
// FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
// Call the class function that loads the parameters for this class.
- fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ //fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
// *********************************************************************************
diff --git a/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
new file mode 100644
index 0000000000000000000000000000000000000000..f51e18525ad2cab1e31875c4c31014a98eda1d6a
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
@@ -0,0 +1,3 @@
+string[] yamlFileNames
+---
+float64 waitTime