diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
index 29402101f6d6adb55613083985730a790e972315..c43a05a150ef89c7bc217dedcbff520903518394 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/include/MainWindow.h
@@ -10,6 +10,7 @@
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieData.h"
+#include "d_fall_pps/Setpoint.h"
// commands for CrazyRadio
@@ -56,6 +57,8 @@ private slots:
void on_motors_OFF_button_clicked();
+ void on_set_setpoint_button_clicked();
+
private:
Ui::MainWindow *ui;
@@ -67,18 +70,24 @@ private:
int m_student_id;
CrazyflieContext m_context;
+ Setpoint m_setpoint;
+
ros::Publisher crazyRadioCommandPublisher;
ros::Subscriber crazyRadioStatusSubscriber;
ros::Publisher PPSClientCommandPublisher;
ros::Subscriber CFBatterySubscriber;
ros::Subscriber flyingStateSubscriber;
+ ros::Publisher setpointPublisher;
+ ros::Subscriber setpointSubscriber;
+
ros::ServiceClient centralManager;
// callbacks
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
void CFBatteryCallback(const std_msgs::Float32& msg);
void flyingStateChangedCallback(const std_msgs::Int32& msg);
+ void setpointCallback(const Setpoint& newSetpoint);
float fromVoltageToPercent(float voltage);
void updateBatteryVoltage(float battery_voltage);
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
index aebb671803b9e86b473f453b35720e1809ffd3c5..66a5631c0436de7ee5b618e5592497baab3c31ea 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.cpp
@@ -29,12 +29,17 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
ros::NodeHandle nodeHandle(ros_namespace);
+
+ // subscribers
crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);
flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);
+ setpointPublisher = nodeHandle.advertise<Setpoint>("SafeControllerService/Setpoint", 1);
+ setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this);
+
// communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
ros::NodeHandle nh_PPSClient(ros_namespace + "/PPSClient");
@@ -48,9 +53,6 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
ROS_ERROR("Failed to get studentID");
}
-
-
-
// Then, Central manager
centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
loadCrazyflieContext();
@@ -74,10 +76,22 @@ MainWindow::~MainWindow()
void MainWindow::disableGUI()
{
+ ui->groupBox->setEnabled(false);
}
void MainWindow::enableGUI()
{
+ ui->groupBox->setEnabled(true);
+}
+
+void MainWindow::setpointCallback(const Setpoint& newSetpoint)
+{
+ m_setpoint = newSetpoint;
+ // here we get the new setpoint, need to update it in GUI
+ ui->current_setpoint_x->setText(QString::number(newSetpoint.x));
+ ui->current_setpoint_y->setText(QString::number(newSetpoint.y));
+ ui->current_setpoint_z->setText(QString::number(newSetpoint.z));
+ ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw));
}
void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
@@ -229,21 +243,18 @@ void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to ne
coordinatesToLocal(local);
// now we have the local coordinates, put them in the labels
- QString qstr = "x = ";
- qstr.append(QString::number(local.x));
- ui->current_x->setText(qstr);
-
- qstr = "y = ";
- qstr.append(QString::number(local.y));
- ui->current_y->setText(qstr);
-
- qstr = "z = ";
- qstr.append(QString::number(local.z));
- ui->current_z->setText(qstr);
-
- qstr = "yaw = ";
- qstr.append(QString::number(local.yaw));
- ui->current_yaw->setText(qstr);
+ ui->current_x->setText(QString::number(local.x));
+ ui->current_y->setText(QString::number(local.y));
+ ui->current_z->setText(QString::number(local.z));
+ ui->current_yaw->setText(QString::number(local.yaw));
+ ui->current_pitch->setText(QString::number(local.pitch));
+ ui->current_roll->setText(QString::number(local.roll));
+
+ // also update diff
+ ui->diff_x->setText(QString::number(m_setpoint.x - local.x));
+ ui->diff_y->setText(QString::number(m_setpoint.y - local.y));
+ ui->diff_z->setText(QString::number(m_setpoint.z - local.z));
+ ui->diff_yaw->setText(QString::number(m_setpoint.yaw - local.yaw));
}
}
}
@@ -276,3 +287,14 @@ void MainWindow::on_motors_OFF_button_clicked()
msg.data = CMD_CRAZYFLY_MOTORS_OFF;
this->PPSClientCommandPublisher.publish(msg);
}
+
+void MainWindow::on_set_setpoint_button_clicked()
+{
+ Setpoint msg_setpoint;
+ msg_setpoint.x = (ui->new_setpoint_x->text()).toFloat();
+ msg_setpoint.y = (ui->new_setpoint_y->text()).toFloat();
+ msg_setpoint.z = (ui->new_setpoint_z->text()).toFloat();
+ msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat();
+
+ this->setpointPublisher.publish(msg_setpoint);
+}
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
index 4f11a2e4944ae4f70a2e19efcfe5bff9a5a7cb04..c2772eae2672a22b3a12e84078c55c6628903cab 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/src/MainWindow.ui
@@ -35,13 +35,6 @@
</property>
</widget>
</item>
- <item row="4" column="2">
- <widget class="QLabel" name="flying_state_label">
- <property name="text">
- <string>FlyingState</string>
- </property>
- </widget>
- </item>
<item row="0" column="3">
<widget class="QLabel" name="raw_voltage">
<property name="text">
@@ -52,37 +45,135 @@
<item row="4" column="1">
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
- <string>Current Position</string>
+ <string/>
</property>
<layout class="QGridLayout" name="gridLayout_3">
- <item row="0" column="0">
- <widget class="QLabel" name="current_x">
+ <item row="6" column="1">
+ <widget class="QLineEdit" name="current_roll">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="1">
+ <widget class="QLineEdit" name="current_y">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="current_yaw">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_z">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0">
+ <widget class="QLabel" name="current_x_label">
<property name="text">
<string>x =</string>
</property>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="current_z">
+ <item row="3" column="0">
+ <widget class="QLabel" name="current_z_label">
<property name="text">
<string>z =</string>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="current_y">
+ <item row="2" column="0">
+ <widget class="QLabel" name="current_y_label">
<property name="text">
<string>y =</string>
</property>
</widget>
</item>
- <item row="3" column="0">
- <widget class="QLabel" name="current_yaw">
+ <item row="4" column="0">
+ <widget class="QLabel" name="current_yaw_label">
<property name="text">
<string>yaw = </string>
</property>
</widget>
</item>
+ <item row="1" column="1">
+ <widget class="QLineEdit" name="current_x">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <widget class="QLabel" name="current_pitch_label">
+ <property name="text">
+ <string>pitch =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <widget class="QLabel" name="current_roll_label">
+ <property name="text">
+ <string>roll =</string>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="current_pitch">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="1">
+ <widget class="QLabel" name="label_4">
+ <property name="text">
+ <string>Current</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLineEdit" name="diff_x">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="2" column="2">
+ <widget class="QLineEdit" name="diff_y">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="diff_z">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="diff_yaw">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_5">
+ <property name="text">
+ <string>Difference</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
@@ -106,81 +197,135 @@
<string>Controller and current set point</string>
</property>
<layout class="QGridLayout" name="gridLayout_4">
- <item row="0" column="0">
- <widget class="QPushButton" name="pushButton_3">
+ <item row="0" column="2">
+ <widget class="QPushButton" name="pushButton_4">
<property name="text">
- <string>Custom</string>
+ <string>Safe</string>
</property>
</widget>
</item>
- <item row="2" column="0">
- <widget class="QLabel" name="label_7">
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="new_setpoint_y"/>
+ </item>
+ <item row="6" column="2">
+ <widget class="QLineEdit" name="new_setpoint_yaw"/>
+ </item>
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="new_setpoint_x"/>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_2">
<property name="text">
- <string>x =</string>
+ <string>Controller:</string>
</property>
</widget>
</item>
- <item row="4" column="0">
- <widget class="QLabel" name="label_9">
- <property name="text">
- <string>z =</string>
+ <item row="3" column="1">
+ <widget class="QLineEdit" name="current_setpoint_x">
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="5" column="0">
+ <item row="4" column="1">
+ <widget class="QLineEdit" name="current_setpoint_y">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
<widget class="QLabel" name="label_11">
<property name="text">
<string>yaw =</string>
</property>
</widget>
</item>
- <item row="3" column="0">
- <widget class="QLabel" name="label_8">
+ <item row="2" column="1">
+ <widget class="QLabel" name="label_12">
<property name="text">
- <string>y =</string>
+ <string>Current</string>
</property>
</widget>
</item>
- <item row="2" column="1">
- <widget class="QLineEdit" name="lineEdit"/>
- </item>
- <item row="3" column="1">
- <widget class="QLineEdit" name="lineEdit_2"/>
+ <item row="3" column="0">
+ <widget class="QLabel" name="label_7">
+ <property name="text">
+ <string>x =</string>
+ </property>
+ </widget>
</item>
- <item row="4" column="1">
- <widget class="QLineEdit" name="lineEdit_3"/>
+ <item row="5" column="0">
+ <widget class="QLabel" name="label_9">
+ <property name="text">
+ <string>z =</string>
+ </property>
+ </widget>
</item>
- <item row="5" column="1">
- <widget class="QLineEdit" name="lineEdit_4"/>
+ <item row="4" column="0">
+ <widget class="QLabel" name="label_8">
+ <property name="text">
+ <string>y =</string>
+ </property>
+ </widget>
</item>
- <item row="5" column="2">
- <widget class="QPushButton" name="pushButton_5">
+ <item row="7" column="2">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="font">
+ <font>
+ <pointsize>7</pointsize>
+ </font>
+ </property>
<property name="text">
<string>Set setpoint</string>
</property>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="pushButton_4">
- <property name="text">
- <string>Safe</string>
+ <item row="5" column="1">
+ <widget class="QLineEdit" name="current_setpoint_z">
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="1" column="0">
- <widget class="QLabel" name="label_12">
+ <item row="2" column="0">
+ <widget class="QLabel" name="label_3">
<property name="text">
- <string>Current</string>
+ <string>Setpoint:</string>
</property>
</widget>
</item>
- <item row="1" column="1">
+ <item row="6" column="1">
+ <widget class="QLineEdit" name="current_setpoint_yaw">
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="new_setpoint_z"/>
+ </item>
+ <item row="2" column="2">
<widget class="QLabel" name="label_13">
<property name="text">
<string>New one</string>
</property>
</widget>
</item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="pushButton_3">
+ <property name="text">
+ <string>Custom</string>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="0" colspan="3">
+ <widget class="Line" name="line">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
@@ -198,6 +343,19 @@
</property>
</widget>
</item>
+ <item row="3" column="2">
+ <widget class="QLabel" name="flying_state_label">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Preferred">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="text">
+ <string>FlyingState</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
diff --git a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
index 662cf3214c36ec48b959b3d590e36c4969907377..92b0709c8179e53f89685b4192c62cd4f5553dd4 100644
--- a/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
+++ b/pps_ws/src/d_fall_pps/GUI_Qt/studentGUI/studentGUI.pro.user
@@ -1,6 +1,6 @@
<?xml version="1.0" encoding="UTF-8"?>
<!DOCTYPE QtCreatorProject>
-<!-- Written by QtCreator 4.0.2, 2017-08-28T10:10:56. -->
+<!-- Written by QtCreator 4.0.2, 2017-08-28T12:32:50. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>