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 f2554511981f63fa2937e0bec0bfa24664f2c300..e4d82bbef5428cd8849f90bc21faf74a80c0b3db 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
@@ -75,14 +75,23 @@
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR 14
-// For which controller parameters to fetch from from file
-#define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
-#define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
-
-
-#define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 3
-#define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 4
-
+// For send commends to the controller node informing which
+// parameters to fetch
+// > NOTE: these are identical to the #defines above, but
+// used because thez have the same name as used in
+// the controller files
+// #define FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT 1
+// #define FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT 2
+// #define FETCH_YAML_STUDENT_CONTROLLER_FROM_OWN_AGENT 3
+// #define FETCH_YAML_MPC_CONTROLLER_FROM_OWN_AGENT 4
+
+// #define FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR 11
+// #define FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR 12
+// #define FETCH_YAML_STUDENT_CONTROLLER_FROM_COORDINATOR 13
+// #define FETCH_YAML_MPC_CONTROLLER_FROM_COORDINATOR 14
+
+
+// The types, i.e., agent or coordinator
#define TYPE_INVALID -1
#define TYPE_COORDINATOR 1
#define TYPE_AGENT 2
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 ca00a05acb8921d76d96b5ce4801fdfd3fde8e63..27cd8339c3dc6a1d0ade674860efe87e6e098c94 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -95,25 +95,49 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
// Switch between loading for the different controllers
- if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
+ // ----------------------------------------
+ // FOR THE SAFE CONTROLLER
+ if (
+ ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
// Re-load the parameters of the safe controller:
cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_base_namespace + "/SafeController";
}
- else if ( (controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
+ // ----------------------------------------
+ // FOR THE DEMO CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
- // Re-load the parameters of the safe controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_base_namespace + "/SafeController";
+ // Re-load the parameters of the demo controller:
+ cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
}
- else if ( (controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR) )
+ // ----------------------------------------
+ // FOR THE STUDENT CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
// Re-load the parameters of the demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
+ cmd = "rosparam load " + d_fall_pps_path + "/param/StudentController.yaml " + m_base_namespace + "/StudentController";
}
- else if ( (controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_AGENT) && (my_type==TYPE_AGENT) )
+ // ----------------------------------------
+ // FOR THE MPC CONTROLLER
+ else if (
+ ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ||
+ ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ )
{
// Re-load the parameters of the demo controller:
- cmd = "rosparam load " + d_fall_pps_path + "/param/DemoController.yaml " + m_base_namespace + "/DemoController";
+ cmd = "rosparam load " + d_fall_pps_path + "/param/MpcController.yaml " + m_base_namespace + "/MpcController";
}
else
{
@@ -147,27 +171,29 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// Instantiate a local variable for the fetch message
std_msgs::Int32 fetch_msg;
// Fill in the data of the fetch message
- switch(controller_to_load_yaml)
- {
- case (LOAD_YAML_SAFE_CONTROLLER_COORDINATOR):
- fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR;
- break;
- case (LOAD_YAML_DEMO_CONTROLLER_COORDINATOR):
- fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR;
- break;
- case (LOAD_YAML_SAFE_CONTROLLER_AGENT):
- fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT;
- break;
- case (LOAD_YAML_DEMO_CONTROLLER_AGENT):
- fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT;
- break;
- default:
- // Let the user know that the command was not recognised
- ROS_INFO("Unknown 'controller to load yaml' command, thus a 'ready to fetch' message will NOT be published.");
- // Set the boolean that prevents the fetch message from being sent
- isReadyForFetch = false;
- break;
- }
+ fetch_msg.data = controller_to_load_yaml;
+ // Fill in the data of the fetch message
+ // switch(controller_to_load_yaml)
+ // {
+ // case (LOAD_YAML_SAFE_CONTROLLER_COORDINATOR):
+ // fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR;
+ // break;
+ // case (LOAD_YAML_DEMO_CONTROLLER_COORDINATOR):
+ // fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_COORDINATOR;
+ // break;
+ // case (LOAD_YAML_SAFE_CONTROLLER_AGENT):
+ // fetch_msg.data = FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT;
+ // break;
+ // case (LOAD_YAML_DEMO_CONTROLLER_AGENT):
+ // fetch_msg.data = FETCH_YAML_DEMO_CONTROLLER_FROM_OWN_AGENT;
+ // break;
+ // default:
+ // // Let the user know that the command was not recognised
+ // ROS_INFO("Unknown 'controller to load yaml' command, thus a 'ready to fetch' message will NOT be published.");
+ // // Set the boolean that prevents the fetch message from being sent
+ // isReadyForFetch = false;
+ // break;
+ // }
// Send a message that the YAML parameter have been loaded and hence are
// ready to be fetched (i.e., using getparam())
if (isReadyForFetch)