From 342f165cea15e7820e921093e959639ea8a7609d Mon Sep 17 00:00:00 2001
From: roangel <roangel@student.ethz.ch>
Date: Fri, 25 Aug 2017 10:05:29 +0200
Subject: [PATCH] added buttons to take off and land, some guards in take off
and landing, and feedback from flying state to GUI
---
.../GUI_Qt/studentGUI/include/MainWindow.h | 22 +++++--
.../GUI_Qt/studentGUI/src/MainWindow.cpp | 60 +++++++++++++------
.../GUI_Qt/studentGUI/src/MainWindow.ui | 45 ++++++++++----
.../GUI_Qt/studentGUI/studentGUI.pro.user | 2 +-
pps_ws/src/d_fall_pps/src/PPSClient.cpp | 24 +++++++-
5 files changed, 116 insertions(+), 37 deletions(-)
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 f74e2d41..0ce63901 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
@@ -18,10 +18,17 @@
#define DISCONNECTED 2
// Commands for PPSClient
-#define CMD_USE_SAFE_CONTROLLER 1
+#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
-#define CMD_USE_CRAZYFLY_ENABLE 3
-#define CMD_USE_CRAZYFLY_DISABLE 4
+#define CMD_CRAZYFLY_TAKE_OFF 3
+#define CMD_CRAZYFLY_LAND 4
+#define CMD_CRAZYFLY_MOTORS_OFF 5
+
+// Flying States
+#define STATE_MOTORS_OFF 1
+#define STATE_TAKE_OFF 2
+#define STATE_FLYING 3
+#define STATE_LAND 4
namespace Ui {
class MainWindow;
@@ -38,7 +45,12 @@ public:
private slots:
void updateNewViconData(const ptrToMessage& p_msg);
void on_RF_Connect_button_clicked();
- void on_enable_disable_CF_button_clicked();
+
+ void on_take_off_button_clicked();
+
+ void on_land_button_clicked();
+
+ void on_motors_OFF_button_clicked();
private:
Ui::MainWindow *ui;
@@ -53,10 +65,12 @@ private:
ros::Subscriber crazyRadioStatusSubscriber;
ros::Publisher PPSClientCommandPublisher;
ros::Subscriber CFBatterySubscriber;
+ ros::Subscriber flyingStateSubscriber;
// callbacks
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
void CFBatteryCallback(const std_msgs::Float32& msg);
+ void flyingStateChangedCallback(const std_msgs::Int32& msg);
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 9567a69e..c1bd7a8e 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
@@ -26,6 +26,8 @@ MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);
+ flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, 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");
@@ -43,17 +45,38 @@ MainWindow::~MainWindow()
void MainWindow::disableGUI()
{
- ui->enable_disable_CF_button->setEnabled(false);
ui->battery_bar->setValue(0);
ui->battery_bar->setEnabled(false);
}
void MainWindow::enableGUI()
{
- ui->enable_disable_CF_button->setEnabled(true);
ui->battery_bar->setEnabled(true);
}
+void MainWindow::flyingStateChangedCallback(const std_msgs::Int32& msg)
+{
+ QString qstr = "Flying State: ";
+ switch(msg.data)
+ {
+ case STATE_MOTORS_OFF:
+ qstr.append("Motors OFF");
+ break;
+ case STATE_TAKE_OFF:
+ qstr.append("Take OFF");
+ break;
+ case STATE_FLYING:
+ qstr.append("Flying");
+ break;
+ case STATE_LAND:
+ qstr.append("Land");
+ break;
+ default:
+ break;
+ }
+ ui->flying_state_label->setText(qstr);
+}
+
void MainWindow::setCrazyRadioStatus(int radio_status)
{
// add more things whenever the status is changed
@@ -131,20 +154,23 @@ void MainWindow::on_RF_Connect_button_clicked()
ROS_INFO("command reconnect published");
}
-void MainWindow::on_enable_disable_CF_button_clicked()
+void MainWindow::on_take_off_button_clicked()
{
- if(ui->enable_disable_CF_button->text().toStdString() == "EnableCF") //enabled, disable if success
- {
- std_msgs::Int32 msg;
- msg.data = CMD_USE_CRAZYFLY_ENABLE;
- this->PPSClientCommandPublisher.publish(msg);
- ui->enable_disable_CF_button->setText("DisableCF");
- }
- else //disabled, enable if success
- {
- std_msgs::Int32 msg;
- msg.data = CMD_USE_CRAZYFLY_DISABLE;
- this->PPSClientCommandPublisher.publish(msg);
- ui->enable_disable_CF_button->setText("EnableCF");
- }
+ std_msgs::Int32 msg;
+ msg.data = CMD_CRAZYFLY_TAKE_OFF;
+ this->PPSClientCommandPublisher.publish(msg);
+}
+
+void MainWindow::on_land_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_CRAZYFLY_LAND;
+ this->PPSClientCommandPublisher.publish(msg);
+}
+
+void MainWindow::on_motors_OFF_button_clicked()
+{
+ std_msgs::Int32 msg;
+ msg.data = CMD_CRAZYFLY_MOTORS_OFF;
+ this->PPSClientCommandPublisher.publish(msg);
}
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 2339f8bb..eebf582b 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
@@ -6,8 +6,8 @@
<rect>
<x>0</x>
<y>0</y>
- <width>490</width>
- <height>396</height>
+ <width>525</width>
+ <height>424</height>
</rect>
</property>
<property name="windowTitle">
@@ -28,6 +28,20 @@
<string>StudentID # connected to CF #</string>
</property>
<layout class="QGridLayout" name="gridLayout_2">
+ <item row="3" column="1">
+ <widget class="QPushButton" name="take_off_button">
+ <property name="text">
+ <string>Take Off</string>
+ </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">
@@ -35,7 +49,7 @@
</property>
</widget>
</item>
- <item row="3" column="1">
+ <item row="4" column="1">
<widget class="QGroupBox" name="groupBox_2">
<property name="title">
<string>Current Position</string>
@@ -72,13 +86,6 @@
</layout>
</widget>
</item>
- <item row="0" column="1">
- <widget class="QPushButton" name="enable_disable_CF_button">
- <property name="text">
- <string>EnableCF</string>
- </property>
- </widget>
- </item>
<item row="2" column="3">
<widget class="QProgressBar" name="battery_bar">
<property name="value">
@@ -93,7 +100,7 @@
</property>
</widget>
</item>
- <item row="3" column="3">
+ <item row="4" column="3">
<widget class="QGroupBox" name="groupBox_3">
<property name="title">
<string>Controller and current set point</string>
@@ -177,6 +184,20 @@
</layout>
</widget>
</item>
+ <item row="0" column="1">
+ <widget class="QPushButton" name="motors_OFF_button">
+ <property name="text">
+ <string>Motors OFF</string>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="3">
+ <widget class="QPushButton" name="land_button">
+ <property name="text">
+ <string>Land</string>
+ </property>
+ </widget>
+ </item>
</layout>
</widget>
</item>
@@ -194,7 +215,7 @@
<rect>
<x>0</x>
<y>0</y>
- <width>490</width>
+ <width>525</width>
<height>19</height>
</rect>
</property>
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 7bde36e1..11e0d6b5 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-22T18:03:35. -->
+<!-- Written by QtCreator 4.0.2, 2017-08-25T10:03:36. -->
<qtcreator>
<data>
<variable>EnvironmentId</variable>
diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
index d0238a06..bf9a41ab 100755
--- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp
+++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp
@@ -40,7 +40,6 @@
#define CMD_CRAZYFLY_MOTORS_OFF 5
// Flying states
-
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF 2
#define STATE_FLYING 3
@@ -71,6 +70,9 @@ ros::Publisher controlCommandPublisher;
// communicate with safeControllerService, setpoint, etc...
ros::Publisher safeControllerServiceSetpointPublisher;
+// publisher for flying state
+ros::Publisher flyingStatePublisher;
+
rosbag::Bag bag;
// variables for the states:
@@ -195,6 +197,9 @@ void changeFlyingStateTo(int new_state)
{
flying_state = new_state;
changed_state_flag = true;
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = flying_state;
+ flyingStatePublisher.publish(flying_state_msg);
}
int counter = 0;
@@ -372,11 +377,17 @@ void commandCallback(const std_msgs::Int32& commandMsg) {
break;
case CMD_CRAZYFLY_TAKE_OFF:
- changeFlyingStateTo(STATE_TAKE_OFF);
+ if(flying_state == STATE_MOTORS_OFF)
+ {
+ changeFlyingStateTo(STATE_TAKE_OFF);
+ }
break;
case CMD_CRAZYFLY_LAND:
- changeFlyingStateTo(STATE_LAND);
+ if(flying_state != STATE_MOTORS_OFF)
+ {
+ changeFlyingStateTo(STATE_LAND);
+ }
break;
case CMD_CRAZYFLY_MOTORS_OFF:
changeFlyingStateTo(STATE_MOTORS_OFF);
@@ -413,6 +424,13 @@ int main(int argc, char* argv[])
//this topic lets us use the terminal to communicate with crazyRadio node.
ros::Publisher crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
+ // this topic will publish flying state whenever it changes.
+ flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
+ // publish first flying state data
+ std_msgs::Int32 flying_state_msg;
+ flying_state_msg.data = flying_state;
+ flyingStatePublisher.publish(flying_state_msg);
+
// SafeControllerServicePublisher:
ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);
--
GitLab