MainWindow.h 15.5 KB
Newer Older
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
roangel's avatar
roangel committed
2
//
3
//    This file is part of D-FaLL-System.
pragash1's avatar
pragash1 committed
4
//
5
//    D-FaLL-System is free software: you can redistribute it and/or modify
roangel's avatar
roangel committed
6
7
8
//    it under the terms of the GNU General Public License as published by
//    the Free Software Foundation, either version 3 of the License, or
//    (at your option) any later version.
pragash1's avatar
pragash1 committed
9
//
10
//    D-FaLL-System is distributed in the hope that it will be useful,
roangel's avatar
roangel committed
11
12
13
//    but WITHOUT ANY WARRANTY; without even the implied warranty of
//    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
//    GNU General Public License for more details.
pragash1's avatar
pragash1 committed
14
//
roangel's avatar
roangel committed
15
//    You should have received a copy of the GNU General Public License
16
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
pragash1's avatar
pragash1 committed
17
//
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    Main window of the Student's GUI
//
//    ----------------------------------------------------------------------------------

roangel's avatar
roangel committed
32

roangel's avatar
roangel committed
33
34
35
36
#ifndef MAINWINDOW_H
#define MAINWINDOW_H

#include <QMainWindow>
37
#include <QShortcut>
roangel's avatar
roangel committed
38

39
#include <std_msgs/Int32.h>
40
#include <std_msgs/Float32.h>
41

42
#include "rosNodeThread_for_studentGUI.h"
43

44
45
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/CrazyflieData.h"
46
#include "d_fall_pps/Setpoint.h"
47
#include "d_fall_pps/ViconSubscribeObjectName.h"
48
49


50
// Types of controllers being used:
51
52
53
54
#define SAFE_CONTROLLER    1
#define DEMO_CONTROLLER    2
#define STUDENT_CONTROLLER 3
#define MPC_CONTROLLER     4
55
#define REMOTE_CONTROLLER  5
56
#define TUNING_CONTROLLER  6
57
#define PICKER_CONTROLLER  7
pragash1's avatar
pragash1 committed
58
#define DRONEX_CONTROLLER 8
59

60

61
// Commands for CrazyRadio
62
#define CMD_RECONNECT  0
63
#define CMD_DISCONNECT 1
64

65
66
67
68
69
// CrazyRadio states:
#define CONNECTED        0
#define CONNECTING       1
#define DISCONNECTED     2

70
71
72
73
// The constants that "command" changes in the
// operation state of this agent. These "commands"
// are sent from this GUI node to the "PPSClient"
// node where the command is enacted
74
75
76
77
#define CMD_USE_SAFE_CONTROLLER      1
#define CMD_USE_DEMO_CONTROLLER      2
#define CMD_USE_STUDENT_CONTROLLER   3
#define CMD_USE_MPC_CONTROLLER       4
78
#define CMD_USE_REMOTE_CONTROLLER    5
79
#define CMD_USE_TUNING_CONTROLLER    6
80
#define CMD_USE_PICKER_CONTROLLER    7
pragash1's avatar
pragash1 committed
81
#define CMD_USE_DRONEX_CONTROLLER 8
82
83
84
85

#define CMD_CRAZYFLY_TAKE_OFF        11
#define CMD_CRAZYFLY_LAND            12
#define CMD_CRAZYFLY_MOTORS_OFF      13
86
87
88
89
90
91

// Flying States
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF   2
#define STATE_FLYING     3
#define STATE_LAND       4
92

93
// Battery states
94
95
96
#define BATTERY_STATE_NORMAL 0
#define BATTERY_STATE_LOW    1

97
98
99
100
101
102
103
104
105
// Battery label image index
#define BATTERY_LABEL_IMAGE_INDEX_EMPTY     0
#define BATTERY_LABEL_IMAGE_INDEX_20        1
#define BATTERY_LABEL_IMAGE_INDEX_40        2
#define BATTERY_LABEL_IMAGE_INDEX_60        3
#define BATTERY_LABEL_IMAGE_INDEX_80        4
#define BATTERY_LABEL_IMAGE_INDEX_FULL      5
#define BATTERY_LABEL_IMAGE_INDEX_UNKNOWN   6

106
// For which controller parameters to load
107
#define LOAD_YAML_SAFE_CONTROLLER_AGENT           1
108
109
110
#define LOAD_YAML_DEMO_CONTROLLER_AGENT           2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT        3
#define LOAD_YAML_MPC_CONTROLLER_AGENT            4
111
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT         5
112
#define LOAD_YAML_TUNING_CONTROLLER_AGENT         6
113
#define LOAD_YAML_PICKER_CONTROLLER_AGENT         7
pragash1's avatar
pragash1 committed
114
#define LOAD_YAML_DRONEX_CONTROLLER_AGENT     8
115
116
117
118
119

#define LOAD_YAML_SAFE_CONTROLLER_COORDINATOR     11
#define LOAD_YAML_DEMO_CONTROLLER_COORDINATOR     12
#define LOAD_YAML_STUDENT_CONTROLLER_COORDINATOR  13
#define LOAD_YAML_MPC_CONTROLLER_COORDINATOR      14
120
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR   15
121
#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR   16
122
#define LOAD_YAML_PICKER_CONTROLLER_COORDINATOR   17
pragash1's avatar
pragash1 committed
123
124
#define LOAD_YAML_DRONEX_CONTROLLER_COORDINATOR 18

125
126
127
128
129
130
131
132


// FOR WHICH BUTTON WAS PRESSED IN THE PICKER CONTOLLER
#define PICKER_BUTTON_GOTOSTART     1
#define PICKER_BUTTON_CONNECT       2
#define PICKER_BUTTON_PICKUP        3
#define PICKER_BUTTON_GOTOEND       4
#define PICKER_BUTTON_PUTDOWN       5
133
134
#define PICKER_BUTTON_SQUAT         6
#define PICKER_BUTTON_DISCONNECT    7
135
136
137
138
139
140

#define PICKER_BUTTON_1             11
#define PICKER_BUTTON_2             12
#define PICKER_BUTTON_3             13
#define PICKER_BUTTON_4             14

roangel's avatar
roangel committed
141

142
// Universal constants
roangel's avatar
roangel committed
143
144
145
146
147
#define PI 3.141592653589

#define RAD2DEG 180.0/PI
#define DEG2RAD PI/180.0

roangel's avatar
roangel committed
148
149
150
151
152
153
154
155
156
namespace Ui {
class MainWindow;
}

class MainWindow : public QMainWindow
{
    Q_OBJECT

public:
157
    explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
roangel's avatar
roangel committed
158
159
    ~MainWindow();

160
161
private slots:
    void updateNewViconData(const ptrToMessage& p_msg);
162
163

    // # RF Crazyradio Connect Disconnect
164
    void on_RF_Connect_button_clicked();
165
    void on_RF_disconnect_button_clicked();
166

167
    // # Take off, lanf, motors off
168
169
170
    void on_take_off_button_clicked();
    void on_land_button_clicked();
    void on_motors_OFF_button_clicked();
171

172
    // # Setpoint
173
174
175
176
    void on_set_setpoint_button_safe_clicked();
    void on_set_setpoint_button_demo_clicked();
    void on_set_setpoint_button_student_clicked();
    void on_set_setpoint_button_mpc_clicked();
177

178
    // # Load Yaml when acting as the GUI for an Agent
179
    void on_load_safe_yaml_button_clicked();
180
181
182
    void on_load_demo_yaml_button_clicked();
    void on_load_student_yaml_button_clicked();
    void on_load_mpc_yaml_button_clicked();
183
    void on_load_remote_yaml_button_clicked();
184
    void on_load_tuning_yaml_button_clicked();
185
    void on_load_picker_yaml_button_clicked();
186

187
188
189
190
191
    // # Enable controllers
    void on_enable_safe_controller_clicked();
    void on_enable_demo_controller_clicked();
    void on_enable_student_controller_clicked();
    void on_enable_mpc_controller_clicked();
192
    void on_enable_remote_controller_clicked();
193
    void on_enable_tuning_controller_clicked();
194
    void on_enable_picker_controller_clicked();
195

pragash1's avatar
pragash1 committed
196

197

198
199
200
201
202
203
204
    void on_demoButton_1_clicked();
    void on_demoButton_2_clicked();
    void on_demoButton_3_clicked();

    void on_studentButton_1_clicked();
    void on_studentButton_2_clicked();
    void on_studentButton_3_clicked();
205

206
207
208
209
210
211
    // Buttons within the REMOTE controller tab
    void on_remote_subscribe_button_clicked();
    void on_remote_unsubscribe_button_clicked();
    void on_remote_activate_button_clicked();
    void on_remote_deactivate_button_clicked();

212
213
214
215
216
    // Buttons within the TUNING controller tab
    void on_tuning_test_horizontal_button_clicked();
    void on_tuning_test_vertical_button_clicked();
    void on_tuning_test_heading_button_clicked();
    void on_tuning_test_all_button_clicked();
217
    void on_tuning_test_circle_button_clicked();
218
219
220
221
    void on_tuning_slider_horizontal_valueChanged(int value);
    void on_tuning_slider_vertical_valueChanged(int value);
    void on_tuning_slider_heading_valueChanged(int value);

222
223
224
225
226
227
228
    // Interations with the PICKER controller tab
    // > For the buttons
    void on_picker_gotostart_button_clicked();
    void on_picker_connect_button_clicked();
    void on_picker_pickup_button_clicked();
    void on_picker_gotoend_button_clicked();
    void on_picker_putdown_button_clicked();
229
    void on_picker_squat_button_clicked();
230
231
232
233
234
235
236
237
238
239
240
241
242
    void on_picker_disconnect_button_clicked();
    void on_picker_1_button_clicked();
    void on_picker_2_button_clicked();
    void on_picker_3_button_clicked();
    void on_picker_4_button_clicked();
    // > For the sliders
    void on_picker_x_slider_valueChanged(int value);
    void on_picker_y_slider_valueChanged(int value);
    void on_picker_z_slider_valueChanged(int value);
    void on_picker_mass_slider_valueChanged(int value);
    // > For the dial
    void on_picker_yaw_dial_valueChanged(int value);

pragash1's avatar
pragash1 committed
243
244
245
246
247
    // Interationswith the DroneX controller tab
    void on_dronex_take_off_button_clicked();
    void on_dronex_land_button_clicked();


248
249


250

251

roangel's avatar
roangel committed
252
253
private:
    Ui::MainWindow *ui;
254

255
256
    QShortcut* m_close_GUI_shortcut;

257
    rosNodeThread* m_rosNodeThread;
258
    int m_radio_status;
259
260
    float m_battery_voltage;
    int m_battery_level;
261

262
263
    std::string m_ros_namespace;

beuchatp's avatar
beuchatp committed
264
    ros::Timer m_timer_yaml_file_for_safe_controller;
265
266
267
    ros::Timer m_timer_yaml_file_for_demo_controller;
    ros::Timer m_timer_yaml_file_for_student_controller;
    ros::Timer m_timer_yaml_file_for_mpc_controller;
268
    ros::Timer m_timer_yaml_file_for_remote_controller;
269
    ros::Timer m_timer_yaml_file_for_tuning_controller;
270
    ros::Timer m_timer_yaml_file_for_picker_controller;
271

roangel's avatar
roangel committed
272

273
274
275
    int m_student_id;
    CrazyflieContext m_context;

276
    Setpoint m_safe_setpoint;
277
278
279
    Setpoint m_demo_setpoint;
    Setpoint m_student_setpoint;
    Setpoint m_mpc_setpoint;
280
    Setpoint m_picker_setpoint;
281

282
    int m_flying_state;
283
284
285
    QMutex m_flying_state_mutex;
    QMutex voltage_field_mutex;
    QMutex battery_status_label_mutex;
286
    QMutex rf_status_label_mutex;
roangel's avatar
roangel committed
287
    int m_battery_state;
288
289
    // BATTERY EMPTY VOLTAGES (THESE SHOULD BE READ IN AS PARAMTERS)
    //const std::vector<float> m_cutoff_voltages {3.1966,        3.2711,        3.3061,        3.3229,        3.3423,        3.3592,        3.3694,        3.385,        3.4006,        3.4044,        3.4228,        3.4228,        3.4301,        3.4445,        3.4531,        3.4677,        3.4705,        3.4712,        3.4756,        3.483,        3.4944,        3.5008,        3.5008,        3.5084,        3.511,        3.5122,        3.5243,        3.5329,        3.5412,        3.5529,        3.5609,        3.5625,        3.5638,        3.5848,        3.6016,        3.6089,        3.6223,        3.628,        3.6299,        3.6436,        3.6649,        3.6878,        3.6983,        3.7171,        3.7231,        3.7464,        3.7664,        3.7938,        3.8008,        3.816,        3.8313,        3.8482,        3.866,        3.8857,        3.8984,        3.9159,        3.9302,        3.9691,        3.997,        4.14    };
290
291
    const float battery_voltage_empty_while_flying      =  2.80;   // in Volts
    const float battery_voltage_empty_while_motors_off  =  3.30;  // in Volts
292
    // BATTERY FULL VOLTAGES
293
    const float battery_voltage_full_while_flying      =  3.70;   // in Volts
294
    const float battery_voltage_full_while_motors_off  =  4.20;   // in Volts
roangel's avatar
roangel committed
295

296
297
    int m_battery_label_image_current_index;

298
    ros::Publisher crazyRadioCommandPublisher;
299
    ros::Subscriber crazyRadioStatusSubscriber;
roangel's avatar
roangel committed
300
    ros::Publisher PPSClientCommandPublisher;
301
    ros::Subscriber CFBatterySubscriber;
302
    ros::Subscriber flyingStateSubscriber;
303
    ros::Subscriber batteryStateSubscriber;
304

305
    ros::Publisher controllerSetpointPublisher;
306
307
    ros::Subscriber safeSetpointSubscriber;

308
309
    // SUBSCRIBERS AND PUBLISHERS:
    // > For the Demo Controller SETPOINTS
310
311
    ros::Publisher  demoSetpointPublisher;
    ros::Subscriber demoSetpointSubscriber;
312
    // > For the Student Controller SETPOINTS
313
314
    ros::Publisher  studentSetpointPublisher;
    ros::Subscriber studentSetpointSubscriber;
315
    // > For the MPC Controller SETPOINTS
316
317
    ros::Publisher  mpcSetpointPublisher;
    ros::Subscriber mpcSetpointSubscriber;
318

319
320
321
322
323
324
325
326
    // > For the Remote Controller subscribe action
    ros::Publisher remoteSubscribePublisher;
    // > For the Remote Controller activate action
    ros::Publisher remoteActivatePublisher;
    // > For the Remote Controller data
    ros::Subscriber remoteDataSubscriber;
    // > For the Remote Control setpoint
    ros::Subscriber remoteControlSetpointSubscriber;
327

328
329
330
331
332
333
334
    // > For the TUNING CONTROLLER "test" button publisher
    ros::Publisher tuningActivateTestPublisher;
    // > For the TUNING CONTOLLER "gain" sliders
    ros::Publisher tuningHorizontalGainPublisher;
    ros::Publisher tuningVerticalGainPublisher;
    ros::Publisher tuningHeadingGainPublisher;

pragash1's avatar
pragash1 committed
335
336
337
338
    // > For the DRONEX CONTROLLER
    ros::Publisher droneXSetpointPublisher;
    ros::Subscriber droneXSetpointSubscriber;

339

340
341

	// > For the PICKER CONTROLLER
342
343
344
345
346
347
348
	ros::Publisher  pickerButtonPressedPublisher;
	ros::Publisher  pickerZSetpointPublisher;
	ros::Publisher  pickerYawSetpointPublisher;
	ros::Publisher  pickerMassPublisher;
	ros::Publisher  pickerXAdjustmentPublisher;
	ros::Publisher  pickerYAdjustmentPublisher;
	ros::Publisher  pickerSetpointPublisher;
349
	ros::Subscriber pickerSetpointSubscriber;
350
    ros::Subscriber pickerSetpointToGUISubscriber;
351
352
353
354




355
356
    ros::Publisher demoCustomButtonPublisher;
    ros::Publisher studentCustomButtonPublisher;
Paul Beuchat's avatar
Paul Beuchat committed
357

358
359
    ros::Subscriber DBChangedSubscriber;

360
361


Paul Beuchat's avatar
Paul Beuchat committed
362
    // > For publishing a message that requests the
363
364
365
366
367
368
369
    //   YAML parameters to be re-loaded from file
    // > The message contents specify which controller
    //   the parameters should be re-loaded for
    ros::Publisher requestLoadControllerYamlPublisher;

    // Subscriber for locking the load the controller YAML
    // parameters when the Coordintor GUI requests a load
370
    ros::Subscriber requestLoadControllerYaml_from_my_GUI_Subscriber;
371

372

373
374
    ros::Subscriber controllerUsedSubscriber;

375
376
    ros::ServiceClient centralManager;

377
    // callbacks
378
    void crazyRadioStatusCallback(const std_msgs::Int32& msg);
379
    void CFBatteryCallback(const std_msgs::Float32& msg);
380
    void flyingStateChangedCallback(const std_msgs::Int32& msg);
381

382
    void safeSetpointCallback(const Setpoint& newSetpoint);
383
384
385
    void demoSetpointCallback(const Setpoint& newSetpoint);
    void studentSetpointCallback(const Setpoint& newSetpoint);
    void mpcSetpointCallback(const Setpoint& newSetpoint);
386
    void pickerSetpointCallback(const Setpoint& newSetpoint);
pragash1's avatar
pragash1 committed
387
    
388
389
390

    void remoteDataCallback(const CrazyflieData& objectData);
    void remoteControlSetpointCallback(const CrazyflieData& setpointData);
391
392
393
394


    // > For actually sending the button message
    void send_picker_button_clicked_message(int button_index);
pragash1's avatar
pragash1 committed
395

396

397
    void DBChangedCallback(const std_msgs::Int32& msg);
398
399

    // # Load Yaml when acting as the GUI for an Agent
400
    void safeYamlFileTimerCallback(const ros::TimerEvent&);
401
402
403
    void demoYamlFileTimerCallback(const ros::TimerEvent&);
    void studentYamlFileTimerCallback(const ros::TimerEvent&);
    void mpcYamlFileTimerCallback(const ros::TimerEvent&);
404
    void remoteYamlFileTimerCallback(const ros::TimerEvent&);
405
    void tuningYamlFileTimerCallback(const ros::TimerEvent&);
406
    void pickerYamlFileTimerCallback(const ros::TimerEvent&);
pragash1's avatar
pragash1 committed
407

408
409


410
    void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg);
411
    void controllerUsedChangedCallback(const std_msgs::Int32& msg);
412
    void batteryStateChangedCallback(const std_msgs::Int32& msg);
413
414
415

    float fromVoltageToPercent(float voltage);
    void updateBatteryVoltage(float battery_voltage);
416
    void setCrazyRadioStatus(int radio_status);
417
418
    void loadCrazyflieContext();
    void coordinatesToLocal(CrazyflieData& cf);
419
420
421
422

    void initialize_demo_setpoint();
    void initialize_student_setpoint();
    void initialize_mpc_setpoint();
423
    void initialize_picker_setpoint();
424

425
426
427

    void disableGUI();
    void enableGUI();
428
    void highlightSafeControllerTab();
429
430
431
    void highlightDemoControllerTab();
    void highlightStudentControllerTab();
    void highlightMpcControllerTab();
432
    void highlightRemoteControllerTab();
433
    void highlightTuningControllerTab();
434
    void highlightPickerControllerTab();
435

436
437
438
    bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
    Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);

roangel's avatar
roangel committed
439
440
441
};

#endif // MAINWINDOW_H