MainWindow.h 15.1 KB
Newer Older
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero
roangel's avatar
roangel committed
2
//
3
4
5
//    This file is part of D-FaLL-System.
//    
//    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.
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.
14
//    
roangel's avatar
roangel committed
15
//    You should have received a copy of the GNU General Public License
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    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
58

59

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

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

69
70
71
72
// 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
73
74
75
76
#define CMD_USE_SAFE_CONTROLLER      1
#define CMD_USE_DEMO_CONTROLLER      2
#define CMD_USE_STUDENT_CONTROLLER   3
#define CMD_USE_MPC_CONTROLLER       4
77
#define CMD_USE_REMOTE_CONTROLLER    5
78
#define CMD_USE_TUNING_CONTROLLER    6
79
#define CMD_USE_PICKER_CONTROLLER    7
80
81
82
83

#define CMD_CRAZYFLY_TAKE_OFF        11
#define CMD_CRAZYFLY_LAND            12
#define CMD_CRAZYFLY_MOTORS_OFF      13
84
85
86
87
88
89

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

91
// Battery states
92
93
94
#define BATTERY_STATE_NORMAL 0
#define BATTERY_STATE_LOW    1

95
96
97
98
99
100
101
102
103
// 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

104
// For which controller parameters to load
105
#define LOAD_YAML_SAFE_CONTROLLER_AGENT           1
106
107
108
#define LOAD_YAML_DEMO_CONTROLLER_AGENT           2
#define LOAD_YAML_STUDENT_CONTROLLER_AGENT        3
#define LOAD_YAML_MPC_CONTROLLER_AGENT            4
109
#define LOAD_YAML_REMOTE_CONTROLLER_AGENT         5
110
#define LOAD_YAML_TUNING_CONTROLLER_AGENT         6
111
#define LOAD_YAML_PICKER_CONTROLLER_AGENT         7
112
113
114
115
116

#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
117
#define LOAD_YAML_REMOTE_CONTROLLER_COORDINATOR   15
118
#define LOAD_YAML_TUNING_CONTROLLER_COORDINATOR   16
119
120
121
122
123
124
125
126
127
#define LOAD_YAML_PICKER_CONTROLLER_COORDINATOR   17


// 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
128
129
#define PICKER_BUTTON_SQUAT         6
#define PICKER_BUTTON_DISCONNECT    7
130
131
132
133
134
135

#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
136

137
// Universal constants
roangel's avatar
roangel committed
138
139
140
141
142
#define PI 3.141592653589

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

roangel's avatar
roangel committed
143
144
145
146
147
148
149
150
151
namespace Ui {
class MainWindow;
}

class MainWindow : public QMainWindow
{
    Q_OBJECT

public:
152
    explicit MainWindow(int argc, char **argv, QWidget *parent = 0);
roangel's avatar
roangel committed
153
154
    ~MainWindow();

155
156
private slots:
    void updateNewViconData(const ptrToMessage& p_msg);
157
158

    // # RF Crazyradio Connect Disconnect
159
    void on_RF_Connect_button_clicked();
160
    void on_RF_disconnect_button_clicked();
161

162
    // # Take off, lanf, motors off
163
164
165
    void on_take_off_button_clicked();
    void on_land_button_clicked();
    void on_motors_OFF_button_clicked();
166

167
    // # Setpoint
168
169
170
171
    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();
172

173
    // # Load Yaml when acting as the GUI for an Agent
174
    void on_load_safe_yaml_button_clicked();
175
176
177
    void on_load_demo_yaml_button_clicked();
    void on_load_student_yaml_button_clicked();
    void on_load_mpc_yaml_button_clicked();
178
    void on_load_remote_yaml_button_clicked();
179
    void on_load_tuning_yaml_button_clicked();
180
    void on_load_picker_yaml_button_clicked();
181

182
183
184
185
186
    // # 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();
187
    void on_enable_remote_controller_clicked();
188
    void on_enable_tuning_controller_clicked();
189
    void on_enable_picker_controller_clicked();
190

191
    
192

193
194
195
196
197
198
199
    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();
200

201
202
203
204
205
206
    // 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();

207
208
209
210
211
    // 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();
212
    void on_tuning_test_circle_button_clicked();
213
214
215
216
    void on_tuning_slider_horizontal_valueChanged(int value);
    void on_tuning_slider_vertical_valueChanged(int value);
    void on_tuning_slider_heading_valueChanged(int value);

217
218
219
220
221
222
223
    // 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();
224
    void on_picker_squat_button_clicked();
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
    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);



240

241

roangel's avatar
roangel committed
242
243
private:
    Ui::MainWindow *ui;
244

245
246
    QShortcut* m_close_GUI_shortcut;

247
    rosNodeThread* m_rosNodeThread;
248
    int m_radio_status;
249
250
    float m_battery_voltage;
    int m_battery_level;
251

252
253
    std::string m_ros_namespace;

beuchatp's avatar
beuchatp committed
254
    ros::Timer m_timer_yaml_file_for_safe_controller;
255
256
257
    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;
258
    ros::Timer m_timer_yaml_file_for_remote_controller;
259
    ros::Timer m_timer_yaml_file_for_tuning_controller;
260
    ros::Timer m_timer_yaml_file_for_picker_controller;
261

roangel's avatar
roangel committed
262

263
264
265
    int m_student_id;
    CrazyflieContext m_context;

266
    Setpoint m_safe_setpoint;
267
268
269
    Setpoint m_demo_setpoint;
    Setpoint m_student_setpoint;
    Setpoint m_mpc_setpoint;
270
    Setpoint m_picker_setpoint;
271

272
    int m_flying_state;
273
274
275
    QMutex m_flying_state_mutex;
    QMutex voltage_field_mutex;
    QMutex battery_status_label_mutex;
276
    QMutex rf_status_label_mutex;
roangel's avatar
roangel committed
277
    int m_battery_state;
278
279
    // 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    };
280
281
    const float battery_voltage_empty_while_flying      =  2.80;   // in Volts
    const float battery_voltage_empty_while_motors_off  =  3.30;  // in Volts
282
    // BATTERY FULL VOLTAGES
283
    const float battery_voltage_full_while_flying      =  3.70;   // in Volts
284
    const float battery_voltage_full_while_motors_off  =  4.20;   // in Volts
roangel's avatar
roangel committed
285

286
287
    int m_battery_label_image_current_index;

288
    ros::Publisher crazyRadioCommandPublisher;
289
    ros::Subscriber crazyRadioStatusSubscriber;
roangel's avatar
roangel committed
290
    ros::Publisher PPSClientCommandPublisher;
291
    ros::Subscriber CFBatterySubscriber;
292
    ros::Subscriber flyingStateSubscriber;
293
    ros::Subscriber batteryStateSubscriber;
294

295
    ros::Publisher controllerSetpointPublisher;
296
297
    ros::Subscriber safeSetpointSubscriber;

298
299
    // SUBSCRIBERS AND PUBLISHERS:
    // > For the Demo Controller SETPOINTS
300
301
    ros::Publisher  demoSetpointPublisher;
    ros::Subscriber demoSetpointSubscriber;
302
    // > For the Student Controller SETPOINTS
303
304
    ros::Publisher  studentSetpointPublisher;
    ros::Subscriber studentSetpointSubscriber;
305
    // > For the MPC Controller SETPOINTS
306
307
    ros::Publisher  mpcSetpointPublisher;
    ros::Subscriber mpcSetpointSubscriber;
308

309
310
311
312
313
314
315
316
    // > 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;
317

318
319
320
321
322
323
324
    // > 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;

325

326
327

	// > For the PICKER CONTROLLER
328
329
330
331
332
333
334
	ros::Publisher  pickerButtonPressedPublisher;
	ros::Publisher  pickerZSetpointPublisher;
	ros::Publisher  pickerYawSetpointPublisher;
	ros::Publisher  pickerMassPublisher;
	ros::Publisher  pickerXAdjustmentPublisher;
	ros::Publisher  pickerYAdjustmentPublisher;
	ros::Publisher  pickerSetpointPublisher;
335
	ros::Subscriber pickerSetpointSubscriber;
336
    ros::Subscriber pickerSetpointToGUISubscriber;
337
338
339
340




341
342
    ros::Publisher demoCustomButtonPublisher;
    ros::Publisher studentCustomButtonPublisher;
Paul Beuchat's avatar
Paul Beuchat committed
343

344
345
    ros::Subscriber DBChangedSubscriber;

346
347


Paul Beuchat's avatar
Paul Beuchat committed
348
    // > For publishing a message that requests the
349
350
351
352
353
354
355
    //   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
356
    ros::Subscriber requestLoadControllerYaml_from_my_GUI_Subscriber;
357

358

359
360
    ros::Subscriber controllerUsedSubscriber;

361
362
    ros::ServiceClient centralManager;

363
    // callbacks
364
    void crazyRadioStatusCallback(const std_msgs::Int32& msg);
365
    void CFBatteryCallback(const std_msgs::Float32& msg);
366
    void flyingStateChangedCallback(const std_msgs::Int32& msg);
367

368
    void safeSetpointCallback(const Setpoint& newSetpoint);
369
370
371
    void demoSetpointCallback(const Setpoint& newSetpoint);
    void studentSetpointCallback(const Setpoint& newSetpoint);
    void mpcSetpointCallback(const Setpoint& newSetpoint);
372
    void pickerSetpointCallback(const Setpoint& newSetpoint);
373

374
375
376

    void remoteDataCallback(const CrazyflieData& objectData);
    void remoteControlSetpointCallback(const CrazyflieData& setpointData);
377
378
379
380


    // > For actually sending the button message
    void send_picker_button_clicked_message(int button_index);
381
382
    

383
    void DBChangedCallback(const std_msgs::Int32& msg);
384
385

    // # Load Yaml when acting as the GUI for an Agent
386
    void safeYamlFileTimerCallback(const ros::TimerEvent&);
387
388
389
    void demoYamlFileTimerCallback(const ros::TimerEvent&);
    void studentYamlFileTimerCallback(const ros::TimerEvent&);
    void mpcYamlFileTimerCallback(const ros::TimerEvent&);
390
    void remoteYamlFileTimerCallback(const ros::TimerEvent&);
391
    void tuningYamlFileTimerCallback(const ros::TimerEvent&);
392
    void pickerYamlFileTimerCallback(const ros::TimerEvent&);
393
394
395
    


396
    void requestLoadControllerYaml_from_my_GUI_Callback(const std_msgs::Int32& msg);
397
    void controllerUsedChangedCallback(const std_msgs::Int32& msg);
398
    void batteryStateChangedCallback(const std_msgs::Int32& msg);
399
400
401

    float fromVoltageToPercent(float voltage);
    void updateBatteryVoltage(float battery_voltage);
402
    void setCrazyRadioStatus(int radio_status);
403
404
    void loadCrazyflieContext();
    void coordinatesToLocal(CrazyflieData& cf);
405
406
407
408

    void initialize_demo_setpoint();
    void initialize_student_setpoint();
    void initialize_mpc_setpoint();
409
    void initialize_picker_setpoint();
410

411
412
413

    void disableGUI();
    void enableGUI();
414
    void highlightSafeControllerTab();
415
416
417
    void highlightDemoControllerTab();
    void highlightStudentControllerTab();
    void highlightMpcControllerTab();
418
    void highlightRemoteControllerTab();
419
    void highlightTuningControllerTab();
420
    void highlightPickerControllerTab();
421

422
423
424
    bool setpointInsideBox(Setpoint setpoint, CrazyflieContext context);
    Setpoint correctSetpointBox(Setpoint setpoint, CrazyflieContext context);

roangel's avatar
roangel committed
425
426
427
};

#endif // MAINWINDOW_H