diff --git a/pps_ws/src/d_fall_pps/CMakeLists.txt b/pps_ws/src/d_fall_pps/CMakeLists.txt
index 452128a20d1f61c76fa01400313add28e308129c..84a1ade4b018bf254b11e41b16ce57709acddae0 100755
--- a/pps_ws/src/d_fall_pps/CMakeLists.txt
+++ b/pps_ws/src/d_fall_pps/CMakeLists.txt
@@ -205,7 +205,11 @@ add_message_files(
SetpointV2.msg
CrazyflieEntry.msg
CrazyflieDB.msg
- #----------------------------------------------------------------------
+ #------------------------
+ IntWithHeader.msg
+ FloatWithHeader.msg
+ StringWithHeader.msg
+ #------------------------
DebugMsg.msg
CustomButton.msg
ViconSubscribeObjectName.msg
@@ -219,7 +223,6 @@ add_message_files(
# )
add_service_files(
FILES
- LoadYamlFiles.srv
Controller.srv
CMRead.srv
CMQuery.srv
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
index 0a4414f367093816133d3435abfde017dde64b2d..1f073f9b767a700020a3ae8cf72d1e6e13170fe7 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp
@@ -130,7 +130,7 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
flyingStateSubscriber = base_nodeHandle.subscribe("PPSClient/flyingState", 1, &CoordinatorRow::flyingStateChangedCallback, this);
// > For changes in the database that defines {agentID,cfID,flying zone} links
databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("/my_GUI/DBChanged", 1, &CoordinatorRow::databaseChangedCallback, this);;
- centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
+ centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<CMQuery>("CentralManagerService/Query", false);
// > For updating the controller that is currently operating
controllerUsedSubscriber = base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index a1822cb3a9ac01b0f437a40ba7ed0e7749bfcfe8..ce661742656d78e579c419e42cba24bc752d4931 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -192,7 +192,9 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
}
// Then, Central manager
- centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle dfall_root_nodeHandle("/dfall");
+ centralManager = dfall_root_nodeHandle.serviceClient<CMQuery>("CentralManagerService/Query", false);
loadCrazyflieContext();
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 705382ffbc80f9c397bc8cce2d237d09aaa9b67d..d7eff9d1840e4c7c45c3673561430e5d4d9fa3c6 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/BatteryMonitor.h
@@ -42,20 +42,28 @@
// ----------------------------------------------------------------------------------
#include <stdlib.h>
+#include <iostream>
#include <string>
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/network.h>
+
+// Include the standard message types
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
//#include <std_msgs/String.h>
-#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/LoadYamlFiles.h"
+// Include the DFALL message types
+#include "d_fall_pps/IntWithHeader.h"
// Include the shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+using namespace d_fall_pps;
+//using namespace std;
+
// ----------------------------------------------------------------------------------
@@ -117,18 +125,15 @@
// The ID of the agent that this node is monitoring
int m_agentID;
+// The ID of the agent that can coordinate this node
+int m_coordID;
+
// 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;
-
@@ -198,5 +203,29 @@ void updateBatteryStateBasedOnLevel(int level);
// LOAD YAML PARAMETER FUNCTIONS
void fetchYamlParameters(ros::NodeHandle& nodeHandle);
void processFetchedParameters();
+
+void yamlReadyForFetchCallback(const IntWithHeader & msg);
+
+
+
float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name);
-int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
\ No newline at end of file
+int getParameterInt(ros::NodeHandle& nodeHandle, std::string name);
+
+
+
+
+
+
+
+
+
+// FUNCTIONS FOR TEMPLATING A GET STUFF CLASS
+
+// Get the "agentID" and "coordID" from the client node
+bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref);
+
+// Construct the namespaces for the parameter services
+void getConstructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace );
+
+// Check the header of a message for whether it is relevant
+bool checkMessageHeader( int agentID , bool shouldCheckForID , const std::vector<uint> & agentIDs );
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h b/pps_ws/src/d_fall_pps/include/nodes/Constants.h
similarity index 68%
rename from pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
rename to pps_ws/src/d_fall_pps/include/nodes/Constants.h
index 9801abf3ae0cd73dbf65a6bcbe655dc4c0081aea..91e1f664c7d370a25892dd7a4100b0c8f4f82e8f 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterServiceDefinitions.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/Constants.h
@@ -25,15 +25,72 @@
//
//
// DESCRIPTION:
-// The service that manages the loading of YAML parameters
+// Constants that are used across multiple files
//
// ----------------------------------------------------------------------------------
-// DEFINES THAT ARE SHARED WITH OTHER FILES
+// ----------------------------------------------------------------------------------
+// BBBB A TTTTT TTTTT EEEEE RRRR Y Y
+// B B A A T T E R R Y Y
+// BBBB A A T T EEE RRRR Y
+// B B AAAAA T T E R R Y
+// BBBB A A T T EEEEE R R Y
+// ----------------------------------------------------------------------------------
+
+// Battery levels
+#define BATTERY_LEVEL_000 0
+#define BATTERY_LEVEL_010 1
+#define BATTERY_LEVEL_020 2
+#define BATTERY_LEVEL_030 3
+#define BATTERY_LEVEL_040 4
+#define BATTERY_LEVEL_050 5
+#define BATTERY_LEVEL_060 6
+#define BATTERY_LEVEL_070 7
+#define BATTERY_LEVEL_080 8
+#define BATTERY_LEVEL_090 9
+#define BATTERY_LEVEL_100 10
+#define BATTERY_LEVEL_UNAVAILABLE -1
+
+// Battery states
+#define BATTERY_STATE_NORMAL 0
+#define BATTERY_STATE_LOW 1
+
+
+
+
+
+// ----------------------------------------------------------------------------------
+// Y Y A M M L
+// Y Y A A MM MM L
+// Y A A M M M L
+// Y AAAAA M M L
+// Y A A M M LLLLL
+// ----------------------------------------------------------------------------------
+
+// For where to load the yaml file from
+#define LOAD_YAML_FROM_AGENT 1
+#define LOAD_YAML_FROM_COORDINATOR 2
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+// OLD STUFF FOR LOADING YAML FILES
// For which controller parameters to load from file
#define LOAD_YAML_SAFE_CONTROLLER_AGENT 1
#define LOAD_YAML_DEMO_CONTROLLER_AGENT 2
diff --git a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
index 7c0f469f4a949d5e217c943d82d30ff6fae7929f..9441c08c84fa509a9dee21039ca800e81a1ba421 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/DemoControllerService.h
@@ -59,7 +59,7 @@
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
index aac50d36e36bb10588f730045fa5f06f85e85007..f9baaca80dcac8b8cc601c6e7bcbce99fa664b5a 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PPSClient.h
@@ -58,7 +58,7 @@
#include "std_msgs/Float32.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>
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 36153c11a6fe4639fd150f7c588c4db3769d16fd..665071442bbaf24072ab46e31148bf3565bfa33a 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/ParameterService.h
@@ -47,15 +47,19 @@
#include <ros/ros.h>
#include <ros/package.h>
#include <ros/network.h>
+
+// Include the standard message types
#include "std_msgs/Int32.h"
//#include "std_msgs/Float32.h"
-//#include <std_msgs/String.h>
+#include <std_msgs/String.h>
-#include "d_fall_pps/Controller.h"
-#include "d_fall_pps/LoadYamlFiles.h"
+// Include the DFALL message types
+#include "d_fall_pps/IntWithHeader.h"
+//#include "d_fall_pps/FloatWithHeader.h"
+#include "d_fall_pps/StringWithHeader.h"
// Include the shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
// ----------------------------------------------------------------------------------
@@ -89,20 +93,17 @@ using namespace d_fall_pps;
// The type of this node, i.e., agent or a coordinator, specified as a parameter in the
// "*.launch" file
-int my_type = 0;
+int m_type = 0;
// The ID of this agent, i.e., the ID of this computer in the case that this computer is
// and agent
-std::string my_agentID = "000";
+int m_ID = 0;
-// Publisher that notifies the relevant nodes when the YAML paramters have been loaded
-// from file into ram/cache, and hence are ready to be fetched
-ros::Publisher controllerYamlReadyForFetchPublisher;
+// The namespace into which this parameter service loads yaml parameters
+std::string m_base_namespace;
-std::string m_base_namespace;
-ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
// ----------------------------------------------------------------------------------
@@ -121,4 +122,8 @@ ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
void requestLoadControllerYamlCallback(const std_msgs::Int32& msg);
-bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response);
+void requestLoadYamlFilenameCallback(const StringWithHeader& yamlFilenameToLoad);
+
+bool getTypeAndIDParameters();
+
+bool constructNamespaces();
diff --git a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
index 60ad9e02f25b51f400e3549689202548cb40fc47..8b51e35eaf4f0fd8d178126083f448973de9dfa7 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/PickerControllerService.h
@@ -60,7 +60,7 @@
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
#include <std_msgs/Float32.h>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
index a8adb30557c94b51091ee23ece23a8bfba345ead..33d91be9f75ea34a59ce2ca6a61e13ec8dbfaf23 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/RemoteControllerService.h
@@ -65,7 +65,7 @@
#include "d_fall_pps/ViconSubscribeObjectName.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
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 19a1a90d349978f5b4c2392b9a5350400dae4305..684b813434e2b3434b296f50111cc3d5d9893044 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/SafeControllerService.h
@@ -59,7 +59,7 @@
#include <std_msgs/Int32.h>
// Include the shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
diff --git a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
index 818993f0758f58f4a980ab1352944764831bf6a0..478c94ff1e63ccc2a9bd524b0c51f493fa162f05 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/StudentControllerService.h
@@ -59,7 +59,7 @@
#include "d_fall_pps/CustomButton.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
diff --git a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
index 9866bdbe93c3a79cbc59c8470aaf75ea08b8cd8b..1b950747f99ea0aa0bd04d2f26e6204529fffe1e 100644
--- a/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
+++ b/pps_ws/src/d_fall_pps/include/nodes/TuningControllerService.h
@@ -65,7 +65,7 @@
#include "d_fall_pps/ViconSubscribeObjectName.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
diff --git a/pps_ws/src/d_fall_pps/launch/Agent.launch b/pps_ws/src/d_fall_pps/launch/Agent.launch
index 7d83cb924002d4df362419d7458e5430c9b9a7a5..f4d2d83d017dd982380d067413f974f3feab64ee 100755
--- a/pps_ws/src/d_fall_pps/launch/Agent.launch
+++ b/pps_ws/src/d_fall_pps/launch/Agent.launch
@@ -3,6 +3,9 @@
<!-- INPUT ARGUMENT OF THE AGENT's ID -->
<arg name="agentID" default="$(optenv DFALL_DEFAULT_AGENT_ID)" />
+ <!-- INPUT ARGUMENT OF THE COORDINATOR's ID -->
+ <arg name="coordID" default="$(optenv DFALL_DEFAULT_COORD_ID)" />
+
<!-- INPUT ARGUMENT FOR LAUNCHING A GUI OR NOT -->
<arg name="withGUI" default="true" />
@@ -33,6 +36,7 @@
>
<rosparam command="load" file="$(find d_fall_pps)/param/ClientConfig.yaml" />
<param name="agentID" value="$(arg agentID)" />
+ <param name="coordID" value="$(arg coordID)" />
</node>
<!-- BATTERY MONITOR -->
@@ -118,8 +122,8 @@
<param name="agentID" type="str" value="$(arg agentID)" />
<rosparam
command = "load"
- file = "$(find d_fall_pps)/param/YamlFileNames.yaml"
- ns = "YamlFileNames"
+ file = "$(find d_fall_pps)/param/BatteryMonitor.yaml"
+ ns = "BatteryMonitor"
/>
<rosparam
command = "load"
diff --git a/pps_ws/src/d_fall_pps/launch/Config.sh b/pps_ws/src/d_fall_pps/launch/Config.sh
index 48d929ff5876d90a5548b4e1d0a49fcf657b3ac1..3a8fb3872789b05cf070f8c169a3483a6b52e58c 100755
--- a/pps_ws/src/d_fall_pps/launch/Config.sh
+++ b/pps_ws/src/d_fall_pps/launch/Config.sh
@@ -1,6 +1,9 @@
+# TO RUN THE SYSTEM FULLY ON THE LOCAL COMPUTER:
+# export ROS_MASTER_URI=http://localhost:11311
+# TO RUN THE SYSTEM ON A DEFAULT CONFIGURATION OF THE NETWROK:
export ROS_MASTER_URI=http://teacher:11311
+# OTHER NECESSARY ENVIRONMENT VARIABLES:
export ROS_IP=$(hostname -I | awk '{print $1;}')
export DFALL_DEFAULT_AGENT_ID=$(cat /etc/dfall_default_agent_id)
export DFALL_DEFAULT_COORD_ID=$(cat /etc/dfall_default_coord_id)
-export ROS_NAMESPACE='dfall'
-
+export ROS_NAMESPACE='dfall'
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/launch/Coordinator.launch b/pps_ws/src/d_fall_pps/launch/Coordinator.launch
index c77f5104509c44d70d789a74826306bccfd05976..59b085301c78d2fffd9c495ff879f2ab7c8f4d3a 100755
--- a/pps_ws/src/d_fall_pps/launch/Coordinator.launch
+++ b/pps_ws/src/d_fall_pps/launch/Coordinator.launch
@@ -1,8 +1,11 @@
<launch>
- <!-- INPUT ARGUMENT OF THE AGENT's ID -->
+ <!-- INPUT ARGUMENT OF THE COORDINATOR's ID -->
<arg name="coordID" default="$(optenv DFALL_DEFAULT_COORD_ID)" />
+ <!-- INPUT ARGUMENT FOR LAUNCHING A GUI OR NOT -->
+ <arg name="withGUI" default="true" />
+
<!-- Example of how to use the value in coordID -->
<!-- <param name="param" value="$(arg coordID)"/> -->
@@ -12,14 +15,68 @@
<group ns="coord$(arg coordID)">
<!-- COORDINATOR GUI -->
+ <group if="$(arg withGUI)">
+ <node
+ pkg="d_fall_pps"
+ name="flyingAgentGUI"
+ output="screen"
+ type="flyingAgentGUI"
+ >
+ <param name="type" type="str" value="coordinator" />
+ <param name="coordID" value="$(arg coordID)" />
+ </node>
+ </group>
+
+
+ <!-- PARAMETER SERVICE -->
<node
- pkg="d_fall_pps"
- name="flyingAgentGUI"
- output="screen"
- type="flyingAgentGUI"
+ pkg = "d_fall_pps"
+ name = "ParameterService"
+ output = "screen"
+ type = "ParameterService"
>
<param name="type" type="str" value="coordinator" />
- <param name="coordID" type="str" value="$(arg coordID)" />
+ <param name="coordID" value="$(arg coordID)" />
+ <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"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/DemoController.yaml"
+ ns = "DemoController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/StudentController.yaml"
+ ns = "StudentController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/MpcController.yaml"
+ ns = "MpcController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/RemoteController.yaml"
+ ns = "RemoteController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/TuningController.yaml"
+ ns = "TuningController"
+ />
+ <rosparam
+ command = "load"
+ file = "$(find d_fall_pps)/param/PickerController.yaml"
+ ns = "PickerController"
+ />
</node>
</group>
diff --git a/pps_ws/src/d_fall_pps/launch/Teacher.launch b/pps_ws/src/d_fall_pps/launch/Teacher.launch
index ef7b20a73aaf513096282fc723b2e33c8fe9d3d7..21028d0c5d9ef72ccb006007c0dfc2609d9f54c5 100755
--- a/pps_ws/src/d_fall_pps/launch/Teacher.launch
+++ b/pps_ws/src/d_fall_pps/launch/Teacher.launch
@@ -10,12 +10,4 @@
<node pkg="d_fall_pps" name="my_GUI" output="screen" type="my_GUI">
</node>
- <!-- PARAMETER SERVICE -->
- <node pkg="d_fall_pps" name="ParameterService" output="screen" type="ParameterService">
- <param name="type" type="str" value="coordinator" />
- <param name="agentID" value="0" />
- <rosparam command="load" file="$(find d_fall_pps)/param/SafeController.yaml" ns="SafeController" />
- <rosparam command="load" file="$(find d_fall_pps)/param/DemoController.yaml" ns="CustomController" />
- </node>
-
</launch>
diff --git a/pps_ws/src/d_fall_pps/msg/FloatWithHeader.msg b/pps_ws/src/d_fall_pps/msg/FloatWithHeader.msg
new file mode 100755
index 0000000000000000000000000000000000000000..a5802e7e4e8cb2fba46228bdfdd6943a9075cfe3
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/FloatWithHeader.msg
@@ -0,0 +1,3 @@
+float32 data
+bool shouldCheckForID
+uint8[] agentIDs
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/msg/IntWithHeader.msg b/pps_ws/src/d_fall_pps/msg/IntWithHeader.msg
new file mode 100755
index 0000000000000000000000000000000000000000..64cbfd8c32b1ac19c1ecdd00d52d269cfe2a1868
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/IntWithHeader.msg
@@ -0,0 +1,3 @@
+uint32 data
+bool shouldCheckForID
+uint32[] agentIDs
\ No newline at end of file
diff --git a/pps_ws/src/d_fall_pps/msg/StringWithHeader.msg b/pps_ws/src/d_fall_pps/msg/StringWithHeader.msg
new file mode 100755
index 0000000000000000000000000000000000000000..97b649562eda44e0a090183a97bb7e99298f80b0
--- /dev/null
+++ b/pps_ws/src/d_fall_pps/msg/StringWithHeader.msg
@@ -0,0 +1,3 @@
+string data
+bool shouldCheckForID
+uint8[] agentIDs
\ No newline at end of file
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 a2f02eb03ad652d10d5466eb4a39487da5aa418a..15deb2f6bd20c10cf0ede2611010dc28f72c5ea7 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/BatteryMonitor.cpp
@@ -40,9 +40,7 @@
-// SPECIFY THE PACKAGE NAMESPACE
-using namespace d_fall_pps;
-//using namespace std;
+
@@ -284,52 +282,82 @@ void updateBatteryStateBasedOnLevel(int level)
// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
-void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
+void yamlReadyForFetchCallback(const IntWithHeader & msg)
{
- // Extract from the "msg" for which controller the and from where to fetch the YAML
- // parameters
- int controller_to_fetch_yaml = msg.data;
+ // Check whether the message is relevant
+ bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForID , msg.agentIDs );
- // Switch between fetching for the different controllers and from different locations
- switch(controller_to_fetch_yaml)
+ // Continue if the message is relevant
+ if (isRevelant)
{
-
- // > 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:
+ // Extract the data
+ int parameter_service_to_load_from = msg.data;
+ // Load from the respective parameter service
+ // Switch between fetching for the different controllers and from different locations
+ switch(parameter_service_to_load_from)
{
- // 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;
+ // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+ case LOAD_YAML_FROM_AGENT:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[BATTERY MONITOR] Now fetching the YAML parameter values from this agent.");
+ // Create a node handle to the parameter service of this agent
+ 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 LOAD_YAML_FROM_COORDINATOR:
+ {
+ // Let the user know that this message was received
+ ROS_INFO("[BATTERY MONITOR] Now fetching the YAML parameter values from this agent's coordinator.");
+ // Create a node handle to the parameter service of this agent's coordinator
+ 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;
+ }
}
}
}
+// Check the header of a message for whether it is relevant
+bool checkMessageHeader( int agentID , bool shouldCheckForID , const std::vector<uint> & agentIDs )
+{
+ // The messag is by default relevant if the "shouldCheckForID"
+ // flag is false
+ if (!shouldCheckForID)
+ {
+ return true;
+ }
+ else
+ {
+ // Iterate through the vector of IDs
+ for ( int i_ID=0 ; i_ID < agentIDs.size() ; i_ID++ )
+ {
+ if ( agentIDs[i_ID] == agentID )
+ {
+ return true;
+ }
+ }
+ }
+ // If the function made it to here, then the message is
+ // NOT relevant, hence return false
+ return false;
+}
@@ -424,6 +452,55 @@ int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
+bool getAgentIDandCoordIDfromClientNode( std::string client_namespace , int * agentID_ref , int * coordID_ref)
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Create a node handle to the client
+ ros::NodeHandle client_nodeHandle(client_namespace);
+
+ // Get the value of the "agentID" parameter
+ int agentID_fetched;
+ if(!client_nodeHandle.getParam("agentID", agentID_fetched))
+ {
+ return_was_successful = false;
+ }
+ else
+ {
+ *agentID_ref = agentID_fetched;
+ }
+
+ // Get the value of the "coordID" parameter
+ int coordID_fetched;
+ if(!client_nodeHandle.getParam("coordID", coordID_fetched))
+ {
+ return_was_successful = false;
+ }
+ else
+ {
+ *coordID_ref = coordID_fetched;
+ }
+
+ // Return
+ return return_was_successful;
+}
+
+
+
+void getConstructNamespaceForCoordinatorParameterService( int coordID, std::string & coord_param_service_namespace )
+{
+ // 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
+ // Convert the agent ID to a zero padded string
+ std::ostringstream str_stream;
+ str_stream << std::setw(3) << std::setfill('0') << coordID;
+ std::string coordID_as_string(str_stream.str());
+ coord_param_service_namespace = "/dfall/coord" + coordID_as_string + "/ParameterService";
+}
+
// ----------------------------------------------------------------------------------
// M M A III N N
@@ -438,137 +515,72 @@ int main(int argc, char* argv[])
// Starting the ROS-node
ros::init(argc, argv, "BatteryMonitor");
- // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
- // the "~" indcates that "self" is the node handle assigned to this variable.
+ // Create a "ros::NodeHandle" type local variable named "nodeHandle",
+ // the "~" indcates that "self" is the node handle assigned.
ros::NodeHandle nodeHandle("~");
- // Get the namespace of this "ParameterService" node
+ // Get the namespace of this node
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);
- }
+ // AGENT ID AND COORDINATOR ID
+ // Get the ID of the agent and its coordinator
+ bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/PPSClient" , &m_agentID , &m_coordID);
+ // Stall the node IDs are not valid
+ if ( !isValid_IDs )
+ {
+ ROS_ERROR("[BATTERY SERVICE] Node NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ else
+ {
+ ROS_INFO_STREAM("[BATTERY MONITOR] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+ }
- // *********************************************************************************
- // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE
- // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:
+ // PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
- // 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
+ // Set the class variable "m_namespace_to_own_agent_parameter_service",
+ // i.e., the namespace of parameter service for 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_namespace_to_coordinator_parameter_service",
+ // i.e., the namespace of parameter service for this agent's coordinator
+ getConstructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
- // 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
+ // Inform the user of what namespaces are being used
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);
+ // Create, as local variables, node handles to the parameters services
+ ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
+ ros::NodeHandle nodeHandle_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.");
- }
- // *********************************************************************************
+ // SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+ // The parameters service publish messages with names of the form:
+ // /dfall/.../ParameterService/<filename with .yaml extension>
+ ros::Subscriber batteryMonitor_yamlReady_fromAgent = nodeHandle_to_own_agent_parameter_service.subscribe( "BatteryMonitor", 1, yamlReadyForFetchCallback);
+ ros::Subscriber batteryMonitor_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("BatteryMonitor", 1, yamlReadyForFetchCallback);
+
+ // FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+ // Call the class function that loads the parameters for this class.
+ fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
// PUBLISHERS
+
// Publisher for the filtered battery voltage
ros::Publisher filteredBatteryVoltagePublisher = nodeHandle.advertise<std_msgs::Float32>("FilteredVoltage",1);
@@ -579,7 +591,9 @@ int main(int argc, char* argv[])
ros::Publisher batteryStateChangedPublisher = nodeHandle.advertise<std_msgs::Int32>("ChangedStateTo",1);
+
// SUBSCRIBERS
+
// Subscribe to the voltage of the battery
ros::Subscriber newBatteryVoltageSubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, newBatteryVoltageCallback);
@@ -594,8 +608,9 @@ int main(int argc, char* argv[])
-
+ // Inform the user the this node is ready
ROS_INFO("[BATTERY MONITOR] Ready :-)");
+ // Spin as a single-thread node
ros::spin();
return 0;
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 9195d629a3266051b94d64da16535a1f4ae22faf..031d7bdd5857b34d02022ca21822f41953a9ab14 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/MpcControllerService.cpp
@@ -58,7 +58,7 @@
#include "d_fall_pps/DebugMsg.h"
// Include the Parameter Service shared definitions
-#include "nodes/ParameterServiceDefinitions.h"
+#include "nodes/Constants.h"
#include <std_msgs/Int32.h>
diff --git a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
index 144ddd19c4944b7534c2ca47ed2bc105e512ce64..2d656f30998e3321cf2444ace2661a2e06f12526 100755
--- a/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp
@@ -1048,7 +1048,9 @@ int main(int argc, char* argv[])
controller_setpoint.yaw = default_setpoint[3];
//ros::service::waitForService("/CentralManagerService/CentralManager");
- centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
+ // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
+ ros::NodeHandle dfall_root_nodeHandle("/dfall");
+ centralManager = dfall_root_nodeHandle.serviceClient<CMQuery>("CentralManagerService/Query", false);
loadCrazyflieContext();
//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
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 536629adb3a2529ab92a6b84d4d0d6b4ccdfc966..934e978d9a642cdd531cbd70d6483ba8762e4d58 100644
--- a/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
+++ b/pps_ws/src/d_fall_pps/src/nodes/ParameterService.cpp
@@ -74,7 +74,7 @@
-
+/*
void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
{
// Extract from the "msg" for which controller the YAML
@@ -98,9 +98,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// ----------------------------------------
// 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_COORDINATOR) && (m_type==TYPE_COORDINATOR))
||
- ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ ((controller_to_load_yaml==LOAD_YAML_SAFE_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
)
{
// Re-load the parameters of the safe controller:
@@ -109,9 +109,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// ----------------------------------------
// 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_COORDINATOR) && (m_type==TYPE_COORDINATOR))
||
- ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ ((controller_to_load_yaml==LOAD_YAML_DEMO_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
)
{
// Re-load the parameters of the demo controller:
@@ -120,9 +120,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// ----------------------------------------
// 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_COORDINATOR) && (m_type==TYPE_COORDINATOR))
||
- ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ ((controller_to_load_yaml==LOAD_YAML_STUDENT_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
)
{
// Re-load the parameters of the demo controller:
@@ -131,9 +131,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// ----------------------------------------
// 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_COORDINATOR) && (m_type==TYPE_COORDINATOR))
||
- ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ ((controller_to_load_yaml==LOAD_YAML_MPC_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
)
{
// Re-load the parameters of the demo controller:
@@ -142,9 +142,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// ----------------------------------------
// FOR THE REMOTE CONTROLLER
else if (
- ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
||
- ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ ((controller_to_load_yaml==LOAD_YAML_REMOTE_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
)
{
// Re-load the parameters of the demo controller:
@@ -153,9 +153,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// ----------------------------------------
// FOR THE TUNING CONTROLLER
else if (
- ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
||
- ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ ((controller_to_load_yaml==LOAD_YAML_TUNING_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
)
{
// Re-load the parameters of the demo controller:
@@ -164,9 +164,9 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// ----------------------------------------
// FOR THE PICKER CONTROLLER
else if (
- ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_COORDINATOR) && (my_type==TYPE_COORDINATOR))
+ ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_COORDINATOR) && (m_type==TYPE_COORDINATOR))
||
- ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_AGENT) && (my_type==TYPE_AGENT))
+ ((controller_to_load_yaml==LOAD_YAML_PICKER_CONTROLLER_AGENT) && (m_type==TYPE_AGENT))
)
{
// Re-load the parameters of the demo controller:
@@ -177,7 +177,7 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
// Let the user know that the command was not recognised
ROS_INFO_STREAM("[PARAMETER SERVICE] > Nothing to load for this parameter service with.");
ROS_INFO_STREAM("[PARAMETER SERVICE] > The message received has 'controller_to_load_yaml' = " << controller_to_load_yaml);
- ROS_INFO_STREAM("[PARAMETER SERVICE] > And the type of this Parameter Service is 'my_type' = " << my_type);
+ ROS_INFO_STREAM("[PARAMETER SERVICE] > And the type of this Parameter Service is 'm_type' = " << m_type);
// Set the boolean that prevents the fetch message from being sent
isValidToAttemptLoad = false;
}
@@ -235,126 +235,131 @@ void requestLoadControllerYamlCallback(const std_msgs::Int32& msg)
}
}
}
+*/
-
-bool loadYamlFiles(LoadYamlFiles::Request &request, LoadYamlFiles::Response &response)
+void requestLoadYamlFilenameCallback(const StringWithHeader& yaml_filename_to_load_with_header)
{
- ROS_INFO_STREAM("[PARAMETER SERVICE] DEBUG 1");
+ // LOAD THE YAML FILE
+
// Get the yaml file name requested
- std::string yamlFileName_toLoad = request.yamlFileName;
+ std::string yaml_filename_to_load = yaml_filename_to_load_with_header.data;
// Get the node handle to this parameter service
ros::NodeHandle nodeHandle("~");
-
- // 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;
-
// 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_toLoad + ".yaml " + m_base_namespace + "/" + yamlFileName_toLoad;
-
+ cmd = "rosparam load " + d_fall_pps_path + "/param" + "/" + yaml_filename_to_load + ".yaml " + m_base_namespace + "/" + yaml_filename_to_load;
// 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 );
-
+ // Send the "load yaml" command to the system
system(cmd.c_str());
- // Pause breifly to ensure that the yaml file is fully loaded
- //ros::Duration(0.5).sleep();
- // Set the response wait time
- response.waitTime = 2.0f;
- ROS_INFO_STREAM("[PARAMETER SERVICE] DEBUG 2");
- return true;
-}
+ // PUBLISH A MESSAGE THAT THE YAML FILE WAS LOADED
+ // Create publisher as a local variable, using the filename
+ // as the name of the message
+ ros::Publisher yamlParametersReadyForFetchPublisher = nodeHandle.advertise<StringWithHeader>(yaml_filename_to_load, 1);
+ // Create a local variable for the message
+ IntWithHeader yaml_ready_msg;
+ // Specify with the data the "type" of this parameter service
+ switch (m_type)
+ {
+ case TYPE_AGENT:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ case TYPE_COORDINATOR:
+ {
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ default:
+ {
+ // Throw an error if the type is not recognised
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
+ // Specify to load from the agent by default
+ yaml_ready_msg.data = LOAD_YAML_FROM_AGENT;
+ break;
+ }
+ }
+ // Copy across the boolean field
+ yaml_ready_msg.shouldCheckForID = yaml_filename_to_load_with_header.shouldCheckForID;
+ // Copy across the vector of IDs
+ if (yaml_filename_to_load_with_header.shouldCheckForID)
+ {
+ int length_of_IDs = yaml_filename_to_load_with_header.agentIDs.size();
+ for ( int i_ID=0 ; i_ID<length_of_IDs ; i_ID++ )
+ {
+ yaml_ready_msg.agentIDs.push_back(yaml_filename_to_load_with_header.agentIDs[i_ID]);
+ }
+ }
+ // Send the message
+ yamlParametersReadyForFetchPublisher.publish(yaml_ready_msg);
+}
-// ----------------------------------------------------------------------------------
-// M M A III N N
-// MM MM A A I NN N
-// M M M A A I N N N
-// M M AAAAA I N NN
-// M M A A III N N
-// ----------------------------------------------------------------------------------
-int main(int argc, char* argv[])
+bool getTypeAndIDParameters()
{
- // Starting the ROS-node
- ros::init(argc, argv, "ParameterService");
+ // Initialise the return variable as success
+ bool return_was_successful = true;
// Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
// the "~" indcates that "self" is the node handle assigned to this variable.
ros::NodeHandle nodeHandle("~");
- // Get the namespace of this "ParameterService" node
- std::string m_namespace = ros::this_node::getNamespace();
- ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << m_namespace);
-
-
-
// Get the value of the "type" parameter into a local string variable
std::string type_string;
if(!nodeHandle.getParam("type", type_string))
{
// Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("[PARAMETER SERVICE] Failed to get type from ParameterService");
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get type");
}
- // Set the "my_type" instance variable based on this string loaded
+ // Set the "m_type" class variable based on this string loaded
if ((!type_string.compare("coordinator")))
{
- my_type = TYPE_COORDINATOR;
+ m_type = TYPE_COORDINATOR;
}
else if ((!type_string.compare("agent")))
{
- my_type = TYPE_AGENT;
+ m_type = TYPE_AGENT;
}
else
{
- // Set "my_type" to the value indicating that it is invlid
- my_type = TYPE_INVALID;
+ // Set "m_type" to the value indicating that it is invlid
+ m_type = TYPE_INVALID;
+ return_was_successful = false;
ROS_ERROR("[PARAMETER SERVICE] The 'type' parameter retrieved was not recognised.");
}
- // Get the value of the "agentID" parameter into the instance variable "my_agentID"
- if(!nodeHandle.getParam("agentID", my_agentID))
- {
- // Throw an error if the agent ID parameter could not be obtained
- ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID from ParameterService");
- }
-
-
- // Publisher that notifies the relevant nodes when the YAML paramters have been loaded
- // from file into ram/cache, and hence are ready to be fetched
- controllerYamlReadyForFetchPublisher = nodeHandle.advertise<std_msgs::Int32>("controllerYamlReadyForFetch", 1);
-
-
// Construct the string to the namespace of this Paramater Service
- switch (my_type)
+ switch (m_type)
{
case TYPE_AGENT:
{
- //m_base_namespace = ros::this_node::getNamespace();
- //m_base_namespace = "/agent" + my_agentID + '/' + "ParameterService";
- m_base_namespace = m_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ // Get the value of the "agentID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("agentID", m_ID))
+ {
+ // Throw an error if the agent ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get agentID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type AGENT with ID = " << m_ID);
+ }
break;
}
@@ -362,54 +367,56 @@ int main(int argc, char* argv[])
// > The master GUI
case TYPE_COORDINATOR:
{
- //m_base_namespace = ros::this_node::getNamespace();
- //m_base_namespace = "/ParameterService";
- m_base_namespace = m_namespace + '/' + "ParameterService";
- ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
+ // Get the value of the "coordID" parameter into the class variable "m_Id"
+ if(!nodeHandle.getParam("coordID", m_ID))
+ {
+ // Throw an error if the coord ID parameter could not be obtained
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] Failed to get coordID");
+ }
+ else
+ {
+ // Inform the user about the type and ID
+ ROS_INFO_STREAM("[PARAMETER SERVICE] Is of type COORDINATOR with ID = " << m_ID);
+ }
break;
}
default:
{
- ROS_ERROR("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' variable was not recognised.");
break;
}
}
-
+ // Return
+ return return_was_successful;
+}
+
- // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
- // Delare the subscribers as local variables here so that they persist for the life
- // time of this main() function. The varaibles will be assigned in the switch case below
- // > Subscribers for when this Parameter Service node is: TYPE_AGENT
- ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_self;
- ros::Subscriber requestLoadControllerYamlSubscriber_agent_to_coordinator;
- // > Subscribers for when this Parameter Service node is: TYPE_COORDINATOR
- ros::Subscriber requestLoadControllerYamlSubscriber_coordinator_to_self;
- // SUBSCRIBE TO THE APPROPRIATE "request" MESSAGES DEPENDING ON THE "my_type"
- switch (my_type)
+
+bool constructNamespaces()
+{
+ // Initialise the return variable as success
+ bool return_was_successful = true;
+
+ // Get the namespace of this "ParameterService" node
+ std::string this_node_namespace = ros::this_node::getNamespace();
+ ROS_INFO_STREAM("[PARAMETER SERVICE] ros::this_node::getNamespace() = " << this_node_namespace);
+
+ // Construct the string to the namespace of this Paramater Service
+ switch (m_type)
{
- // AN AGENT TYPE PARAMETER SERVICE IS REQUESTED FROM:
- // > The master GUI
- // > The agent's own "PPSClient" node
case TYPE_AGENT:
{
- // Subscribing to the agent's own PPSclient
- // > First: Construct a node handle to the PPSclient
- ros::NodeHandle nh_PPSClient_for_this_agent("PPSClient");
- // > Second: Subscribe to the "requestLoadControllerYaml" topic
- requestLoadControllerYamlSubscriber_agent_to_self = nh_PPSClient_for_this_agent.subscribe("requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
-
- // Subscribing to the coordinator
- // > First: construct a node handle to the coordinator
- ros::NodeHandle nh_coordinator_for_this_agent = ros::NodeHandle();
- // > Second: Subscribe to the "requestLoadControllerYaml" topic
- requestLoadControllerYamlSubscriber_agent_to_coordinator = nh_coordinator_for_this_agent.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
-
- // Inform the user what was subscribed to:
- ROS_INFO_STREAM("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from both the 'my_GUI' and the 'PPSClient'");
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/agent" + m_Id + '/' + "ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
break;
}
@@ -417,36 +424,67 @@ int main(int argc, char* argv[])
// > The master GUI
case TYPE_COORDINATOR:
{
- // Subscribing to the coordinator's own "my_GUI"
- // > First: Get the node handle required
- ros::NodeHandle nh_coordinator_for_this_coordinator = ros::NodeHandle();
- // > Second: Subscribe to requests from: the master GUI
- requestLoadControllerYamlSubscriber_coordinator_to_self = nh_coordinator_for_this_coordinator.subscribe("/my_GUI/requestLoadControllerYaml", 1, requestLoadControllerYamlCallback);
-
- // Inform the user what was subscribed to:
- ROS_INFO_STREAM("[PARAMETER SERVICE] Subscribed to 'requestLoadControllerYaml' messages from 'my_GUI'");
+ //m_base_namespace = ros::this_node::getNamespace();
+ //m_base_namespace = "/ParameterService";
+ m_base_namespace = this_node_namespace + '/' + "ParameterService";
+ ROS_INFO_STREAM("[PARAMETER SERVICE] .yaml file parameters will be loaded into the 'base' namespace: " << m_base_namespace);
break;
}
default:
{
- ROS_ERROR("[PARAMETER SERVICE] The 'my_type' type parameter was not recognised.");
+ // Throw an error if the type is not recognised
+ return_was_successful = false;
+ ROS_ERROR("[PARAMETER SERVICE] The 'm_type' type parameter was not recognised.");
break;
}
}
+ // Return
+ return return_was_successful;
+}
+
+
+
+
- // Advertise the service for loading Yaml Files
- ros::ServiceServer service = nodeHandle.advertiseService("LoadYamlFiles", loadYamlFiles);
+// ----------------------------------------------------------------------------------
+// M M A III N N
+// MM MM A A I NN N
+// M M M A A I N N N
+// M M AAAAA I N NN
+// M M A A III N N
+// ----------------------------------------------------------------------------------
+int main(int argc, char* argv[])
+{
+ // Starting the ROS-node
+ ros::init(argc, argv, "ParameterService");
+ // Create a "ros::NodeHandle" type local variable "nodeHandle" as the current node,
+ // the "~" indcates that "self" is the node handle assigned to this variable.
+ ros::NodeHandle nodeHandle("~");
- // LOAD THE LIST OF YAML FILE NAMES
+ // Get the type and ID of this parameter service
+ bool isValid_type_and_ID = getTypeAndIDParameters();
+ // Construct the namespace into which this parameter service
+ // loads yaml parameters
+ bool isValid_namespaces = constructNamespaces();
+ // Stall the parameter service is the TYPE and ID are not valid
+ if ( !( isValid_type_and_ID && isValid_namespaces ) )
+ {
+ ROS_ERROR("[PARAMETER SERVICE] Service NOT FUNCTIONING :-)");
+ ros::spin();
+ }
+ // Subscribe to the messages that request loading a yaml file
+ ros::Subscriber requestLoadYamlFilenameSubscriber = nodeHandle.subscribe("requestLoadYamlFilename", 1, requestLoadYamlFilenameCallback);
+ // Inform the user the this node is ready
ROS_INFO("[PARAMETER SERVICE] Service ready :-)");
+ // Spin as a single-thread node
ros::spin();
return 0;
diff --git a/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv b/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
deleted file mode 100644
index fc7c7120d937bbb9ee7b0e8838a01f5e9b150499..0000000000000000000000000000000000000000
--- a/pps_ws/src/d_fall_pps/srv/LoadYamlFiles.srv
+++ /dev/null
@@ -1,3 +0,0 @@
-string yamlFileName
----
-float64 waitTime