Commit 452c6c3f authored by roangel's avatar roangel
Browse files

Added some new fields to GUI: diff, setpoint wired, etc. Need to test downstairs

parent 62c3394e
......@@ -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);
......
......@@ -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);
}
......@@ -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>
......
<?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>
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment