The coordinator row and (dis-)connect, start-stop bar are now fully connected...

The coordinator row and (dis-)connect,  start-stop bar are now fully connected and fully generalised for operating as both an agent and a coordinator GUI

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -224,6 +224,7 @@ add_message_files(
 # )
 add_service_files(
   FILES
+  IntIntService.srv
   Controller.srv
   CMRead.srv
   CMQuery.srv

pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/flyingagentgui.qrc

@@ -16,6 +16,6 @@
         <file>images/flying_state_flying.png</file>
         <file>images/flying_state_off.png</file>
         <file>images/flying_state_unknown.png</file>
-        <file>images/flying_state_unavilable.png</file>
+        <file>images/flying_state_unavailable.png</file>
     </qresource>
 </RCC>
\ No newline at end of file

pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/Constants_for_Qt_compile.h

@@ -79,10 +79,11 @@
 #define CMD_CRAZYFLY_MOTORS_OFF      13
 
 // Flying states
-#define STATE_MOTORS_OFF 1
-#define STATE_TAKE_OFF   2
-#define STATE_FLYING     3
-#define STATE_LAND       4
+#define STATE_MOTORS_OFF     1
+#define STATE_TAKE_OFF       2
+#define STATE_FLYING         3
+#define STATE_LAND           4
+#define STATE_UNAVAILABLE    5
 
 
 // Commands for CrazyRadio

pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/connectstartstopbar.h

@@ -53,6 +53,7 @@
 #include "d_fall_pps/IntWithHeader.h"
 #include "d_fall_pps/AreaBounds.h"
 #include "d_fall_pps/CrazyflieContext.h"
+#include "d_fall_pps/IntIntService.h"
 #include "d_fall_pps/CMQuery.h"
 
 // Include the shared definitions

pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/include/coordinatorrow.h

@@ -52,6 +52,7 @@
 #include "d_fall_pps/AreaBounds.h"
 #include "d_fall_pps/CrazyflieContext.h"
 #include "d_fall_pps/CMQuery.h"
+#include "d_fall_pps/IntIntService.h"
 
 // Include the shared definitions
 #include "nodes/Constants.h"
@@ -155,6 +156,14 @@ private:
     //   i.e., the {agentID,cfID,flying zone} tuple
     void loadCrazyflieContext();
 
+    // > For requesting the current flying state
+    //   i.e., using the service advertised by the PPS client
+    void getCurrentFlyingState();
+
+    // > For requesting the current state of the Crazy Radio
+    //   i.e., using the service advertised by the PPS client
+    void getCurrentCrazyRadioState();
+
     // > For updating the text in the UI element of
     //   "controller_enabled_label"
     void setControllerEnabled(int new_controller);
@@ -192,6 +201,14 @@ private:
     // > For updating the controller that is currently operating
     ros::Subscriber controllerUsedSubscriber;
 
+    // > For requesting the current flying state,
+    //   this is used only for initialising the icon
+    ros::ServiceClient getCurrentFlyingStateService;
+
+    // > For requesting the current state of the Crazy Radio,
+    //   this is used only for initialising the icon
+    ros::ServiceClient getCurrentCrazyRadioStateService;
+
 
     // --------------------------------------------------- //
     // PRIVATE CALLBACKS IN RESPONSE TO ROS MESSAGES

pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/connectstartstopbar.cpp

@@ -86,7 +86,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
     setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
 
     // SET THE STARTING FLYING STATE STATUS TO BE: MOTORS OFF
-    setFlyingState(CMD_CRAZYFLY_MOTORS_OFF);
+    setFlyingState(STATE_UNAVAILABLE);
 
     // ENSURE THE F:YING STATE BUTTONS ARE AVAILABLE FOR A COORDINATOR
     if (m_type == TYPE_COORDINATOR)
@@ -101,7 +101,7 @@ ConnectStartStopBar::ConnectStartStopBar(QWidget *parent) :
     ros::NodeHandle base_nodeHandle(base_namespace);
 
     // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
-    ros::NodeHandle dfall_root_nodeHandle("/dfall");
+    //ros::NodeHandle dfall_root_nodeHandle("/dfall");
 
     // SUBSCRIBERS AND PUBLISHERS:
 
@@ -481,6 +481,15 @@ void ConnectStartStopBar::setFlyingState(int new_flying_state)
             break;
         }
 
+        case STATE_UNAVAILABLE:
+        {
+            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+            QPixmap flying_state_disabling_pixmap(":/images/flying_state_unavailable.png");
+            ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
+            ui->flying_state_label->setScaledContents(true);
+            break;
+        }
+
         default:
         {
             // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
@@ -593,6 +602,79 @@ void ConnectStartStopBar::setAgentIDsToCoordinate(QVector<int> agentIDs , bool s
     // Unlock the mutex
     m_agentIDs_toCoordinate_mutex.unlock();
 
+
+#ifdef CATKIN_MAKE
+    // If there is only one agent to coordinate,
+    // then subscribe to the relevant data
+    if (agentIDs.length() == 1)
+    {
+
+        // > Create the appropriate node handle
+        QString agent_base_namespace = "/dfall/agent" + QString::number(agentIDs[0]).rightJustified(3, '0');
+        ros::NodeHandle agent_base_nodeHandle(agent_base_namespace.toStdString());
+
+        // > Request the current flying state
+        ros::ServiceClient getCurrentFlyingStateService = agent_base_nodeHandle.serviceClient<d_fall_pps::IntIntService>("PPSClient/getCurrentFlyingState", false);
+        d_fall_pps::IntIntService getFlyingStateCall;
+        getFlyingStateCall.request.data = 0;
+        getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0));
+        if(getCurrentFlyingStateService.call(getFlyingStateCall))
+        {
+            setFlyingState(getFlyingStateCall.response.data);
+        }
+        else
+        {
+            setFlyingState(STATE_UNAVAILABLE);
+        }
+
+        // > Request the current status of the crazy radio
+        ros::ServiceClient getCurrentCrazyRadioStateService = agent_base_nodeHandle.serviceClient<d_fall_pps::IntIntService>("CrazyRadio/getCurrentCrazyRadioStatus", false);
+        d_fall_pps::IntIntService getCrazyRadioCall;
+        getCrazyRadioCall.request.data = 0;
+        getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0));
+        if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall))
+        {
+            setCrazyRadioStatus(getCrazyRadioCall.response.data);
+        }
+        else
+        {
+            setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+        }
+
+        // > For updating the "rf_status_label" picture
+        crazyRadioStatusSubscriber = agent_base_nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &ConnectStartStopBar::crazyRadioStatusCallback, this);
+
+        // > For updating the current battery voltage
+        batteryVoltageSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/FilteredVoltage", 1, &ConnectStartStopBar::batteryVoltageCallback, this);
+
+        // > For updating the current battery state
+        //batteryStateSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/ChangedStateTo", 1, &ConnectStartStopBar::batteryStateChangedCallback, this);
+
+        // > For updating the current battery level
+        batteryLevelSubscriber = agent_base_nodeHandle.subscribe("BatteryMonitor/Level", 1, &ConnectStartStopBar::batteryLevelCallback, this);
+
+        // > For updating the "flying_state_label" picture
+        flyingStateSubscriber = agent_base_nodeHandle.subscribe("PPSClient/flyingState", 1, &ConnectStartStopBar::flyingStateChangedCallback, this);
+    }
+    else
+    {
+        // Unsubscribe
+        crazyRadioStatusSubscriber.shutdown();
+        batteryVoltageSubscriber.shutdown();
+        //batteryStateSubscriber.shutdown();
+        batteryLevelSubscriber.shutdown();
+        flyingStateSubscriber.shutdown();
+
+        // Set information back to the default
+        setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+        setBatteryVoltageText(-1.0f);
+        setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
+        setFlyingState(STATE_UNAVAILABLE);
+
+    }
+#endif
+
+
 #ifdef CATKIN_MAKE
 #else
     // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"

pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/src/coordinatorrow.cpp

@@ -83,7 +83,7 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
     setBatteryImageBasedOnLevel(BATTERY_LEVEL_UNAVAILABLE);
     
     // SET THE STARTING FLYING STATE STATUS TO BE: MOTORS OFF
-    setFlyingState(CMD_CRAZYFLY_MOTORS_OFF);
+    setFlyingState(STATE_UNAVAILABLE);
     
     // SET THE DEFAULT NAME FOR THE SELECTED CONTROLLER
     setControllerEnabled(SAFE_CONTROLLER);
@@ -130,12 +130,26 @@ CoordinatorRow::CoordinatorRow(QWidget *parent, int agentID) :
     // > For updating the controller that is currently operating
     controllerUsedSubscriber = base_nodeHandle.subscribe("PPSClient/controllerUsed", 1, &CoordinatorRow::controllerUsedChangedCallback, this);
 
+    // > For requesting the current flying state,
+    //   this is used only for initialising the icon
+    getCurrentFlyingStateService = base_nodeHandle.serviceClient<d_fall_pps::IntIntService>("PPSClient/getCurrentFlyingState", false);
+
+    // > For requesting the current state of the Crazy Radio,
+    //   this is used only for initialising the icon
+    getCurrentCrazyRadioStateService = base_nodeHandle.serviceClient<d_fall_pps::IntIntService>("CrazyRadio/getCurrentCrazyRadioStatus", false);
+
 #endif
 
     // FURTHER INITILIASATIONS NEED TO OCCUR AFTER THE ROS RELATED
     // INITIALISATIONS ARE COMPLETE
     loadCrazyflieContext();
 
+    // > Request the current flying state
+    getCurrentFlyingState();
+
+    // > Request the current state of the Crazy Radio
+    getCurrentCrazyRadioState();
+
 }
 
 CoordinatorRow::~CoordinatorRow()
@@ -541,6 +555,15 @@ void CoordinatorRow::setFlyingState(int new_flying_state)
             break;
         }
 
+        case STATE_UNAVAILABLE:
+        {
+            // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
+            QPixmap flying_state_disabling_pixmap(":/images/flying_state_unavailable.png");
+            ui->flying_state_label->setPixmap(flying_state_disabling_pixmap);
+            ui->flying_state_label->setScaledContents(true);
+            break;
+        }
+
         default:
         {
             // SET THE APPROPRIATE IMAGE FOR THE FLYING STATE LABEL
@@ -619,6 +642,41 @@ void CoordinatorRow::loadCrazyflieContext()
 
 
 
+void CoordinatorRow::getCurrentFlyingState()
+{
+    d_fall_pps::IntIntService getFlyingStateCall;
+    getFlyingStateCall.request.data = 0;
+    getCurrentFlyingStateService.waitForExistence(ros::Duration(2.0));
+    if(getCurrentFlyingStateService.call(getFlyingStateCall))
+    {
+        setFlyingState(getFlyingStateCall.response.data);
+    }
+    else
+    {
+        setFlyingState(STATE_UNAVAILABLE);
+    }
+}
+
+
+
+
+void CoordinatorRow::getCurrentCrazyRadioState()
+{
+    d_fall_pps::IntIntService getCrazyRadioCall;
+    getCrazyRadioCall.request.data = 0;
+    getCurrentCrazyRadioStateService.waitForExistence(ros::Duration(2.0));
+    if(getCurrentCrazyRadioStateService.call(getCrazyRadioCall))
+    {
+        setCrazyRadioStatus(getCrazyRadioCall.response.data);
+    }
+    else
+    {
+        setCrazyRadioStatus(CRAZY_RADIO_STATE_DISCONNECTED);
+    }
+}
+
+
+
 
 //    ----------------------------------------------------------------------------------
 //     CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR

pps_ws/src/d_fall_pps/crazyradio/CrazyRadio.py

@@ -38,6 +38,7 @@ import rospy
 from std_msgs.msg import Int32
 from d_fall_pps.msg import ControlCommand
 from d_fall_pps.msg import IntWithHeader
+from d_fall_pps.srv import IntIntService
 
 
 # General import
@@ -333,6 +334,16 @@ class PPSRadioClient:
                     print "[CRAZY RADIO] received command to DISCONNECT (current status is DISCONNECTED)"
                     #self.status_pub.publish(DISCONNECTED)
 
+
+
+    def getCurrentCrazyRadioStatusServiceCallback(self, req):
+        """Callback to service the request for the connection status"""
+        # Directly return the current status
+        return self._status
+
+
+
+
 def controlCommandCallback(data):
     """Callback for controller actions"""
     #rospy.loginfo("controller callback : %s, %s, %s", data.roll, data.pitch, data.yaw)
@@ -352,6 +363,8 @@ def controlCommandCallback(data):
 
 
 
+
+
 if __name__ == '__main__':
 
     # Starting the ROS-node
@@ -406,6 +419,12 @@ if __name__ == '__main__':
     rospy.Subscriber("/dfall/coord" + coordID_as_string + "/PPSClient/crazyRadioCommand", IntWithHeader, cf_client.crazyRadioCommandCallback)
 
 
+    # Advertise a Serice for the current status
+    # of the Crazy Radio connect
+    s = rospy.Service(node_name + "/getCurrentCrazyRadioStatus", IntIntService , cf_client.getCurrentCrazyRadioStatusServiceCallback)
+
+
+
     time.sleep(1.0)
 
     rospy.Subscriber("PPSClient/ControlCommand", ControlCommand, controlCommandCallback)

pps_ws/src/d_fall_pps/include/nodes/Constants.h

@@ -125,10 +125,11 @@
 #define CMD_CRAZYFLY_MOTORS_OFF      13
 
 // Flying states
-#define STATE_MOTORS_OFF 1
-#define STATE_TAKE_OFF   2
-#define STATE_FLYING     3
-#define STATE_LAND       4
+#define STATE_MOTORS_OFF     1
+#define STATE_TAKE_OFF       2
+#define STATE_FLYING         3
+#define STATE_LAND           4
+#define STATE_UNAVAILABLE    5
 
 
 // Commands for CrazyRadio

pps_ws/src/d_fall_pps/include/nodes/PPSClient.h

@@ -62,6 +62,7 @@
 // Include the DFALL service types
 #include "d_fall_pps/Controller.h"
 #include "d_fall_pps/CMQuery.h"
+#include "d_fall_pps/IntIntService.h"
 
 // Include the shared definitions
 #include "nodes/Constants.h"
@@ -301,4 +302,8 @@ int getControllerUsed();
 //void setBatteryStateTo(int new_battery_state);
 //float movingAverageBatteryFilter(float new_input);
 //void CFBatteryCallback(const std_msgs::Float32& msg);
-void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);
\ No newline at end of file
+void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);
+
+
+// > For the FLYING STATE
+bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response);
\ No newline at end of file

pps_ws/src/d_fall_pps/src/nodes/PPSClient.cpp

@@ -911,6 +911,25 @@ void CFBatteryCallback(const std_msgs::Float32& msg)
 
 
 
+
+
+
+
+bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response)
+{
+    // Put the flying state into the response variable
+    response.data = flying_state;
+    // Return
+    return true;
+}
+
+
+
+
+
+
+
+
 //    ----------------------------------------------------------------------------------
 //    L       OOO     A    DDDD
 //    L      O   O   A A   D   D
@@ -1346,6 +1365,10 @@ int main(int argc, char* argv[])
     setControllerUsed(SAFE_CONTROLLER);
     setInstantController(SAFE_CONTROLLER); //initialize this also, so we notify GUI
 
+
+    // Advertise the service for the current flying state
+    ros::ServiceServer getCurrentFlyingStateService = nodeHandle.advertiseService("getCurrentFlyingState", getCurrentFlyingStateServiceCallback);
+
     
 
     // Open a ROS "bag" to store data for post-analysis

pps_ws/src/d_fall_pps/srv/IntIntService.srv

+uint32 data
+---
+uint32 data