Added and tested CrazyRadio Emulator

dfall_ws/src/dfall_pkg/CMakeLists.txt

@@ -387,6 +387,8 @@ add_executable(FlyingAgentClient         src/nodes/FlyingAgentClient.cpp
                                          src/classes/GetParamtersAndNamespaces.cpp)
 add_executable(BatteryMonitor            src/nodes/BatteryMonitor.cpp
                                          src/classes/GetParamtersAndNamespaces.cpp)
+add_executable(CrazyRadioEmulator        src/nodes/CrazyRadioEmulator.cpp
+                                         src/classes/GetParamtersAndNamespaces.cpp)
 add_executable(DefaultControllerService  src/nodes/DefaultControllerService.cpp
                                          src/classes/GetParamtersAndNamespaces.cpp)
 add_executable(SafeControllerService     src/nodes/SafeControllerService.cpp)
@@ -474,6 +476,7 @@ endif()
 
 add_dependencies(FlyingAgentClient         dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(BatteryMonitor            dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
+add_dependencies(CrazyRadioEmulator        dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(DefaultControllerService  dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(SafeControllerService     dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
 add_dependencies(DemoControllerService     dfall_pkg_generate_messages_cpp ${catkin_EXPORTED_TARGETS})
@@ -524,6 +527,7 @@ endif()
 
 target_link_libraries(FlyingAgentClient         ${catkin_LIBRARIES})
 target_link_libraries(BatteryMonitor            ${catkin_LIBRARIES})
+target_link_libraries(CrazyRadioEmulator        ${catkin_LIBRARIES})
 target_link_libraries(DefaultControllerService  ${catkin_LIBRARIES})
 target_link_libraries(SafeControllerService     ${catkin_LIBRARIES})
 target_link_libraries(DemoControllerService     ${catkin_LIBRARIES})

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/main.cpp

@@ -48,6 +48,7 @@ int main(int argc, char *argv[])
     // ----------------------------------------------------------- //
     // THE FOLLOWING IS A SLIGHT ADAPTATION FOR COMBING WITH ROS:
     QApplication a(argc, argv);
+    //a.setWindowIcon(QIcon(":/images/battery_empty.png"));
     MainWindow w(argc, argv);
     w.show();
     return a.exec();

dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h

+//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+//    This file is part of D-FaLL-System.
+//    
+//    D-FaLL-System is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//    
+//    D-FaLL-System is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//    
+//    You should have received a copy of the GNU General Public License
+//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
+//    
+//
+//    ----------------------------------------------------------------------------------
+//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
+//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
+//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
+//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
+//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
+//
+//
+//    DESCRIPTION:
+//    A node to emulator a CrazyRadio node
+//
+//    ----------------------------------------------------------------------------------
+
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    III  N   N   CCCC  L      U   U  DDDD   EEEEE   SSSS
+//     I   NN  N  C      L      U   U  D   D  E      S
+//     I   N N N  C      L      U   U  D   D  EEE     SSS
+//     I   N  NN  C      L      U   U  D   D  E          S
+//    III  N   N   CCCC  LLLLL   UUU   DDDD   EEEEE  SSSS
+//    ----------------------------------------------------------------------------------
+
+#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 the DFALL message types
+#include "dfall_pkg/IntWithHeader.h"
+#include "dfall_pkg/ControlCommand.h"
+
+// Include the DFALL service types
+#include "dfall_pkg/IntIntService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+#include "classes/GetParamtersAndNamespaces.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+using namespace dfall_pkg;
+//using namespace std;
+
+
+
+//    ----------------------------------------------------------------------------------
+//    DDDD   EEEEE  FFFFF  III  N   N  EEEEE   SSSS
+//    D   D  E      F       I   NN  N  E      S
+//    D   D  EEE    FFF     I   N N N  EEE     SSS
+//    D   D  E      F       I   N  NN  E          S
+//    DDDD   EEEEE  F      III  N   N  EEEEE  SSSS
+//    ----------------------------------------------------------------------------------
+
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    V   V    A    RRRR   III    A    BBBB   L      EEEEE   SSSS
+//    V   V   A A   R   R   I    A A   B   B  L      E      S
+//    V   V  A   A  RRRR    I   A   A  BBBB   L      EEE     SSS
+//     V V   AAAAA  R  R    I   AAAAA  B   B  L      E          S
+//      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;
+
+// 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;
+
+
+
+// The status of the Radio connection
+int m_radio_state = CRAZY_RADIO_STATE_DISCONNECTED;
+
+// Timer for the battery voltage updates
+ros::Timer m_timer_battery_voltage_updates;
+
+
+// PUBLISHERS
+
+// Publisher for the status of the radio connection
+ros::Publisher crazyRadioStatusPublisher;
+
+// Publisher for sending a "Flying Agent Client Command"
+ros::Publisher flyingAgentClientCommandPublisher;
+
+// Publisher for the filtered battery voltage
+ros::Publisher batteryVoltagePublisher;
+
+// Publisher for the control commands
+// > Note this is not needed for the real CrazyRadio node because
+//   this command goes out over the radio, but for emulation we publish
+//   the control command
+ros::Publisher controlCommandPublisher;
+
+
+
+
+
+
+
+// VARIABLES THAT ARE LOADED FROM THE YAML FILE
+
+// Frequency of requesting the battery voltage, in [seconds]
+float yaml_battery_polling_period_in_seconds = 0.2f;
+
+// // 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;
+
+// // Battery thresholds while in the "flying" state, in [Volts]
+// float yaml_battery_voltage_threshold_lower_while_flying = 2.60f;
+// float yaml_battery_voltage_threshold_upper_while_flying = 3.70f;
+
+// // Delay before changing the state of the battery, in [number of measurements]
+// // > Note that the delay in seconds therefore depends on the polling period
+// int yaml_battery_delay_threshold_to_change_state = 5;
+
+
+
+//    ----------------------------------------------------------------------------------
+//    FFFFF  U   U  N   N   CCCC  TTTTT  III   OOO   N   N
+//    F      U   U  NN  N  C        T     I   O   O  NN  N
+//    FFF    U   U  N N N  C        T     I   O   O  N N N
+//    F      U   U  N  NN  C        T     I   O   O  N  NN
+//    F       UUU   N   N   CCCC    T    III   OOO   N   N
+//
+//    PPPP   RRRR    OOO   TTTTT   OOO   TTTTT  Y   Y  PPPP   EEEEE   SSSS
+//    P   P  R   R  O   O    T    O   O    T     Y Y   P   P  E      S
+//    PPPP   RRRR   O   O    T    O   O    T      Y    PPPP   EEE     SSS
+//    P      R  R   O   O    T    O   O    T      Y    P      E          S
+//    P      R   R   OOO     T     OOO     T      Y    P      EEEEE  SSSS
+//    ----------------------------------------------------------------------------------
+
+// CHANGE THE STATE OF THE RADIO
+void change_radio_state_to( int new_state );
+
+// PUBLISH THE CURRENT STATE OF THE RADIO CONNECTION
+void publishCurrentRadioState();
+
+// PERFORM CONNECT AND DISCONNECT
+void connect();
+void disconnect();
+
+// RESPOND TO CONNECTED AND DISCONNECTED STATUS CHANGE
+void connected_callback(const ros::TimerEvent&);
+void connection_failed();
+void connection_lost();
+void disconnected_callback(const ros::TimerEvent&);
+
+
+// SUBSCRIBER CALLBACKS
+void crazyRadioCommandCallback(const IntWithHeader & msg);
+void controlCommandCallback(const ControlCommand & msg);
+
+// SERVICE CALLBACK
+bool getCurrentCrazyRadioStatusServiceCallback(IntIntService::Request &request, IntIntService::Response &response);
+
+
+// UPDATE BATTERY VOLTAGE TIMER CALLBACK
+void update_battery_voltage_callback(const ros::TimerEvent&);
+
+// LOAD YAML PARAMETER FUNCTIONS
+void isReadyBatteryMonitorYamlCallback(const IntWithHeader & msg);
+void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file

dfall_ws/src/dfall_pkg/launch/agent.launch

@@ -9,6 +9,9 @@
 	<!-- INPUT ARGUMENT FOR LAUNCHING A GUI OR NOT -->
 	<arg name="withGUI" default="true" />
 
+	<!-- INPUT ARGUMENT FOR EMULATING THE CRAZY RADIO -->
+	<arg name="emulateRadio" default="false" />
+
 	<!-- Example of how to use the value in agentID -->
     <!-- <param name="param" value="$(arg agentID)"/> -->
 
@@ -21,14 +24,27 @@
     <group ns="$(eval 'agent' + str(agentID).zfill(3))">
 
 		<!-- CRAZY RADIO -->
-		<node
-			pkg    = "dfall_pkg"
-			name   = "CrazyRadio"
-			output = "screen"
-			type   = "CrazyRadio.py"
-			>
-			<rosparam command="load" file="$(find dfall_pkg)/param/BatteryMonitor.yaml" />
-		</node>
+		<group unless="$(arg emulateRadio)">
+			<node
+				pkg    = "dfall_pkg"
+				name   = "CrazyRadio"
+				output = "screen"
+				type   = "CrazyRadio.py"
+				>
+				<rosparam command="load" file="$(find dfall_pkg)/param/BatteryMonitor.yaml" />
+			</node>
+		</group>
+		<group if="$(arg emulateRadio)">
+			<node
+				pkg    = "dfall_pkg"
+				name   = "CrazyRadio"
+				output = "screen"
+				type   = "CrazyRadioEmulator"
+				>
+				<rosparam command="load" file="$(find dfall_pkg)/param/BatteryMonitor.yaml" />
+			</node>
+		</group>
+
 
 		<!-- FLYING AGENT CLIENT -->
 		<node

dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp

@@ -510,11 +510,11 @@ int main(int argc, char* argv[])
 
 	// Get a node handle to the Crazy Radio
 	std::string namespace_to_crazyradio = m_namespace + "/CrazyRadio";
-    ros::NodeHandle nodeHandle_to_crazyradio(namespace_to_crazyradio);
+	ros::NodeHandle nodeHandle_to_crazyradio(namespace_to_crazyradio);
 
-    // Get a node handle to the Flying Agent Client
+	// Get a node handle to the Flying Agent Client
 	std::string namespace_to_FlyingAgentClient = m_namespace + "/FlyingAgentClient";
-    ros::NodeHandle nodeHandle_to_FlyingAgentClient(namespace_to_FlyingAgentClient);
+	ros::NodeHandle nodeHandle_to_FlyingAgentClient(namespace_to_FlyingAgentClient);
 
 	// Subscribe to the voltage of the battery
 	ros::Subscriber newBatteryVoltageSubscriber = nodeHandle_to_crazyradio.subscribe("CFBattery", 1, newBatteryVoltageCallback);

dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp

+//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
+//
+//    This file is part of D-FaLL-System.
+//    
+//    D-FaLL-System is free software: you can redistribute it and/or modify
+//    it under the terms of the GNU General Public License as published by
+//    the Free Software Foundation, either version 3 of the License, or
+//    (at your option) any later version.
+//    
+//    D-FaLL-System is distributed in the hope that it will be useful,
+//    but WITHOUT ANY WARRANTY; without even the implied warranty of
+//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+//    GNU General Public License for more details.
+//    
+//    You should have received a copy of the GNU General Public License
+//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
+//    
+//
+//    ----------------------------------------------------------------------------------
+//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
+//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
+//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
+//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
+//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
+//
+//
+//    DESCRIPTION:
+//    A node to emulator a CrazyRadio node
+//
+//    ----------------------------------------------------------------------------------
+
+
+
+
+
+// INCLUDE THE HEADER
+#include "nodes/CrazyRadioEmulator.h"
+
+void change_radio_state_to( int new_state )
+{
+	// Update the class variable
+	m_radio_state = new_state;
+	// Inform the user of the updated state
+	switch (m_radio_state)
+	{
+		case CRAZY_RADIO_STATE_DISCONNECTED:
+		{
+			ROS_INFO("[CRAZY RADIO EMULATOR] radio state changed to DISCONNECTED)");
+			break;
+		}
+		case CRAZY_RADIO_STATE_CONNECTING:
+		{
+			ROS_INFO("[CRAZY RADIO EMULATOR] radio state changed to CONNECTING)");
+			break;
+		}
+		case CRAZY_RADIO_STATE_CONNECTED:
+		{
+			ROS_INFO("[CRAZY RADIO EMULATOR] radio state changed to CONNECTED)");
+			break;
+		}
+	}
+	// Publish the updated state
+	publishCurrentRadioState();
+}
+
+void publishCurrentRadioState()
+{
+	std_msgs::Int32 msg;
+	msg.data = m_radio_state;
+	crazyRadioStatusPublisher.publish(msg);
+}
+
+
+// PERFORM CONNECT
+void connect()
+{
+	// Declare a static timer instance
+	static float connecting_duration = 1.0;
+	static ros::NodeHandle nodeHandle("~");
+	static ros::Timer timer_connecting = nodeHandle.createTimer(ros::Duration(connecting_duration), connected_callback, true);
+
+	// > Stop any previous instance that might still be running
+	timer_connecting.stop();
+
+	// Update status to CONNECTING
+	change_radio_state_to(CRAZY_RADIO_STATE_CONNECTING);
+	// Inform the user
+	ROS_INFO("[CRAZY RADIO EMULATOR] Connecting...");
+	
+	// > Set the period again (second argument is reset)
+	timer_connecting.setPeriod( ros::Duration(connecting_duration), true);
+	// > Start the timer again
+	timer_connecting.start();
+}
+
+// PERFORM DISCONNECT
+void disconnect()
+{
+	// Declare a static timer instance
+	static float disconnecting_duration = 0.1;
+	static ros::NodeHandle nodeHandle("~");
+	static ros::Timer timer_disconnecting = nodeHandle.createTimer(ros::Duration(disconnecting_duration), disconnected_callback, true);
+
+	// > Stop any previous instance that might still be running
+	timer_disconnecting.stop();
+
+	// Send a MOTOS-OFF command
+	ROS_INFO("[CRAZY RADIO EMMULATOR] sending Motors OFF command before disconnecting");
+
+	// Send the MOTORS-OFF command to the Flying Agent Client
+	IntWithHeader msg;
+	msg.shouldCheckForAgentID = false;
+	msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+	flyingAgentClientCommandPublisher.publish(msg);
+
+	// Inform the user that now disconnecting
+	ROS_INFO("[CRAZY RADIO EMULATOR] Disconnecting from ...");
+
+	// > Set the period again (second argument is reset)
+	timer_disconnecting.setPeriod( ros::Duration(disconnecting_duration), true);
+	// > Start the timer again
+	timer_disconnecting.start();
+
+	// Update status to DISCONNECTED
+	change_radio_state_to(CRAZY_RADIO_STATE_DISCONNECTED);
+}
+
+
+
+
+// RESPOND TO CONNECTED CALLBACK
+void connected_callback(const ros::TimerEvent&)
+{
+	// Update status to CONNECTED
+	change_radio_state_to(CRAZY_RADIO_STATE_CONNECTED);
+
+	// Send the MOTORS-OFF command to the Flying Agent Client
+	IntWithHeader msg;
+	msg.shouldCheckForAgentID = false;
+	msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+	flyingAgentClientCommandPublisher.publish(msg);
+
+	// Start the battery voltage updates
+	m_timer_battery_voltage_updates.start();
+}
+
+void connection_failed()
+{
+	// Inform the user
+	ROS_INFO("[CRAZY RADIO EMMULATOR] Connection failed");
+	// Update status to DISCONNECTED
+	change_radio_state_to(CRAZY_RADIO_STATE_DISCONNECTED);
+	// Stop the battery voltage updates
+	m_timer_battery_voltage_updates.stop();
+}
+
+void connection_lost()
+{
+	// Inform the user
+	ROS_INFO("[CRAZY RADIO EMMULATOR] Connection lost");
+	// Update status to DISCONNECTED
+	change_radio_state_to(CRAZY_RADIO_STATE_DISCONNECTED);
+	// Stop the battery voltage updates
+	m_timer_battery_voltage_updates.stop();
+}
+
+
+// RESPOND TO DISCONNECTED CALLBACK
+void disconnected_callback(const ros::TimerEvent&)
+{
+	// Update status to DISCONNECTED
+	change_radio_state_to(CRAZY_RADIO_STATE_DISCONNECTED);
+
+	// Stop the battery voltage updates
+	m_timer_battery_voltage_updates.stop();
+}
+
+
+
+void crazyRadioCommandCallback(const IntWithHeader & msg)
+{
+	// Check whether the message is relevant
+	bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+	// Continue if the message is relevant
+	if (isRevelant)
+	{
+		// Respond to a "should connect" command
+		if (msg.data == CMD_RECONNECT)
+		{
+			switch (m_radio_state)
+			{
+				case CRAZY_RADIO_STATE_DISCONNECTED:
+				{
+					ROS_INFO("[CRAZY RADIO EMULATOR] received command to CONNECT (current status is DISCONNECTED)");
+					connect();
+					break;
+				}
+
+				case CRAZY_RADIO_STATE_CONNECTING:
+				{
+					ROS_INFO("[CRAZY RADIO EMULATOR] received command to CONNECT (current status is CONNECTING)");
+					//publishCurrentRadioState();
+					break;
+				}
+
+				case CRAZY_RADIO_STATE_CONNECTED:
+				{
+					ROS_INFO("[CRAZY RADIO EMULATOR] received command to CONNECT (current status is CONNECTED)");
+					//publishCurrentRadioState();
+					break;
+				}
+			}
+			
+		}
+		// Respond to a "should disconnect" command
+		else if (msg.data == CMD_DISCONNECT)
+		{
+			switch (m_radio_state)
+			{
+				case CRAZY_RADIO_STATE_CONNECTED:
+				{
+					ROS_INFO("[CRAZY RADIO EMULATOR] received command to DISCONNECT (current status is CONNECTED)");
+					disconnect();
+					break;
+				}
+
+				case CRAZY_RADIO_STATE_CONNECTING:
+				{
+					ROS_INFO("[CRAZY RADIO EMULATOR] received command to DISCONNECT (current status is CONNECTING)");
+					//publishCurrentRadioState();
+					break;
+				}
+
+				case CRAZY_RADIO_STATE_DISCONNECTED:
+				{
+					ROS_INFO("[CRAZY RADIO EMULATOR] received command to CONNECT (current status is DISCONNECTED)");
+					//publishCurrentRadioState();
+					break;
+				}
+			}
+		}
+	}
+}
+
+
+
+void controlCommandCallback(const ControlCommand & msg)
+{
+	// Directly re-publish the message
+	controlCommandPublisher.publish(msg);
+}
+
+
+bool getCurrentCrazyRadioStatusServiceCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+	// Put the radio state into the response variable
+    response.data = m_radio_state;
+    // Return
+    return true;
+}
+
+
+void update_battery_voltage_callback(const ros::TimerEvent&)
+{
+	// Declare static paramters for the voltage
+	static float voltage_current = 4.2;
+	static float voltage_decrement = 0.001;
+	static float voltage_max = 4.2;
+	static float voltage_min = 3.4;
+
+	// Update the voltage
+	voltage_current = voltage_current - voltage_decrement;
+
+	// Wrap the voltage from the min back up to the max
+	if (voltage_current < voltage_min)
+		voltage_current = voltage_max;
+
+	// Publish the current voltage
+	std_msgs::Float32 msg;
+	msg.data = voltage_current;
+	batteryVoltagePublisher.publish(msg);
+}
+
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    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
+//    ----------------------------------------------------------------------------------
+
+
+
+void isReadyBatteryMonitorYamlCallback(const IntWithHeader & msg)
+{
+	// Check whether the message is relevant
+	bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
+
+	// Continue if the message is relevant
+	if (isRevelant)
+	{
+		// Extract the data
+		int parameter_service_to_load_from = msg.data;
+		// Initialise a local variable for the namespace
+		std::string namespace_to_use;
+		// Load from the respective parameter service
+		switch(parameter_service_to_load_from)
+		{
+			// > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
+			case LOAD_YAML_FROM_AGENT:
+			{
+				ROS_INFO("[CRAZY RADIO EMULATOR] Now fetching the BatteryMonitor YAML parameter values from this agent.");
+				namespace_to_use = m_namespace_to_own_agent_parameter_service;
+				break;
+			}
+			// > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
+			case LOAD_YAML_FROM_COORDINATOR:
+			{
+				ROS_INFO("[CRAZY RADIO EMULATOR] Now fetching the BatteryMonitor YAML parameter values from this agent's coordinator.");
+				namespace_to_use = m_namespace_to_coordinator_parameter_service;
+				break;
+			}
+
+			default:
+			{
+				ROS_ERROR("[CRAZY RADIO EMULATOR] Paramter service to load from was NOT recognised.");
+				namespace_to_use = m_namespace_to_own_agent_parameter_service;
+				break;
+			}
+		}
+		// Create a node handle to the selected parameter service
+		ros::NodeHandle nodeHandle_to_use(namespace_to_use);
+		// Call the function that fetches the parameters
+		fetchBatteryMonitorYamlParameters(nodeHandle_to_use);
+	}
+}
+
+
+
+
+
+
+void fetchBatteryMonitorYamlParameters(ros::NodeHandle& nodeHandle)
+{
+	// Here we load the parameters that are specified in the file:
+	// BatteryMonitor.yaml
+
+	// Add the "BatteryMonitor" namespace to the "nodeHandle"
+	ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "BatteryMonitor");
+
+
+
+	// Frequency of requesting the battery voltage, in [milliseconds]
+	yaml_battery_polling_period_in_seconds = getParameterFloat(nodeHandle_for_paramaters,"battery_polling_period");
+	yaml_battery_polling_period_in_seconds = yaml_battery_polling_period_in_seconds * 0.001;
+
+	// // 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("[CRAZY RADIO EMULATOR] DEBUGGING: the fetched BatteryMonitor/battery_polling_period = " << yaml_battery_polling_period_in_seconds);
+
+
+
+	// PROCESS ANY OF THE FETCHED PARAMETERS AS NECESSARY
+}
+
+
+
+
+//    ----------------------------------------------------------------------------------
+//    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, "CrazyRadio");
+
+	// 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 "BatteryMonitor" node
+	std::string m_namespace = ros::this_node::getNamespace();
+	ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] ros::this_node::getNamespace() =  " << m_namespace);
+
+
+
+	// AGENT ID AND COORDINATOR ID
+
+	// 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
+	//   "FlyingAgentClient" node.
+	// > Thus, to get access to this "agentID" paremeter, we first
+	//   need to get a handle to the "FlyingAgentClient" node within which this
+	//   controller service is nested.
+
+
+	// Get the ID of the agent and its coordinator
+	bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
+
+	// Stall the node IDs are not valid
+	if ( !isValid_IDs )
+	{
+		ROS_ERROR("[CRAZY RADIO EMULATOR] Node NOT FUNCTIONING :-)");
+		ros::spin();
+	}
+	else
+	{
+		ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
+	}
+
+
+
+	// PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
+
+	// NOTES:
+	// > The parameters that are specified thorugh the *.yaml files
+	//   are managed by a separate node called the "Parameter Service"
+	// > A separate node is used for reasons of speed and generality
+	// > To allow for a distirbuted architecture, there are two
+	//   "ParamterService" nodes that are relevant:
+	//   1) the one that is nested under the "m_agentID" namespace
+	//   2) the one that is nested under the "m_coordID" namespace
+	// > The following lines of code create the namespace (as strings)
+	//   to there two relevant "ParameterService" nodes.
+	// > The node handles are also created because they are needed
+	//   for the ROS Subscriptions that follow.
+
+	// 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";
+
+	// Set the class variable "m_namespace_to_coordinator_parameter_service",
+	// i.e., the namespace of parameter service for this agent's coordinator
+	constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
+
+	// Inform the user of what namespaces are being used
+	ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] m_namespace_to_own_agent_parameter_service    =  " << m_namespace_to_own_agent_parameter_service);
+	ROS_INFO_STREAM("[CRAZY RADIO EMULATOR] 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);
+
+
+
+	// SUBSCRIBE TO "YAML PARAMTERS READY" MESSAGES
+
+	// The parameter service publishes 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, isReadyBatteryMonitorYamlCallback);
+	ros::Subscriber batteryMonitor_yamlReady_fromCoord = nodeHandle_to_coordinator_parameter_service.subscribe("BatteryMonitor", 1, isReadyBatteryMonitorYamlCallback);
+
+
+	// FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
+
+	// Call the class function that loads the parameters for this class.
+	fetchBatteryMonitorYamlParameters(nodeHandle_to_own_agent_parameter_service);
+
+
+
+	// TIMER FOR THE BATTERY VOLTAGE
+	m_timer_battery_voltage_updates = nodeHandle.createTimer(ros::Duration(yaml_battery_polling_period_in_seconds), update_battery_voltage_callback, false);
+	m_timer_battery_voltage_updates.stop();
+
+
+
+
+	// PUBLISHERS
+
+	// Get a node handle to the Flying Agent Client
+	std::string namespace_to_FlyingAgentClient = m_namespace + "/FlyingAgentClient";
+	ros::NodeHandle nodeHandle_to_FlyingAgentClient(namespace_to_FlyingAgentClient);
+
+	// Publisher for the status of the radio connection
+	crazyRadioStatusPublisher = nodeHandle.advertise<std_msgs::Int32>("CrazyRadioStatus",1);
+
+	// Publisher for sending a "Flying Agent Client Command"
+	flyingAgentClientCommandPublisher = nodeHandle_to_FlyingAgentClient.advertise<IntWithHeader>("Command",1);
+
+	// Publisher for the filtered battery voltage
+	batteryVoltagePublisher = nodeHandle.advertise<std_msgs::Float32>("CFBattery",1);
+
+	// Publisher for the control commands
+	// > Note this is not needed for the real CrazyRadio node because
+	//   this command goes out over the radio, but for emulation we publish
+	//   the control command
+	controlCommandPublisher = nodeHandle.advertise<ControlCommand>("ControlCommand",1);
+
+
+	// SUBSCRIBERS
+
+	// Get a node handle to the coordintor
+	std::string namespace_to_coordinator;
+	constructNamespaceForCoordinator( m_coordID, namespace_to_coordinator );
+	ros::NodeHandle nodeHandle_to_coordinator(namespace_to_coordinator);
+
+	// Get a node handle to the Crazy Radio
+	std::string namespace_to_crazyradio = m_namespace + "/CrazyRadio";
+	ros::NodeHandle nodeHandle_to_crazyradio(namespace_to_crazyradio);
+
+	// Subscriber to the commands for when to (dis-)connect the
+	// CrazyRadio connection with the Crazyflie
+	// > For the radio commands from the FlyingAgentClient of this agent
+	ros::Subscriber crazyRadioCommandSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("crazyRadioCommand", 1, crazyRadioCommandCallback);
+	// > For the radio command from the coordinator
+	ros::Subscriber crazyRadioCommandFromCoordSubscriber = nodeHandle_to_coordinator.subscribe("FlyingAgentClient/crazyRadioCommand", 1, crazyRadioCommandCallback);
+	
+	// Subscriber for the "Control Commands" received from the Flying Agent Client
+	ros::Subscriber controlCommandsSubscriber = nodeHandle_to_FlyingAgentClient.subscribe("ControlCommand", 1, controlCommandCallback);
+
+
+
+	// SERVICE SERVER FOR OTHERS TO GET THE CURRENT RADIO STATE
+	// Advertise the service that return the "m_radio_state"
+	// variable when called upon
+	ros::ServiceServer getCurrentRadioStateService = nodeHandle.advertiseService("getCurrentCrazyRadioStatus", getCurrentCrazyRadioStatusServiceCallback);
+
+
+
+	// Inform the user the this node is ready
+	ROS_INFO("[CRAZY RADIO EMULATOR] Ready :-)");
+	// Spin as a single-thread node
+	ros::spin();
+
+	return 0;
+}
\ No newline at end of file