Commit c95e364d authored by Paul Beuchat's avatar Paul Beuchat
Browse files

Started on making the Parameter Service more generic, compiled and tested,...

Started on making the Parameter Service more generic, compiled and tested, still needs full implementaiton
parent 7f2cfd6a
......@@ -148,6 +148,7 @@ add_message_files(
# )
add_service_files(
FILES
LoadYamlFiles.srv
Controller.srv
CMRead.srv
CMQuery.srv
......
......@@ -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);
......@@ -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;
......
......@@ -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") -->
......
dictionary : {
'ClientConfig' : 'ClientConfig' ,
'SafeController' : 'SafeController'
}
......@@ -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);
// *********************************************************************************
......
......@@ -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();
......
......@@ -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);
// *********************************************************************************
......
......@@ -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);
// *********************************************************************************
......
string[] yamlFileNames
---
float64 waitTime
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment