Added the custom button support to the new GUI, and added a get setpoint...

Added the custom button support to the new GUI, and added a get setpoint service. The Default and Student controller tabs are now fully integrated for the agent and coordinator mode. Needs testing.

pps_ws/src/d_fall_pps/CMakeLists.txt

@@ -231,6 +231,7 @@ add_service_files(
   CMUpdate.srv
   CMCommand.srv
   LoadYamlFromFilename.srv
+  GetSetpointService.srv
 )
 
 ## Generate actions in the 'action' folder

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

@@ -46,6 +46,7 @@ public:
     ~ControllerTabs();
 
     // PUBLIC METHODS FOR TOGGLING THE VISISBLE CONTROLLERS
+    void showHideController_toggle(QString qstr_label, QWidget * tab_widget_to_toggle);
     void showHideController_default_changed();
     void showHideController_student_changed();
     void showHideController_picker_changed();

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

@@ -20,6 +20,9 @@
 //#include "d_fall_pps/IntWithHeader.h"
 #include "d_fall_pps/SetpointWithHeader.h"
 
+// Include the DFALL service types
+#include "d_fall_pps/GetSetpointService.h"
+
 // Include the shared definitions
 #include "nodes/Constants.h"
 
@@ -32,6 +35,8 @@
 
 #endif
 
+
+
 namespace Ui {
 class DefaultControllerTab;
 }
@@ -45,12 +50,14 @@ public:
     ~DefaultControllerTab();
 
 
+
 public slots:
     void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
     void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
     void poseDataUnavailableSlot();
 
 
+
 private slots:
     void on_lineEdit_setpoint_new_x_returnPressed();
     void on_lineEdit_setpoint_new_y_returnPressed();
@@ -70,6 +77,8 @@ private slots:
     void on_yaw_increment_plus_button_clicked();
     void on_yaw_increment_minus_button_clicked();
 
+
+
 private:
     Ui::DefaultControllerTab *ui;
 
@@ -101,6 +110,7 @@ private:
 #endif
 
 
+
     // --------------------------------------------------- //
     // PRIVATE FUNCTIONS
 

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

@@ -2,6 +2,7 @@
 #define STUDENTCONTROLLERTAB_H
 
 #include <QWidget>
+#include <QMutex>
 #include <QVector>
 #include <QTextStream>
 
@@ -18,6 +19,10 @@
 // Include the DFALL message types
 //#include "d_fall_pps/IntWithHeader.h"
 #include "d_fall_pps/SetpointWithHeader.h"
+#include "d_fall_pps/CustomButtonWithHeader"
+
+// Include the DFALL service types
+#include "d_fall_pps/GetSetpointService.h"
 
 // Include the shared definitions
 #include "nodes/Constants.h"
@@ -27,11 +32,12 @@
 
 #else
 // Include the shared definitions
-//#include "include/Constants_for_Qt_compile.h"
+#include "include/Constants_for_Qt_compile.h"
 
 #endif
 
 
+
 namespace Ui {
 class StudentControllerTab;
 }
@@ -45,12 +51,98 @@ public:
     ~StudentControllerTab();
 
 
+
 public slots:
+    void setAgentIDsToCoordinate(QVector<int> agentIDs , bool shouldCoordinateAll);
     void setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded);
+    void poseDataUnavailableSlot();
+
+
+
+private slots:
+    void on_lineEdit_setpoint_new_x_returnPressed();
+    void on_lineEdit_setpoint_new_y_returnPressed();
+    void on_lineEdit_setpoint_new_z_returnPressed();
+    void on_lineEdit_setpoint_new_yaw_returnPressed();
+
+    void on_set_setpoint_button_clicked();
+
+    void on_default_setpoint_button_clicked();
+
+    void on_x_increment_plus_button_clicked();
+    void on_x_increment_minus_button_clicked();
+    void on_y_increment_plus_button_clicked();
+    void on_y_increment_minus_button_clicked();
+    void on_z_increment_plus_button_clicked();
+    void on_z_increment_minus_button_clicked();
+    void on_yaw_increment_plus_button_clicked();
+    void on_yaw_increment_minus_button_clicked();
+
+    void on_custom_button_1_clicked();
+    void on_custom_button_2_clicked();
+    void on_custom_button_3_clicked();
+    void on_custom_button_4_clicked();
+    void on_custom_button_5_clicked();
+
 
 
 private:
     Ui::StudentControllerTab *ui;
+
+    // --------------------------------------------------- //
+    // PRIVATE VARIABLES
+
+    // The type of this node, i.e., agent or a coordinator,
+    // specified as a parameter in the "*.launch" file
+    int m_type = 0;
+
+    // The ID  of this node
+    int m_ID;
+
+    // For coordinating multiple agents
+    std::vector<int> m_vector_of_agentIDs_toCoordinate;
+    bool m_shouldCoordinateAll = true;
+    QMutex m_agentIDs_toCoordinate_mutex;
+
+
+
+#ifdef CATKIN_MAKE
+    // PUBLISHER
+    // > For requesting the setpoint to be changed
+    ros::Publisher requestSetpointChangePublisher;
+
+    // SUBSCRIBER
+    // > For being notified when the setpoint is changed
+    ros::Subscriber setpointChangedSubscriber;
+
+    // PUBLISHER
+    // > For notifying that a custom button is pressed
+    ros::Publisher customButtonPublisher;
+#endif
+
+
+
+    // --------------------------------------------------- //
+    // PRIVATE FUNCTIONS
+
+
+#ifdef CATKIN_MAKE
+    // For receiving message that the setpoint was changed
+    void setpointChangedCallback(const d_fall_pps::SetpointWithHeader& newSetpoint);
+
+    // Publish a message when a custom button is pressed
+    void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
+
+    // Fill the header for a message
+    void fillSetpointMessageHeader( d_fall_pps::SetpointWithHeader & msg );
+    void fillCustomButtonMessageHeader( d_fall_pps::CustomButtonWithHeader & msg );
+
+    // Get the paramters that specify the type and ID
+    bool getTypeAndIDParameters();
+#endif
+
+    void publishSetpoint(float x, float y, float z, float yaw);
+
 };
 
 #endif // STUDENTCONTROLLERTAB_H

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

@@ -131,6 +131,9 @@ private:
 
     // PUBLISHERS AND SUBSRIBERS
 
+    // > For the emergency stop button
+    ros::Publisher emergencyStopPublisher;
+
     // > For changes in the database that defines {agentID,cfID,flying zone} links
     ros::Subscriber databaseChangedSubscriber;
     ros::ServiceClient centralManagerDatabaseService;

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

@@ -88,19 +88,17 @@ ControllerTabs::~ControllerTabs()
 
 
 
-
-
-void ControllerTabs::showHideController_default_changed()
+void ControllerTabs::showHideController_toggle(QString qstr_label, QWidget * tab_widget_to_toggle)
 {
     // Get the current index of the tab
     // > Note the this returns -1 if the tab is not found
-    int current_index_of_tab = ui->controller_tabs_widget->indexOf(ui->default_tab);
+    int current_index_of_tab = ui->controller_tabs_widget->indexOf(tab_widget_to_toggle);
 
     // Switch depending on whether the tab was found
     if (current_index_of_tab < 0)
     {
         // Insert the tab
-        ui->controller_tabs_widget->addTab(ui->default_tab,"Default");
+        ui->controller_tabs_widget->addTab(tab_widget_to_toggle,qstr_label);
     }
     else
     {
@@ -109,61 +107,26 @@ void ControllerTabs::showHideController_default_changed()
     }
 }
 
-void ControllerTabs::showHideController_student_changed()
+
+
+void ControllerTabs::showHideController_default_changed()
 {
-    // Get the current index of the tab
-    // > Note the this returns -1 if the tab is not found
-    int current_index_of_tab = ui->controller_tabs_widget->indexOf(ui->student_tab);
+    showHideController_toggle("Default",ui->default_tab);
+}
 
-    // Switch depending on whether the tab was found
-    if (current_index_of_tab < 0)
-    {
-        // Insert the tab
-        ui->controller_tabs_widget->addTab(ui->student_tab,"Student");
-    }
-    else
-    {
-        // Remove the tab
-        ui->controller_tabs_widget->removeTab(current_index_of_tab);
-    }
+void ControllerTabs::showHideController_student_changed()
+{
+    showHideController_toggle("Student",ui->student_tab);
 }
 
 void ControllerTabs::showHideController_picker_changed()
 {
-    // Get the current index of the tab
-    // > Note the this returns -1 if the tab is not found
-    int current_index_of_tab = ui->controller_tabs_widget->indexOf(ui->picker_tab);
-
-    // Switch depending on whether the tab was found
-    if (current_index_of_tab < 0)
-    {
-        // Insert the tab
-        ui->controller_tabs_widget->addTab(ui->picker_tab,"Picker");
-    }
-    else
-    {
-        // Remove the tab
-        ui->controller_tabs_widget->removeTab(current_index_of_tab);
-    }
+    showHideController_toggle("Picker",ui->picker_tab);
 }
 
 void ControllerTabs::showHideController_safe_changed()
 {
-    // Get the current index of the tab
-    // > Note the this returns -1 if the tab is not found
-    int current_index_of_tab = ui->controller_tabs_widget->indexOf(ui->safe_tab);
-
-    // Switch depending on whether the tab was found
-    if (current_index_of_tab < 0)
-    {
-        // Insert the tab
-        ui->controller_tabs_widget->addTab(ui->safe_tab,"Safe");
-    }
-    else
-    {
-        // Remove the tab
-        ui->controller_tabs_widget->removeTab(current_index_of_tab);
-    }
+    showHideController_toggle("Safe",ui->safe_tab);
 }
 
 

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

@@ -56,6 +56,18 @@ DefaultControllerTab::~DefaultControllerTab()
 }
 
 
+
+
+
+//    ----------------------------------------------------------------------------------
+//    PPPP    OOO    SSSS  EEEEE     DDDD     A    TTTTT    A
+//    P   P  O   O  S      E         D   D   A A     T     A A
+//    PPPP   O   O   SSS   EEE       D   D  A   A    T    A   A
+//    P      O   O      S  E         D   D  AAAAA    T    AAAAA
+//    P       OOO   SSSS   EEEEE     DDDD   A   A    T    A   A
+//    ----------------------------------------------------------------------------------
+
+
 void DefaultControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
 {
     if (!occluded)
@@ -264,7 +276,7 @@ void DefaultControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
 
 void DefaultControllerTab::on_set_setpoint_button_clicked()
 {
-#ifdef CATKIN_MAKE
+
     // Initialise local variable for each of (x,y,z,yaw)
     float x = 0.0f, y = 0.0f, z = 0.0f, yaw = 0.0f;
 
@@ -290,9 +302,9 @@ void DefaultControllerTab::on_set_setpoint_button_clicked()
     else
         yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
 
+#ifdef CATKIN_MAKE
     // Call the function to publish the setpoint
     publishSetpoint(x,y,z,yaw);
-
 #else
     // TO ASSIST WITH DEBUGGING WHEN COMPILED AND RUN IN "QtCreator"
     QTextStream(stdout) << "[DEFAULT CONTROLLER TAB] set setpoint button clicked";
@@ -324,7 +336,6 @@ void DefaultControllerTab::on_default_setpoint_button_clicked()
 
 void DefaultControllerTab::on_x_increment_plus_button_clicked()
 {
-#ifdef CATKIN_MAKE
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
     {
@@ -338,16 +349,17 @@ void DefaultControllerTab::on_x_increment_plus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
+        #endif
 
     }
-#endif
 }
 
 void DefaultControllerTab::on_x_increment_minus_button_clicked()
 {
-#ifdef CATKIN_MAKE
+
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_x->text().isEmpty())
     {
@@ -361,16 +373,15 @@ void DefaultControllerTab::on_x_increment_minus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
-
+        #endif
     }
-#endif
 }
 
 void DefaultControllerTab::on_y_increment_plus_button_clicked()
 {
-#ifdef CATKIN_MAKE
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
     {
@@ -384,16 +395,15 @@ void DefaultControllerTab::on_y_increment_plus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
-
+        #endif
     }
-#endif
 }
 
 void DefaultControllerTab::on_y_increment_minus_button_clicked()
 {
-#ifdef CATKIN_MAKE
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_y->text().isEmpty())
     {
@@ -407,16 +417,15 @@ void DefaultControllerTab::on_y_increment_minus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
-
+        #endif
     }
-#endif
 }
 
 void DefaultControllerTab::on_z_increment_plus_button_clicked()
 {
-#ifdef CATKIN_MAKE
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
     {
@@ -430,16 +439,15 @@ void DefaultControllerTab::on_z_increment_plus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
-
+        #endif
     }
-#endif
 }
 
 void DefaultControllerTab::on_z_increment_minus_button_clicked()
 {
-#ifdef CATKIN_MAKE
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_z->text().isEmpty())
     {
@@ -453,16 +461,15 @@ void DefaultControllerTab::on_z_increment_minus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
-
+        #endif
     }
-#endif
 }
 
 void DefaultControllerTab::on_yaw_increment_plus_button_clicked()
 {
-#ifdef CATKIN_MAKE
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
     {
@@ -476,15 +483,14 @@ void DefaultControllerTab::on_yaw_increment_plus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
-
+        #endif
     }
-#endif
 }
 void DefaultControllerTab::on_yaw_increment_minus_button_clicked()
 {
-#ifdef CATKIN_MAKE
     // Only need to do something if the field is not empty
     if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
     {
@@ -498,11 +504,11 @@ void DefaultControllerTab::on_yaw_increment_minus_button_clicked()
     }
     else
     {
+        #ifdef CATKIN_MAKE
         // Inform the user that nothing can be done
         ROS_INFO_STREAM("[DEFAULT CONTROLLER GUI] Increment x setpoint clicked but field is empty");
-
+        #endif
     }
-#endif
 }
 
 
@@ -549,34 +555,20 @@ void DefaultControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool
         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);
-        // }
-
+        // // > Request the current setpoint
+        ros::ServiceClient getCurrentSetpointServiceClient = agent_base_nodeHandle.serviceClient<d_fall_pps::IntIntService>("DefaultControllerService/GetCurrentSetpoint", false);
+        d_fall_pps::GetSetpointService getSetpointCall;
+        getSetpointCall.request.data = 0;
+        getCurrentSetpointServiceClient.waitForExistence(ros::Duration(2.0));
+        if(getCurrentSetpointServiceClient.call(getSetpointCall))
+        {
+            setpointChangedCallback(getSetpointCall.response.setpointWithHeader);
+        }
+        else
+        {
+            // Inform the user
+            ROS_INFO("[DEFAULT CONTROLLER GUI] Failed to get setpoint from controller using the \"GetCurrentSetpoint\" service");
+        }
 
         // SUBSCRIBERS
         // > For receiving message that the setpoint was changed
@@ -753,4 +745,4 @@ bool DefaultControllerTab::getTypeAndIDParameters()
     // Return
     return return_was_successful;
 }
-#endif
+#endif
\ No newline at end of file

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

@@ -132,6 +132,18 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
             ui->customWidget_controller_tabs , &ControllerTabs::setObjectNameForDisplayingPoseData
             );
 
+
+    // TOGGLE THE CONTROLLERS THAT ARE VISIBLE
+    // By default all controller buttons and tabs are visible
+    // and the menu item is checked. Hence, to hide a controller
+    // the menu item simply needs to be "triggered"
+
+    // > For the picker controller
+    ui->action_showHideController_picker->trigger();
+    // > For the safe controller
+    ui->action_showHideController_safe->trigger();
+
+
 }
 
 MainWindow::~MainWindow()

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

@@ -92,6 +92,9 @@ TopBanner::TopBanner(QWidget *parent) :
     // CREATE A NODE HANDLE TO THE ROOT OF THE D-FaLL SYSTEM
     ros::NodeHandle dfall_root_nodeHandle("/dfall");
 
+    // > Publisher for the emergency stop button
+    emergencyStopPublisher = dfall_root_nodeHandle.advertise<d_fall_pps::IntWithHeader>("emergencyStop", 1);
+
 	// > For changes in the database that defines {agentID,cfID,flying zone} links
 	databaseChangedSubscriber = dfall_root_nodeHandle.subscribe("CentralManagerService/DBChanged", 1, &TopBanner::databaseChangedCallback, this);;
 	centralManagerDatabaseService = dfall_root_nodeHandle.serviceClient<d_fall_pps::CMQuery>("CentralManagerService/Query", false);
@@ -245,10 +248,10 @@ void TopBanner::loadCrazyflieContext(int ID_to_request_from_database , int emit_
 void TopBanner::on_emergency_stop_button_clicked()
 {
 #ifdef CATKIN_MAKE
-    // d_fall_pps::IntWithHeader msg;
-    // fillIntMessageHeader(msg);
-    // msg.data = CMD_RECONNECT;
-    // this->crazyRadioCommandPublisher.publish(msg);
+    d_fall_pps::IntWithHeader msg;
+    msg.shouldCheckIDs = false;
+    msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+    this->emergencyStopPublisher.publish(msg);
     ROS_INFO("[TOP BANNER GUI] EMERGENCY STOP button clicked");
 #endif
 }

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

@@ -57,17 +57,18 @@
 
 // Include the DFALL message types