From 3ae6c2f6610157815d8c8dcc2bfab42f53127ded Mon Sep 17 00:00:00 2001
From: Paul Beuchat <beuchatp@control.ee.ethz.ch>
Date: Mon, 25 Nov 2019 08:57:34 +0100
Subject: [PATCH] Connected the chart to the measured data and checked that it
works in the emulation environment
---
dfall_ws/src/dfall_pkg/CMakeLists.txt | 9 +-
.../forms/csonecontrollertab.ui | 6 +-
.../include/csonecontrollertab.h | 16 +-
.../flyingAgentGUI/src/csonecontrollertab.cpp | 163 ++++++++++++++++--
4 files changed, 175 insertions(+), 19 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/CMakeLists.txt b/dfall_ws/src/dfall_pkg/CMakeLists.txt
index c93e0b49..7b1ca67e 100755
--- a/dfall_ws/src/dfall_pkg/CMakeLists.txt
+++ b/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()
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
index de8163e4..a8244e19 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/csonecontrollertab.ui
+++ b/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>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
index ee39cd66..8b0cd5a3 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/csonecontrollertab.h
+++ b/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;
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
index ef8c15cf..dc90b63b 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/csonecontrollertab.cpp
+++ b/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
}
--
GitLab