From c95e364dd773c21c24f3ba7e798391929008b2dc Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Thu, 26 Apr 2018 09:58:34 +0200
Subject: [PATCH] Started on making the Parameter Service more generic,
compiled and tested, still needs full implementaiton
---
pps_ws/src/d_fall_pps/CMakeLists.txt | 1 +
.../include/nodes/ParameterService.h | 3 ++
.../include/nodes/SafeControllerService.h | 4 ++
pps_ws/src/d_fall_pps/launch/Agent.launch | 7 ++-
.../src/d_fall_pps/param/YamlFileNames.yaml | 4 ++
.../src/nodes/MpcControllerService.cpp | 2 +-
.../d_fall_pps/src/nodes/ParameterService.cpp | 50 +++++++++++++++++++
.../src/nodes/SafeControllerService.cpp | 28 +++++++++++
.../src/nodes/StudentControllerService.cpp | 2 +-
pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv | 3 ++
10 files changed, 101 insertions(+), 3 deletions(-)
create mode 100644 pps_ws/src/d_fall_pps/param/YamlFileNames.yaml
create mode 100644 pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 93592773..e9ebf874 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 6197ce6d..36153c11 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 02e32fa0..19a1a90d 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 6f546763..64a69269 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 00000000..53fd75c2
--- /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 825378bf..3b06b351 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 bf3aa7d9..1e5ba7c3 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 1ac866ed..c361f4cd 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 1bfb40a7..24623df5 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 00000000..f51e1852
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
@@ -0,0 +1,3 @@
+string[] yamlFileNames
+---
+float64 waitTime
--
GitLab