diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 222de5fae7dff4b3a96b1eecb8125a261bbd8b9c..452128a20d1f61c76fa01400313add28e308129c 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -318,6 +318,7 @@ if(VICON_LIBRARY)
endif()
add_executable(PPSClient src/nodes/PPSClient.cpp)
+add_executable(BatteryMonitor src/nodes/BatteryMonitor.cpp)
add_executable(SafeControllerService src/nodes/SafeControllerService.cpp)
add_executable(DemoControllerService src/nodes/DemoControllerService.cpp)
add_executable(StudentControllerService src/nodes/StudentControllerService.cpp)
@@ -401,6 +402,7 @@ if(VICON_LIBRARY)
endif()
add_dependencies(PPSClient d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(BatteryMonitor d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(SafeControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(DemoControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
add_dependencies(StudentControllerService d_fall_pps_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -451,6 +453,7 @@ if(VICON_LIBRARY)
endif()
target_link_libraries(PPSClient ${catkin_LIBRARIES})
+target_link_libraries(BatteryMonitor ${catkin_LIBRARIES})
target_link_libraries(SafeControllerService ${catkin_LIBRARIES})
target_link_libraries(DemoControllerService ${catkin_LIBRARIES})
target_link_libraries(StudentControllerService ${catkin_LIBRARIES})
diff --git a/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h b/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
index 7f4a1efc1eead038fbc75c34290b5b917db1f109..705382ffbc80f9c397bc8cce2d237d09aaa9b67d 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
@@ -48,7 +48,7 @@
#include <ros/package.h>
#include <ros/network.h>
#include "std_msgs/Int32.h"
-//#include "std_msgs/Float32.h"
+#include "std_msgs/Float32.h"
//#include <std_msgs/String.h>
#include "d_fall_pps/Controller.h"
@@ -113,6 +113,25 @@
// V A A R R III A A BBBB LLLLL EEEEE SSSS
// ----------------------------------------------------------------------------------
+
+// The ID of the agent that this node is monitoring
+int m_agentID;
+
+// NAMESPACES FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+std::string m_namespace_to_own_agent_parameter_service;
+// > For the parameter service of the coordinator
+std::string m_namespace_to_coordinator_parameter_service;
+
+// SERVICE CLIENTS FOR THE PARAMETER SERVICES
+// > For the paramter service of this agent
+ros::ServiceClient m_own_agent_parameter_service_client;
+// > For the parameter service of the coordinator
+ros::ServiceClient m_coordinator_parameter_service_client;
+
+
+
+
// The status of the crazyradio as received via messages
//int m_crazyradio_status;
@@ -132,6 +151,9 @@ ros::Publisher batteryStateChangedPublisher;
// VARIABLES THAT ARE LOADED FROM THE YAML FILE
+// Frequency of requesting the battery voltage, in [milliseconds]
+//float battery_polling_period = 200.0f;
+
// Battery thresholds while in the "motors off" state, in [Volts]
float yaml_battery_voltage_threshold_lower_while_standby = 3.30f;
float yaml_battery_voltage_threshold_upper_while_standby = 4.20f;
@@ -162,8 +184,19 @@ int yaml_battery_delay_threshold_to_change_state = 5;
void newBatteryVoltageCallback(const std_msgs::Float32& msg);
+float newBatteryVoltageForFilter(float new_value);
+// > For converting a voltage to a percentage,
+// depending on the current "my_flying_state" value
float fromVoltageToPercent(float voltage , float operating_state);
-
-int level convertVoltageToLevel(float voltage , float operating_state);
-
-
+// > For converting the percentage to a level
+int convertPercentageToLevel(float percentage);
+// > For updating the battery state based on the battery level
+// Possible states are {normal,low}, and changes are delayed
+void updateBatteryStateBasedOnLevel(int level);
+
+
+// LOAD YAML PARAMETER FUNCTIONS
+void fetchYamlParameters(ros::NodeHandle& nodeHandle);
+void processFetchedParameters();
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index a24b9ad1c03a1e998038f6978487b545dae47259..7d83cb924002d4df362419d7458e5430c9b9a7a5 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -35,6 +35,15 @@
<param name="agentID" value="$(arg agentID)" />
</node>
+ <!-- BATTERY MONITOR -->
+ <node
+ pkg = "d_fall_pps"
+ name = "BatteryMonitor"
+ output = "screen"
+ type = "BatteryMonitor"
+ >
+ </node>
+
<!-- SAFE CONTROLLER -->
<node
pkg = "d_fall_pps"
diff --git a/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp b/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
index 53384c11aa2c55c19796115bec1bcd9ccbe15c55..a2f02eb03ad652d10d5466eb4a39487da5aa418a 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
@@ -33,16 +33,16 @@
-// SPECIFY THE PACKAGE NAMESPACE
-using namespace d_fall_pps;
-//using namespace std;
+// INCLUDE THE HEADER
+#include "nodes/BatteryMonitor.h"
-// INCLUDE THE HEADER
-#include "nodes/BatteryMonitor.h"
+// SPECIFY THE PACKAGE NAMESPACE
+using namespace d_fall_pps;
+//using namespace std;
@@ -80,7 +80,7 @@ void crazyRadioStatusCallback(const std_msgs::Int32& msg)
int crazyradio_status = msg.data;
// I the Crazyradio DISCONNECTED
- if (crazyradio_status == DISCONNECTED)
+ if (crazyradio_status == CRAZY_RADIO_STATE_DISCONNECTED)
{
// Then publish that the battery level is unavailable
std_msgs::Int32 battery_level_msg;
@@ -96,14 +96,14 @@ void crazyRadioStatusCallback(const std_msgs::Int32& msg)
void newBatteryVoltageCallback(const std_msgs::Float32& msg)
{
// Extract the data
- m_battery_voltage = msg.data;
+ float battery_voltage = msg.data;
// Provide the new measurement to the filter
- float filtered_battery_voltage = newBatteryVoltageForFilter(m_battery_voltage); //need to perform filtering here
+ float filtered_battery_voltage = newBatteryVoltageForFilter(battery_voltage);
// Convert the filtered voltage to a percentage
// > Note that this depending on the operating state
- float filtered_battery_voltage_as_percentage = convertVoltageToLevel(filtered_battery_voltage,m_agent_operating_state);
+ float filtered_battery_voltage_as_percentage = fromVoltageToPercent(filtered_battery_voltage,m_agent_operating_state);
// Convert the battery percentage to a level
int filtered_battery_voltage_as_level = convertPercentageToLevel(filtered_battery_voltage_as_percentage);
@@ -127,6 +127,15 @@ void newBatteryVoltageCallback(const std_msgs::Float32& msg)
+// > For filtering the battery voltage
+float newBatteryVoltageForFilter(float new_value)
+{
+ return 0.0f;
+}
+
+
+
+
// > For converting a voltage to a percentage, depending on the current "my_flying_state" value
float fromVoltageToPercent(float voltage , float operating_state )
@@ -137,7 +146,7 @@ float fromVoltageToPercent(float voltage , float operating_state )
// COMPUTE THE PERCENTAGE DIFFERENTLY DEPENDING ON
// THE CURRENT FLYING STATE
- if (operating_state == STATE_MOTORS_OFF)
+ if (operating_state == AGENT_OPERATING_STATE_MOTORS_OFF)
{
// Voltage limits for a "standby" type of state
voltage_when_empty = yaml_battery_voltage_threshold_lower_while_standby;
@@ -171,14 +180,14 @@ float fromVoltageToPercent(float voltage , float operating_state )
-
+// > For converting the percentage to a level
int convertPercentageToLevel(float percentage)
{
// Initialise the battery level
static int battery_level = BATTERY_LEVEL_UNAVAILABLE;
// Iterate through the levels
- for (i_level=0;i_level<10;i++)
+ for (int i_level=0 ; i_level<10 ; i_level++)
{
// Compute the threshold for this level
float threshold = float(i_level) * 10.0f;
@@ -203,8 +212,9 @@ int convertPercentageToLevel(float percentage)
-
-void updateBatteryStateBasedOnLevel(level)
+// > For updating the battery state based on the battery level
+// Possible states are {normal,low}, and changes are delayed
+void updateBatteryStateBasedOnLevel(int level)
{
// Initialise the battery state
static int battery_state = BATTERY_STATE_NORMAL;
@@ -258,6 +268,163 @@ void updateBatteryStateBasedOnLevel(level)
+// ----------------------------------------------------------------------------------
+// L OOO A DDDD
+// L O O A A D D
+// L O O A A D D
+// L O O AAAAA D D
+// LLLLL OOO A A DDDD
+//
+// PPPP A RRRR A M M EEEEE TTTTT EEEEE RRRR SSSS
+// P P A A R R A A MM MM E T E R R S
+// PPPP A A RRRR A A M M M EEE T EEE RRRR SSS
+// P AAAAA R R AAAAA M M E T E R R S
+// P A A R R A A M M EEEEE T EEEEE R R SSSS
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+{
+ // Extract from the "msg" for which controller the and from where to fetch the YAML
+ // parameters
+ int controller_to_fetch_yaml = msg.data;
+
+ // Switch between fetching for the different controllers and from different locations
+ switch(controller_to_fetch_yaml)
+ {
+
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[STUDENT CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this agent.");
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ break;
+ }
+
+ // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+ case FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[STUDENT CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator_parameter_service(m_namespace_to_coordinator_parameter_service);
+ // Call the function that fetches the parameters
+ fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
+ break;
+ }
+
+ default:
+ {
+ // Let the user know that the command was not relevant
+ //ROS_INFO("The StudentControllerService received the message that YAML parameters were (re-)loaded");
+ //ROS_INFO("> However the parameters do not relate to this controller, hence nothing will be fetched.");
+ break;
+ }
+ }
+}
+
+
+
+
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters fetched from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void fetchYamlParameters(ros::NodeHandle& nodeHandle)
+{
+ // Here we load the parameters that are specified in the StudentController.yaml file
+
+ // Add the "StudentController" namespace to the "nodeHandle"
+ ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "BatteryMonitor");
+
+
+
+ // Frequency of requesting the battery voltage, in [milliseconds]
+ //yaml_battery_polling_period = getParameterFloat(nodeHandle_for_paramaters,"battery_polling_period");
+
+ // Battery thresholds while in the "motors off" state, in [Volts]
+ yaml_battery_voltage_threshold_lower_while_standby = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_lower_while_standby");
+ yaml_battery_voltage_threshold_upper_while_standby = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_upper_while_standby");
+
+ // Battery thresholds while in the "flying" state, in [Volts]
+ yaml_battery_voltage_threshold_lower_while_flying = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_lower_while_flying");
+ yaml_battery_voltage_threshold_upper_while_flying = getParameterFloat(nodeHandle_for_paramaters,"battery_voltage_threshold_upper_while_flying");
+
+ // Delay before changing the state of the battery, in [number of measurements]
+ // > Note that the delay in seconds therefore depends on the polling period
+ yaml_battery_delay_threshold_to_change_state = getParameterInt(nodeHandle_for_paramaters,"battery_delay_threshold_to_change_state");
+
+
+
+ // DEBUGGING: Print out one of the parameters that was loaded
+ ROS_INFO_STREAM("[BATTERY MONITOR] DEBUGGING: the fetched BatteryMonitor/battery_polling_period = " << yaml_battery_voltage_threshold_lower_while_flying);
+
+ // Call the function that computes details an values that are needed from these
+ // parameters loaded above
+ processFetchedParameters();
+}
+
+
+
+
+
+// This function CAN BE edited for successful completion of the PPS exercise, and the
+// use of parameters loaded from the YAML file is highly recommended to make tuning of
+// your controller easier and quicker.
+void processFetchedParameters()
+{
+ // Compute the feed-forward force that we need to counteract gravity (i.e., mg)
+ // > in units of [Newtons]
+ //cf_weight_in_newtons = cf_mass_in_grams * 9.81/1000.0;
+
+ // DEBUGGING: Print out one of the computed quantities
+ //ROS_INFO_STREAM("[STUDENT CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << cf_weight_in_newtons);
+}
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// GGGG EEEEE TTTTT PPPP A RRRR A M M ( )
+// G E T P P A A R R A A MM MM ( )
+// G EEE T PPPP A A RRRR A A M M M ( )
+// G G E T P AAAAA R R AAAAA M M ( )
+// GGGG EEEEE T P A A R R A A M M ( )
+// ----------------------------------------------------------------------------------
+
+
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
+{
+ float val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+{
+ int val;
+ if(!nodeHandle.getParam(name, val))
+ {
+ ROS_ERROR_STREAM("missing parameter '" << name << "'");
+ }
+ return val;
+}
+
+
+
+
+
// ----------------------------------------------------------------------------------
// M M A III N N
// MM MM A A I NN N
@@ -279,12 +446,127 @@ int main(int argc, char* argv[])
std::string m_namespace = ros::this_node::getNamespace();
ROS_INFO_STREAM("[BATTERY MONITOR] ros::this_node::getNamespace() = " << m_namespace);
+ // Get the agent ID as the "ROS_NAMESPACE" this computer.
+ // NOTES:
+ // > If you look at the "Agent.launch" file in the "launch" folder,
+ // you will see the following line of code:
+ // <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
+ // This line of code adds a parameter named "agentID" to the "PPSClient"
+ // > Thus, to get access to the "agentID" paremeter, we first need to get
+ // a handle to the "PPSClient" node with which this battery monitor
+ // was created.
+ // Get the handle to the "PPSClient" node
+ ros::NodeHandle PPSClient_nodeHandle(m_namespace + "/PPSClient");
+ // Get the value of the "studentID" parameter into the instance variable "my_agentID"
+ if(!PPSClient_nodeHandle.getParam("agentID", m_agentID))
+ {
+ // Throw an error if the student ID parameter could not be obtained
+ ROS_ERROR("[BATTERY MONITOR] Failed to get agentID from PPSClient");
+ }
+ else
+ {
+ // Let the user know what agentID was loaded
+ ROS_INFO_STREAM("[BATTERY MONITOR] loaded agentID = " << m_agentID);
+ }
+
+
+
+
+
+ // *********************************************************************************
+ // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
+
+
+ // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+
+ // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
+ // namespace string for the parameter service that is running on the machine of this
+ // agent
+ m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
+
+ // Create a node handle to the parameter service running on this agent's machine
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:
+
+ // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
+ // for the parameter service that is running on the coordinate machine
+ // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
+ // is very important because it specifies that the name is global
+ m_namespace_to_coordinator_parameter_service = "/ParameterService";
+
+ // Create a node handle to the parameter service running on the coordinator machine
+ ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
+ //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
+
+
+ // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
+ // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
+ // and calls the class function "yamlReadyForFetchCallback" each time a message is
+ // received on this topic and the message is passed as an input argument to the
+ // "yamlReadyForFetchCallback" class function.
+ ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+ //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
+
+
+ // Set the class variable "m_own_agent_parameter_service_client"
+ // for making requests to the "LoadYamlFiles" service that is advertised
+ // by the "ParameterService" nodes
+ std::string own_agent_parameter_service_client_string = m_namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
+ m_own_agent_parameter_service_client = ros::service::createClient<LoadYamlFiles>(own_agent_parameter_service_client_string, true);
+ ROS_INFO_STREAM("[BATTERY MONITOR] Loaded parameter service client: " << m_own_agent_parameter_service_client.getService());
+
+ // PRINT OUT SOME INFORMATION
+
+ // Let the user know what namespaces are being used for linking to the parameter service
+ ROS_INFO_STREAM("[BATTERY MONITOR] The namespace string for accessing the Paramter Services are:");
+ ROS_INFO_STREAM("[BATTERY MONITOR] m_namespace_to_own_agent_parameter_service = " << m_namespace_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[BATTERY MONITOR] m_namespace_to_coordinator_parameter_service = " << m_namespace_to_coordinator_parameter_service);
+
+
+ // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
+
+ // Call the class function that loads the parameters for this class.
+ //fetchYamlParameters(m_nodeHandle_to_own_agent_parameter_service);
+
+ // Load the yaml paramters
+ ros::Duration(2.0).sleep();
+ ROS_INFO_STREAM("[BATTERY MONITOR] DEBUG 1");
+ // Prepare the service call
+ LoadYamlFiles load_yaml_files_service_call;
+ std::string yaml_file_name_to_request = "BatteryMonitor";
+ load_yaml_files_service_call.request.yamlFileName = yaml_file_name_to_request;
+ // Call on the service
+ ROS_INFO_STREAM("[BATTERY MONITOR] DEBUG 2");
+ bool success = m_own_agent_parameter_service_client.call(load_yaml_files_service_call);
+ if (success)
+ {
+ // Extract the data from the response
+ float wait_time_for_yaml = load_yaml_files_service_call.response.waitTime;
+ // Wait for the specified time
+ ros::Duration(wait_time_for_yaml).sleep();
+ // Call the function that loads the yaml paramters
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
+ ROS_INFO_STREAM("[BATTERY MONITOR] DEBUG 3");
+ }
+ else
+ {
+ ROS_INFO("[BATTERY MONITOR] The load yaml file service call was NOT successful.");
+ }
+
+ // *********************************************************************************
- // Advertise the service for loading Yaml Files
- ros::ServiceServer service = nodeHandle.advertiseService("LoadYamlFiles", loadYamlFiles);
// PUBLISHERS
// Publisher for the filtered battery voltage
@@ -311,6 +593,8 @@ int main(int argc, char* argv[])
m_agent_operating_state = AGENT_OPERATING_STATE_MOTORS_OFF;
+
+
ROS_INFO("[BATTERY MONITOR] Ready :-)");
ros::spin();
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 976274ba52c400debaed6918823a5f7e5c708d29..536629adb3a2529ab92a6b84d4d0d6b4ccdfc966 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -241,22 +241,21 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response)
{
-
- std::string yamlFileName_toLoad = request.yamlFileNames[0];
-
+ ROS_INFO_STREAM("[PARAMETER SERVICE] DEBUG 1");
+ // Get the yaml file name requested
+ std::string yamlFileName_toLoad = request.yamlFileName;
+ // Get the node handle to this parameter service
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;
- }
+ // OLD: Get the yaml namespace from a yaml dictionary
+ //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;
@@ -265,7 +264,7 @@ bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &res
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;
+ cmd = "rosparam load " + d_fall_pps_path + "/param" + "/" + yamlFileName_toLoad + ".yaml " + m_base_namespace + "/" + yamlFileName_toLoad;
// 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 );
@@ -273,7 +272,11 @@ bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &res
system(cmd.c_str());
// Pause breifly to ensure that the yaml file is fully loaded
- ros::Duration(0.5).sleep();
+ //ros::Duration(0.5).sleep();
+
+ // Set the response wait time
+ response.waitTime = 2.0f;
+ ROS_INFO_STREAM("[PARAMETER SERVICE] DEBUG 2");
return true;
}
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 4ec1acc3bdb924d3196b80be9e3abd4db3e06d92..7f2098be738605bc360a23230e2efedbe510c7a8 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/StudentControllerService.cpp
@@ -804,7 +804,7 @@ int main(int argc, char* argv[]) {
// Get the agent ID as the "ROS_NAMESPACE" this computer.
// NOTES:
- // > If you look at the "Student.launch" file in the "launch" folder, you will see
+ // > If you look at the "Agent.launch" file in the "launch" folder, you will see
// the following line of code:
// <param name="studentID" value="$(optenv ROS_NAMESPACE)" />
// This line of code adds a parameter named "studentID" to the "PPSClient"
diff --git a/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
index f51e18525ad2cab1e31875c4c31014a98eda1d6a..fc7c7120d937bbb9ee7b0e8838a01f5e9b150499 100644
--- a/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
+++ b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
@@ -1,3 +1,3 @@
-string[] yamlFileNames
+string yamlFileName
---
float64 waitTime