MainWindow.cpp 13.1 KB
Newer Older
roangel's avatar
roangel committed
1
2
#include "MainWindow.h"
#include "ui_MainWindow.h"
3
#include <string>
roangel's avatar
roangel committed
4

5
6
#include <ros/ros.h>
#include <ros/network.h>
7
#include <ros/package.h>
8

9
10
11
12
#include "d_fall_pps/CMQuery.h"

#include "d_fall_pps/ViconData.h"

13
MainWindow::MainWindow(int argc, char **argv, QWidget *parent) :
roangel's avatar
roangel committed
14
    QMainWindow(parent),
15
    ui(new Ui::MainWindow),
16
    m_battery_level(0)
roangel's avatar
roangel committed
17
{
18

roangel's avatar
roangel committed
19
    ui->setupUi(this);
20
21

    m_rosNodeThread = new rosNodeThread(argc, argv, "student_GUI");
22
    m_rosNodeThread->init();
23

24
25
    setCrazyRadioStatus(DISCONNECTED);

26
27
    m_ros_namespace = ros::this_node::getNamespace();
    ROS_INFO("namespace: %s", m_ros_namespace.c_str());
28

29
30
    qRegisterMetaType<ptrToMessage>("ptrToMessage");
    QObject::connect(m_rosNodeThread, SIGNAL(newViconData(const ptrToMessage&)), this, SLOT(updateNewViconData(const ptrToMessage&)));
31

32
    ros::NodeHandle nodeHandle(m_ros_namespace);
33
34

    // subscribers
roangel's avatar
roangel committed
35
    crazyRadioStatusSubscriber = nodeHandle.subscribe("CrazyRadio/CrazyRadioStatus", 1, &MainWindow::crazyRadioStatusCallback, this);
36

37
38
    CFBatterySubscriber = nodeHandle.subscribe("CrazyRadio/CFBattery", 1, &MainWindow::CFBatteryCallback, this);

39
40
    flyingStateSubscriber = nodeHandle.subscribe("PPSClient/flyingState", 1, &MainWindow::flyingStateChangedCallback, this);

41

42
    setpointSubscriber = nodeHandle.subscribe("SafeControllerService/Setpoint", 1, &MainWindow::setpointCallback, this);
43
    DBChangedSubscriber = nodeHandle.subscribe("/my_GUI/DBChanged", 1, &MainWindow::DBChangedCallback, this);
44

45
    ros::NodeHandle my_nodeHandle("~");
46
    controllerSetpointPublisher = nodeHandle.advertise<Setpoint>("ControllerSetpoint", 1);
47
48
49
    customYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("customYAMLloaded", 1);
    safeYAMLloadedPublisher = my_nodeHandle.advertise<std_msgs::Int32>("safeYAMLloaded", 1);

50

51
    // communication with PPS Client, just to make it possible to communicate through terminal also we use PPSClient's name
52
    ros::NodeHandle nh_PPSClient(m_ros_namespace + "/PPSClient");
53
    crazyRadioCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("crazyRadioCommand", 1);
54
    PPSClientCommandPublisher = nh_PPSClient.advertise<std_msgs::Int32>("Command", 1);
55

56

57

58
    // First get student ID
59
60
61
62
63
    if(!nh_PPSClient.getParam("studentID", m_student_id))
    {
		ROS_ERROR("Failed to get studentID");
	}

64
65
66
67
68
69
70
71
72
73
74
75
    // Then, Central manager
    centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
    loadCrazyflieContext();

    // we now have the m_context variable with the current context. Put CF Name in label

    QString qstr = "StudentID ";
    qstr.append(QString::number(m_student_id));
    qstr.append(" connected to CF ");
    qstr.append(QString::fromStdString(m_context.crazyflieName));
    ui->groupBox->setTitle(qstr);

roangel's avatar
roangel committed
76
77
    std::vector<float> default_setpoint(4);
    ros::NodeHandle nh_safeControllerService(m_ros_namespace + "/SafeControllerService");
78
79
80
81

    ROS_INFO_STREAM(m_ros_namespace << "/SafeControllerService");

    if(!nh_safeControllerService.getParam("defaultSetpoint", default_setpoint))
roangel's avatar
roangel committed
82
    {
83
        ROS_ERROR_STREAM("couldn't find parameter 'defaultSetpoint'");
roangel's avatar
roangel committed
84
85
86
87
88
89
90
91
    }


    ui->current_setpoint_x->setText(QString::number(default_setpoint[0]));
    ui->current_setpoint_y->setText(QString::number(default_setpoint[1]));
    ui->current_setpoint_z->setText(QString::number(default_setpoint[2]));
    ui->current_setpoint_yaw->setText(QString::number(default_setpoint[3]));

92
    disableGUI();
roangel's avatar
roangel committed
93
94
}

95

roangel's avatar
roangel committed
96
97
98
99
MainWindow::~MainWindow()
{
    delete ui;
}
100

101
102
void MainWindow::disableGUI()
{
103
    ui->groupBox_general->setEnabled(false);
104
105
106
107
}

void MainWindow::enableGUI()
{
108
    ui->groupBox_general->setEnabled(true);
109
110
}

111
112
113
void MainWindow::DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
114
    ROS_INFO("context reloaded in student_GUI");
115
116
}

117
118
119
120
void MainWindow::setpointCallback(const Setpoint& newSetpoint)
{
    m_setpoint = newSetpoint;
    // here we get the new setpoint, need to update it in GUI
121
122
123
124
    ui->current_setpoint_x->setText(QString::number(newSetpoint.x, 'f', 3));
    ui->current_setpoint_y->setText(QString::number(newSetpoint.y, 'f', 3));
    ui->current_setpoint_z->setText(QString::number(newSetpoint.z, 'f', 3));
    ui->current_setpoint_yaw->setText(QString::number(newSetpoint.yaw * RAD2DEG, 'f', 1));
125
126
}

127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
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);
}

150
151
152
153
154
155
156
157
void MainWindow::setCrazyRadioStatus(int radio_status)
{
    // add more things whenever the status is changed
    switch(radio_status)
    {
        case CONNECTED:
            // change icon, the rest of the GUI is available now
            ui->connected_disconnected_label->setText("Connected!");
158
            enableGUI();
159
160
161
162
163
164
165
166
            break;
        case CONNECTING:
            // change icon
            ui->connected_disconnected_label->setText("Connecting...");
            break;
        case DISCONNECTED:
            // change icon, the rest of the GUI is disabled
            ui->connected_disconnected_label->setText("Disconnected");
167
            disableGUI();
168
169
170
171
172
173
174
175
            break;
        default:
    		ROS_ERROR_STREAM("unexpected radio status: " << m_radio_status);
            break;
    }
    this->m_radio_status = radio_status;
}

176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
float MainWindow::fromVoltageToPercent(float voltage)
{
    int num_cutoffs = m_cutoff_voltages.size();
    float hysteresis = 0.05;

    while(m_battery_level < num_cutoffs && voltage >= m_cutoff_voltages[m_battery_level])
    {
        ++m_battery_level;
    }
    while(m_battery_level > 0 && voltage < m_cutoff_voltages[m_battery_level - 1] - hysteresis)
    {
        --m_battery_level;
    }

    float percentage = 100.0 * m_battery_level/num_cutoffs;
191
192
193
194
195
196
197

    // should not hapen, but just in case...
    if(percentage > 100)
        percentage = 100;
    if(percentage < 0)
        percentage = 0;

198
199
200
201
202
203
204
    return percentage;
}

void MainWindow::updateBatteryVoltage(float battery_voltage)
{
    m_battery_voltage = battery_voltage;
    // Need to take voltage, display it and transform it to percentage
205
206
207
208
209
210
211
    int percentage = (int) fromVoltageToPercent(m_battery_voltage);

    if(percentage != ui->battery_bar->value())
    {
        // ui->battery_bar->setValue(percentage);
    }

212
    QString qstr = "Raw voltage: ";
roangel's avatar
roangel committed
213
    qstr.append(QString::number(battery_voltage, 'f', 2));
214
    ui->raw_voltage->setText(qstr);
215
216
217
218
219
220
221
}

void MainWindow::CFBatteryCallback(const std_msgs::Float32& msg)
{
    updateBatteryVoltage(msg.data);
}

222
223
void MainWindow::crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
224
    ROS_INFO("Callback CrazyRadioStatus called");
225
226
227
    this->setCrazyRadioStatus(msg.data);
}

228
229
230
231
232
233
234
235
void MainWindow::loadCrazyflieContext()
{
	CMQuery contextCall;
	contextCall.request.studentID = m_student_id;
	ROS_INFO_STREAM("StudentID:" << m_student_id);

	centralManager.waitForExistence(ros::Duration(-1));

236
237
	if(centralManager.call(contextCall))
    {
238
239
		m_context = contextCall.response.crazyflieContext;
		ROS_INFO_STREAM("CrazyflieContext:\n" << m_context);
240
241
242
	}
    else
    {
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
		ROS_ERROR("Failed to load context");
	}

	ros::NodeHandle nh("CrazyRadio");
	nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
}

void MainWindow::coordinatesToLocal(CrazyflieData& cf)
{
	AreaBounds area = m_context.localArea;
	float originX = (area.xmin + area.xmax) / 2.0;
	float originY = (area.ymin + area.ymax) / 2.0;
    // change Z origin to zero, i.e., to the table height, zero of global coordinates, instead of middle of the box
    float originZ = 0.0;
	// float originZ = (area.zmin + area.zmax) / 2.0;

	cf.x -= originX;
	cf.y -= originY;
	cf.z -= originZ;
}


265
266
void MainWindow::updateNewViconData(const ptrToMessage& p_msg) //connected to newViconData, from node
{
267
268
269
    for(std::vector<CrazyflieData>::const_iterator it = p_msg->crazyflies.begin(); it != p_msg->crazyflies.end(); ++it)
    {
		CrazyflieData global = *it;
270
271
272
273
274
275
        if(global.crazyflieName == m_context.crazyflieName)
        {
            CrazyflieData local = global;
            coordinatesToLocal(local);

            // now we have the local coordinates, put them in the labels
roangel's avatar
roangel committed
276
277
278
            ui->current_x->setText(QString::number(local.x, 'f', 3));
            ui->current_y->setText(QString::number(local.y, 'f', 3));
            ui->current_z->setText(QString::number(local.z, 'f', 3));
279
280
281
            ui->current_yaw->setText(QString::number(local.yaw * RAD2DEG, 'f', 1));
            ui->current_pitch->setText(QString::number(local.pitch * RAD2DEG, 'f', 1));
            ui->current_roll->setText(QString::number(local.roll * RAD2DEG, 'f', 1));
282
283

            // also update diff
roangel's avatar
roangel committed
284
285
286
            ui->diff_x->setText(QString::number(m_setpoint.x - local.x, 'f', 3));
            ui->diff_y->setText(QString::number(m_setpoint.y - local.y, 'f', 3));
            ui->diff_z->setText(QString::number(m_setpoint.z - local.z, 'f', 3));
287
            ui->diff_yaw->setText(QString::number((m_setpoint.yaw - local.yaw) * RAD2DEG, 'f', 1));
288
        }
289
    }
290
}
291
292
293
294
295

void MainWindow::on_RF_Connect_button_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_RECONNECT;
296
    this->crazyRadioCommandPublisher.publish(msg);
roangel's avatar
roangel committed
297
    ROS_INFO("command reconnect published");
298
}
299

300
void MainWindow::on_take_off_button_clicked()
301
{
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
    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);
319
}
320
321
322
323
324
325
326

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();
roangel's avatar
roangel committed
327
    msg_setpoint.yaw = (ui->new_setpoint_yaw->text()).toFloat() * DEG2RAD;
328

329
    this->controllerSetpointPublisher.publish(msg_setpoint);
330
}
331
332
333
334
335
336
337
338

void MainWindow::on_pushButton_3_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_DISCONNECT;
    this->crazyRadioCommandPublisher.publish(msg);
    ROS_INFO("command disconnect published");
}
339

340
341
342
343
344
345
346
347
348
349
350
void MainWindow::safeYamlFileTimerCallback(const ros::TimerEvent&)
{
    // send msg that says that parameters have changed in YAML file
    std_msgs::Int32 msg;
    msg.data = 1;
    this->safeYAMLloadedPublisher.publish(msg);
    ROS_INFO("YALMloaded published");
    ui->load_safe_yaml_button->setEnabled(true);
}

void MainWindow::on_load_safe_yaml_button_clicked()
351
{
352
353
354
    ui->load_safe_yaml_button->setEnabled(false);
    ros::NodeHandle nodeHandle("~");
    m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::safeYamlFileTimerCallback, this, true);
roangel's avatar
roangel committed
355
356
357
358
359
360
361
362
363
364

    std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
    ROS_INFO_STREAM(d_fall_pps_path);

    // first, reload the name of the custom controller:
    std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);

    // then, reload the parameters of the custom controller:
365
    cmd = "rosparam load " + d_fall_pps_path + "/param/SafeController.yaml " + m_ros_namespace + "/SafeControllerService";
roangel's avatar
roangel committed
366
367
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);
368
369
}

370
371
372


void MainWindow::customYamlFileTimerCallback(const ros::TimerEvent&)
373
{
374
375
376
377
378
379
380
381
382
383
384
    // send msg that says that parameters have changed in YAML file
    std_msgs::Int32 msg;
    msg.data = 1;
    this->customYAMLloadedPublisher.publish(msg);
    ROS_INFO("YALMloaded published");
    ui->load_custom_yaml_button->setEnabled(true);
}

void MainWindow::on_load_custom_yaml_button_clicked()
{
    ui->load_custom_yaml_button->setEnabled(false);
roangel's avatar
roangel committed
385
    ros::NodeHandle nodeHandle("~");
386
387
388
389
390
391
392
393
394
395
396
397
398
399
    m_custom_timer_yaml_file = nodeHandle.createTimer(ros::Duration(1), &MainWindow::customYamlFileTimerCallback, this, true);

    std::string d_fall_pps_path = ros::package::getPath("d_fall_pps");
    ROS_INFO_STREAM(d_fall_pps_path);

    // first, reload the name of the custom controller:
    std::string cmd = "rosparam load " + d_fall_pps_path + "/param/ClientConfig.yaml " + m_ros_namespace + "/PPSClient";
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);

    // then, reload the parameters of the custom controller:
    cmd = "rosparam load " + d_fall_pps_path + "/param/CustomController.yaml " + m_ros_namespace + "/CustomControllerService";
    system(cmd.c_str());
    ROS_INFO_STREAM(cmd);
400
401
}

402
403
404
405
void MainWindow::on_en_custom_controller_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_USE_CUSTOM_CONTROLLER;
roangel's avatar
roangel committed
406
    this->PPSClientCommandPublisher.publish(msg);
407
408
}

409

410
411
412
413
void MainWindow::on_en_safe_controller_clicked()
{
    std_msgs::Int32 msg;
    msg.data = CMD_USE_SAFE_CONTROLLER;
roangel's avatar
roangel committed
414
    this->PPSClientCommandPublisher.publish(msg);
415
}