Tuning controller GUI intergrated with ROS. Next step is to update the Tuning...

Tuning controller GUI intergrated with ROS. Next step is to update the Tuning Controller Service Node

pps_ws/src/d_fall_pps/GUI_Qt/flyingAgentGUI/forms/tuningcontrollertab.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1507</width>
+    <width>1519</width>
     <height>970</height>
    </rect>
   </property>
@@ -162,7 +162,7 @@
           <number>12</number>
          </property>
          <item>
-          <widget class="QLineEdit" name="lineEdit_5">
+          <widget class="QLineEdit" name="lineEdit_error_D">
            <property name="sizePolicy">
             <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
              <horstretch>0</horstretch>
@@ -258,7 +258,7 @@
           <number>12</number>
          </property>
          <item>
-          <widget class="QLineEdit" name="lineEdit">
+          <widget class="QLineEdit" name="lineEdit_gain_P">
            <property name="sizePolicy">
             <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
              <horstretch>0</horstretch>
@@ -318,7 +318,7 @@
           <number>12</number>
          </property>
          <item>
-          <widget class="QLineEdit" name="lineEdit_2">
+          <widget class="QLineEdit" name="lineEdit_angle">
            <property name="sizePolicy">
             <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
              <horstretch>0</horstretch>
@@ -490,7 +490,7 @@
           <number>12</number>
          </property>
          <item>
-          <widget class="QLineEdit" name="lineEdit_4">
+          <widget class="QLineEdit" name="lineEdit_gain_D">
            <property name="sizePolicy">
             <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
              <horstretch>0</horstretch>
@@ -550,7 +550,7 @@
           <number>12</number>
          </property>
          <item>
-          <widget class="QLineEdit" name="lineEdit_3">
+          <widget class="QLineEdit" name="lineEdit_error_P">
            <property name="sizePolicy">
             <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
              <horstretch>0</horstretch>
@@ -773,6 +773,23 @@
          </item>
         </layout>
        </item>
+       <item row="3" column="1">
+        <layout class="QHBoxLayout" name="horizontalLayout_15">
+         <property name="leftMargin">
+          <number>12</number>
+         </property>
+         <property name="rightMargin">
+          <number>12</number>
+         </property>
+         <item>
+          <widget class="QLabel" name="label_22">
+           <property name="text">
+            <string>=</string>
+           </property>
+          </widget>
+         </item>
+        </layout>
+       </item>
       </layout>
      </item>
      <item row="11" column="1">

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

@@ -2,6 +2,64 @@
 #define TUNINGCONTROLLERTAB_H
 
 #include <QWidget>
+#include <QMutex>
+#include <QVector>
+#include <QLineEdit>
+#include <QTextStream>
+
+#ifdef CATKIN_MAKE
+#include <ros/ros.h>
+#include <ros/network.h>
+#include <ros/package.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 "d_fall_pps/IntWithHeader.h"
+#include "d_fall_pps/FloatWithHeader.h"
+#include "d_fall_pps/SetpointWithHeader.h"
+#include "d_fall_pps/CustomButtonWithHeader.h"
+
+// Include the DFALL service types
+#include "d_fall_pps/GetSetpointService.h"
+
+// Include the shared definitions
+#include "nodes/Constants.h"
+
+// SPECIFY THE PACKAGE NAMESPACE
+//using namespace d_fall_pps;
+
+#else
+// Include the shared definitions
+#include "include/Constants_for_Qt_compile.h"
+
+#endif
+
+
+#define P_GAIN_MIN_GUI  1
+#define P_GAIN_MAX_GUI  10
+#define P_TO_D_GAIN_RATIO_GUI 0.4
+
+#define DECIMAL_PLACES_POSITION_MEASURED 3
+#define DECIMAL_PLACES_ANGLE_DEGREES 1
+#define DECIMAL_PLACES_VELOCITY 2
+#define DECIMAL_PLACES_GAIN 2
+
+#define DECIMAL_PLACES_SETPOINT 2
+
+#define SETPOINT_X_MINUS -0.25
+#define SETPOINT_X_PLUS   0.25
+
+#define SETPOINT_Y  0.0
+#define SETPOINT_Z  0.4
+#define SETPOINT_YAW_DEGREES  0.0
+
+#define MEASUREMENT_FRQUENCY 200.0
+
+
 
 namespace Ui {
 class TuningControllerTab;
@@ -15,6 +73,15 @@ public:
     explicit TuningControllerTab(QWidget *parent = 0);
     ~TuningControllerTab();
 
+
+
+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_button_setpoint_toggle_clicked();
 
@@ -22,8 +89,80 @@ private slots:
 
     void on_slider_gain_P_valueChanged(int value);
 
+
+
 private:
     Ui::TuningControllerTab *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;
+
+
+
+        float m_gain_P = 0.0;
+        float m_gain_D = 0.0;
+
+        float m_current_setpoint = 0.0;
+
+        QMutex m_gain_setpoint_mutex;
+
+        float m_x_previous = 0.0;
+
+
+
+    #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 the P gain is changed
+        ros::Publisher requestNewGainChangePublisher;
+
+        // 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 fillFloatMessageHeader( d_fall_pps::FloatWithHeader & msg );
+
+        // Get the paramters that specify the type and ID
+        bool getTypeAndIDParameters();
+    #endif
+
+        void publishSetpoint(float x, float y, float z, float yaw);
+
+        void publishGain(float new_gain);
+
 };
 
 #endif // TUNINGCONTROLLERTAB_H