diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index 9b97c231c1b14cee7e717e8400cb5f89c77be974..da7483259ebb2492dba7cef0224c216197dcdcde 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/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})
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/main.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/main.cpp
index 4d01be0bdd2687811a3f6bf504493d159312f763..d73d9e4824e89defe9996c325ec6dc8e5fd7deb9 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/main.cpp
+++ b/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();
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
new file mode 100644
index 0000000000000000000000000000000000000000..03905238f5f1c6bcf7e760b8b76832e56306616f
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/include/nodes/CrazyRadioEmulator.h
@@ -0,0 +1,202 @@
+// 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
diff --git a/dfall_ws/src/dfall_pkg/launch/agent.launch b/dfall_ws/src/dfall_pkg/launch/agent.launch
index a6e5c9f35321dc65d656cb15ea97f5e1d6de9c32..0815543d28bece2e63ddf5929e93f3a6126fc254 100755
--- a/dfall_ws/src/dfall_pkg/launch/agent.launch
+++ b/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
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp b/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
index 80ddc1daaf98098898cd7e538f82cb46e50215ab..bf0af4453af893b840ef497f19ff3a472912587e 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/BatteryMonitor.cpp
+++ b/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);
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..19fb02d244743c1355839469b0e3402f2bcb3161
--- /dev/null
+++ b/dfall_ws/src/dfall_pkg/src/nodes/CrazyRadioEmulator.cpp
@@ -0,0 +1,559 @@
+// 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