Connected the chart to the measured data and checked that it works in the emulation environment

dfall_ws/src/dfall_pkg/CMakeLists.txt

@@ -29,6 +29,7 @@ find_package(Qt5Widgets CONFIG)
 find_package(Qt5Core CONFIG)
 find_package(Qt5Gui CONFIG)
 find_package(Qt5Svg CONFIG)
+find_package(Qt5Charts CONFIG)
 
 
 
@@ -81,9 +82,14 @@ if(Qt5Svg_FOUND)
 else()
 	message(STATUS "NOTE: the Qt5 Svg package was NOT found")
 endif()
+if(Qt5Charts_FOUND)
+	message(STATUS "NOTE: the Qt5 Charts package was found")
+else()
+	message(STATUS "NOTE: the Qt5 Charts package was NOT found")
+endif()
 
 
-if(Qt5Widgets_FOUND AND Qt5Core_FOUND AND Qt5Gui_FOUND AND Qt5Svg_FOUND)
+if(Qt5Widgets_FOUND AND Qt5Core_FOUND AND Qt5Gui_FOUND AND Qt5Svg_FOUND AND Qt5Charts_FOUND)
 	set(Qt5_FOUND TRUE)
 else()
 	set(Qt5_FOUND FALSE)
@@ -572,6 +578,7 @@ if(Qt5_FOUND)
 
   # Flying Agent GUI -- link libraries
   target_link_libraries(FlyingAgentGUI Qt5::Widgets) # GUI -- let FlyingAgentGUI have acesss to Qt stuff
+  target_link_libraries(FlyingAgentGUI Qt5::Charts)
   target_link_libraries(FlyingAgentGUI ${catkin_LIBRARIES})
 endif()
 

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui

@@ -6,7 +6,7 @@
    <rect>
     <x>0</x>
     <y>0</y>
-    <width>1492</width>
+    <width>1494</width>
     <height>1434</height>
    </rect>
   </property>
@@ -733,7 +733,7 @@
       </layout>
      </item>
      <item row="1" column="4">
-      <widget class="QPushButton" name="custom_button_1">
+      <widget class="QPushButton" name="perform_step_button">
        <property name="sizePolicy">
         <sizepolicy hsizetype="Minimum" vsizetype="Fixed">
          <horstretch>0</horstretch>
@@ -741,7 +741,7 @@
         </sizepolicy>
        </property>
        <property name="text">
-        <string>Button 1</string>
+        <string>Perform Step</string>
        </property>
       </widget>
      </item>

dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h

@@ -103,6 +103,8 @@ public slots:
 
 
 private slots:
+    void newDataForPerformingStepAndPlotting(float x);
+
     void on_lineEdit_setpoint_new_x_returnPressed();
     void on_lineEdit_setpoint_new_y_returnPressed();
     void on_lineEdit_setpoint_new_z_returnPressed();
@@ -112,7 +114,8 @@ private slots:
 
     void on_default_setpoint_button_clicked();
 
-    void on_custom_button_1_clicked();
+    void on_perform_step_button_clicked();
+
     void on_custom_button_2_clicked();
     void on_custom_button_3_clicked();
 
@@ -138,7 +141,16 @@ private:
     bool m_shouldCoordinateAll = true;
     QMutex m_agentIDs_toCoordinate_mutex;
 
-    // For plotting data on the chart
+    // FOR PLOTTING DATA ON THE CHART
+    // > Mutex for serialising acess to any charting variable
+    QMutex m_chart_mutex;
+    // > Flag for whether to perform a step
+    bool m_shouldPerformStep = false;
+    // > Flag for whether to store data for plotting
+    bool m_shouldStoreData_for_plotting = false;
+    // > Time (as a float) for the horizontal axis
+    float m_time_for_step = 0.0f;
+    // > Line Series for the data
     QLineSeries *m_lineSeries_for_setpoint_x;
     QLineSeries *m_lineSeries_for_measured_x;
 

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

@@ -70,18 +70,24 @@ CsoneControllerTab::CsoneControllerTab(QWidget *parent) :
     m_lineSeries_for_setpoint_x = new QLineSeries();
     m_lineSeries_for_measured_x = new QLineSeries();
 
-    // DEBUGGING: SET SOME FAKE DATA FOR TESTING FOR A LINE CHART
-    m_lineSeries_for_setpoint_x->append(0, 6);
-    m_lineSeries_for_setpoint_x->append(2, 4);
-    m_lineSeries_for_setpoint_x->append(3, 8);
-    m_lineSeries_for_setpoint_x->append(7, 4);
-    m_lineSeries_for_setpoint_x->append(10, 5);
-    *m_lineSeries_for_setpoint_x << QPointF(11, 1) << QPointF(13, 3) << QPointF(17, 6) << QPointF(18, 3) << QPointF(20, 2);
-
-    // Add the
+//    // DEBUGGING: SET SOME FAKE DATA FOR TESTING FOR A LINE CHART
+//    m_lineSeries_for_setpoint_x->append(0, 6);
+//    m_lineSeries_for_setpoint_x->append(2, 4);
+//    m_lineSeries_for_setpoint_x->append(3, 8);
+//    m_lineSeries_for_setpoint_x->append(7, 4);
+//    m_lineSeries_for_setpoint_x->append(10, 5);
+//    *m_lineSeries_for_setpoint_x << QPointF(11, 1) << QPointF(13, 3) << QPointF(17, 6) << QPointF(18, 3) << QPointF(20, 2);
+
+    // Add the line series to the chart
     ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_setpoint_x);
     ui->chartView_for_x->chart()->addSeries(m_lineSeries_for_measured_x);
+
+    // Set the initial axes limits
     ui->chartView_for_x->chart()->createDefaultAxes();
+    ui->chartView_for_x->chart()->axisX()->setMin(-1.0);
+    ui->chartView_for_x->chart()->axisX()->setMax(10.0);
+    ui->chartView_for_x->chart()->axisY()->setMin(-0.6);
+    ui->chartView_for_x->chart()->axisY()->setMax( 0.6);
 
 
 
@@ -196,12 +202,30 @@ void CsoneControllerTab::publish_custom_button_command(int button_index , QLineE
 #endif
 
 
-void CsoneControllerTab::on_custom_button_1_clicked()
+void CsoneControllerTab::on_perform_step_button_clicked()
 {
-    m_lineSeries_for_setpoint_x->append(10, 6);
-    //ui->chartView_for_x->chart()->createDefaultAxes();
+    // Lock the mutex
+    m_chart_mutex.lock();
+
+    // Set the flag that a step should be performed
+    m_shouldPerformStep = true;
+
+    // Set the time back to zero
+    m_time_for_step = -1.0;
+
+    // Clear any data from the line series
+    m_lineSeries_for_setpoint_x->removePoints(0,m_lineSeries_for_setpoint_x->count());
+    m_lineSeries_for_measured_x->removePoints(0,m_lineSeries_for_measured_x->count());
+
+    // Set the flag that should store data
+    m_shouldStoreData_for_plotting = true;
+
+    // Unlock the mutex
+    m_chart_mutex.unlock();
+
+    // Inform the user about the change
 #ifdef CATKIN_MAKE
-    publish_custom_button_command(1,ui->lineEdit_custom_1);
+    ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] Perform step started");
 #endif
 }
 
@@ -239,6 +263,7 @@ void CsoneControllerTab::on_custom_button_3_clicked()
 
 void CsoneControllerTab::setMeasuredPose(float x , float y , float z , float roll , float pitch , float yaw , bool occluded)
 {
+    // Check if the object is occluded for this data
     if (!occluded)
     {
         // INITIALISE A STRING VARIABLE FOR ADDING THE "+"
@@ -272,6 +297,16 @@ void CsoneControllerTab::setMeasuredPose(float x , float y , float z , float rol
         if ( !(ui->red_frame_position_right->isVisible()) )
             ui->red_frame_position_right->setVisible(true);
     }
+
+    // Pass the data through to the plotting function
+    if (!occluded)
+    {
+        newDataForPerformingStepAndPlotting(x);
+    }
+    else
+    {
+        newDataForPerformingStepAndPlotting(0.0);
+    }
 }
 
 
@@ -288,6 +323,91 @@ void CsoneControllerTab::poseDataUnavailableSlot()
 
 
 
+void CsoneControllerTab::newDataForPerformingStepAndPlotting(float x)
+{
+    // Static variables for the min and max of the vertical axis
+    static float min_value_plotted   = -0.01;
+    static float max_value_plotted   =  0.01;
+    static float next_rezoom_at_time =  2.00;
+
+    // Only do something if the flag indicates to do so
+    if (m_shouldStoreData_for_plotting)
+    {
+        // Lock the mutex
+        m_chart_mutex.lock();
+
+        // Add the data to the line series
+        float temp_x_setpoint = (ui->lineEdit_setpoint_current_x->text()).toFloat();
+        m_lineSeries_for_setpoint_x->append(m_time_for_step, (ui->lineEdit_setpoint_current_x->text()).toFloat() );
+        m_lineSeries_for_measured_x->append(m_time_for_step, x );
+
+        // Reset the min, max, and rezoom time at the start of performing the step
+        if (m_time_for_step <= -0.998)
+        {
+            min_value_plotted   = -0.01;
+            max_value_plotted   =  0.01;
+            next_rezoom_at_time =  2.00;
+        }
+
+        // Update the min and max values
+        min_value_plotted = std::min( min_value_plotted , std::min(temp_x_setpoint,x) );
+        max_value_plotted = std::max( max_value_plotted , std::max(temp_x_setpoint,x) );
+
+        // Increment the time
+        m_time_for_step += 0.005;
+
+        // Change the setpoint if the time has been incremented to zero
+        // > Note: use 2 milliseconds as the threshold for zero
+        if (m_shouldPerformStep)
+        {
+            if (m_time_for_step >= -0.002)
+            {
+                // Inform the user about the change
+                #ifdef CATKIN_MAKE
+                ROS_INFO_STREAM("[CSONE CONTROLLER TAB GUI] DEBUG 1");
+                #endif
+                // Set the flag so that we do not enter this loop again
+                m_shouldPerformStep = false;
+
+                // Extract the current setpoints
+                float current_x   = (ui->lineEdit_setpoint_current_x->text()).toFloat();
+                float current_y   = (ui->lineEdit_setpoint_current_y->text()).toFloat();
+                float current_z   = (ui->lineEdit_setpoint_current_z->text()).toFloat();
+                float current_yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
+
+                // Determine the new x setpoint
+                float new_x = -current_x;
+
+                // Publish the new setpoint
+                publishSetpoint(new_x,current_y,current_z,current_yaw);
+            }
+        }
+
+        // Rezoom when the next time is reached
+        if ( m_time_for_step >= (next_rezoom_at_time+0.002) )
+        {
+            float diff_of_values_plotted = max_value_plotted - min_value_plotted;
+            ui->chartView_for_x->chart()->axisY()->setMin( min_value_plotted - 0.1*diff_of_values_plotted );
+            ui->chartView_for_x->chart()->axisY()->setMax( max_value_plotted + 0.1*diff_of_values_plotted );
+            // Increment the next rezoom time
+            next_rezoom_at_time += 0.5;
+        }
+
+        // Stop data collection and analyse the results if the time has passed 10 seconds
+        if (m_time_for_step >= 10.002)
+        {
+            m_shouldStoreData_for_plotting = false;
+            // Aslo rezoom the axes
+            float diff_of_values_plotted = max_value_plotted - min_value_plotted;
+            ui->chartView_for_x->chart()->axisY()->setMin( min_value_plotted - 0.1*diff_of_values_plotted );
+            ui->chartView_for_x->chart()->axisY()->setMax( max_value_plotted + 0.1*diff_of_values_plotted );
+        }
+
+        // Unlock the mutex
+        m_chart_mutex.unlock();
+    }
+}
+
 
 
 //    ----------------------------------------------------------------------------------
@@ -323,6 +443,9 @@ void CsoneControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHe
     float z = newSetpoint.z;
     float yaw = newSetpoint.yaw;
 
+    // Lock the mutex
+    m_chart_mutex.lock();
+
     // UPDATE THE SETPOINT COLUMN
     if (x < 0.0f) qstr = ""; else qstr = "+";
     ui->lineEdit_setpoint_current_x->setText(qstr + QString::number( x, 'f', 3));
@@ -333,6 +456,9 @@ void CsoneControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWithHe
 
     if (yaw < 0.0f) qstr = ""; else qstr = "+";
     ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
+
+    // Unlock the mutex
+    m_chart_mutex.unlock();
 }
 #endif
 
@@ -410,6 +536,8 @@ void CsoneControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
 
 void CsoneControllerTab::on_set_setpoint_button_clicked()
 {
+    // Lock the mutex
+    m_chart_mutex.lock();
 
     // Initialise local variable for each of (x,y,z,yaw)
     float x = 0.0f, y = 0.0f, z = 0.0f, yaw = 0.0f;
@@ -436,6 +564,9 @@ void CsoneControllerTab::on_set_setpoint_button_clicked()
     else
         yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
 
+    // Unlock the mutex
+    m_chart_mutex.unlock();
+
     // Call the function to publish the setpoint
     publishSetpoint(x,y,z,yaw);
 }
@@ -531,12 +662,18 @@ void CsoneControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool sh
         // Unsubscribe
         setpointChangedSubscriber.shutdown();
 
+        // Lock the mutex
+        m_chart_mutex.lock();
+
         // Set information back to the default
         ui->lineEdit_setpoint_current_x->setText("xx.xx");
         ui->lineEdit_setpoint_current_y->setText("xx.xx");
         ui->lineEdit_setpoint_current_z->setText("xx.xx");
         ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
 
+        // Unlock the mutex
+        m_chart_mutex.unlock();
+
     }
 #endif
 }