PPSClient.cpp 42.9 KB
Newer Older
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
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
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,
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
//
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:
//    ROS node that manages the student's setup.
//
//    ----------------------------------------------------------------------------------

32

33
34
35



Paul Beuchat's avatar
Paul Beuchat committed
36
// INCLUDE THE HEADER
37
#include "nodes/PPSClient.h"
roangel's avatar
roangel committed
38

39

40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55



//    ----------------------------------------------------------------------------------
//    FFFFF  U   U  N   N   CCCC  TTTTT  III   OOO   N   N
//    F      U   U  NN  N  C        T     I   O   O  NN  N
//    FFF    U   U  N N N  C        T     I   O   O  N N N
//    F      U   U  N  NN  C        T     I   O   O  N  NN
//    F       UUU   N   N   CCCC    T    III   OOO   N   N
//
//    III M   M PPPP  L     EEEEE M   M EEEEE N   N TTTTT   A   TTTTT III  OOO  N   N
//     I  MM MM P   P L     E     MM MM E     NN  N   T    A A    T    I  O   O NN  N
//     I  M M M PPPP  L     EEE   M M M EEE   N N N   T   A   A   T    I  O   O N N N
//     I  M   M P     L     E     M   M E     N  NN   T   AAAAA   T    I  O   O N  NN
//    III M   M P     LLLLL EEEEE M   M EEEEE N   N   T   A   A   T   III  OOO  N   N
//    ----------------------------------------------------------------------------------
56

57
58


roangel's avatar
roangel committed
59

60
61
62
63




Paul Beuchat's avatar
Paul Beuchat committed
64
//checks if crazyflie is within allowed area and if demo controller returns no data
65
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
66
	//position check
67
	if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
68
		ROS_INFO_STREAM("[PPS CLIENT] x safety failed");
69
		return false;
muelmarc's avatar
muelmarc committed
70
	}
71
	if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
72
		ROS_INFO_STREAM("[PPS CLIENT] y safety failed");
73
		return false;
muelmarc's avatar
muelmarc committed
74
	}
75
	if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
76
		ROS_INFO_STREAM("[PPS CLIENT] z safety failed");
77
		return false;
muelmarc's avatar
muelmarc committed
78
	}
79

80
81
82
83
84
	//attitude check
	//if strictSafety is set to true in ClientConfig.yaml the SafeController takes also over if the roll and pitch angles get to large
	//the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
	if(strictSafety){
		if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
85
			ROS_INFO_STREAM("[PPS CLIENT] roll too big.");
86
87
88
			return false;
		}
		if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
89
			ROS_INFO_STREAM("[PPS CLIENT] pitch too big.");
90
91
92
			return false;
		}
	}
93

94
	return true;
95
}
phfriedl's avatar
phfriedl committed
96

bucyril's avatar
bucyril committed
97
98
99
100
void coordinatesToLocal(CrazyflieData& cf) {
	AreaBounds area = context.localArea;
	float originX = (area.xmin + area.xmax) / 2.0;
	float originY = (area.ymin + area.ymax) / 2.0;
101
102
103
    // 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;
bucyril's avatar
bucyril committed
104
105
106
107
108
109

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

110

111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
void setCurrentSafeSetpoint(Setpoint setpoint)
{
    current_safe_setpoint = setpoint;
}

void calculateDistanceToCurrentSafeSetpoint(CrazyflieData& local)
{
    double dx = current_safe_setpoint.x - local.x;
    double dy = current_safe_setpoint.y - local.y;
    double dz = current_safe_setpoint.z - local.z;

    distance = sqrt(pow(dx,2) + pow(dy,2) + pow(dz,2));

    unit_vector[0] = dx/distance;
    unit_vector[1] = dy/distance;
    unit_vector[2] = dz/distance;
}

129
130
131
132
133
134
135

void takeOffCF(CrazyflieData& current_local_coordinates) //local because the setpoint is in local coordinates
{
    // set the setpoint and call safe controller
    Setpoint setpoint_msg;
    setpoint_msg.x = current_local_coordinates.x;           // previous one
    setpoint_msg.y = current_local_coordinates.y;           //previous one
136
    setpoint_msg.z = current_local_coordinates.z + take_off_distance;           //previous one plus some offset
137
138
    // setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
    setpoint_msg.yaw = 0.0;
139
    safeControllerServiceSetpointPublisher.publish(setpoint_msg);
140
141
142
143
144
    ROS_INFO("[PPS CLIENT] Take OFF:");
    ROS_INFO("[PPS CLIENT] ------Current coordinates:");
    ROS_INFO("[PPS CLIENT] X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
    ROS_INFO("[PPS CLIENT] ------New coordinates:");
    ROS_INFO("[PPS CLIENT] X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
145
146
147

    // now, use safe controller to go to that setpoint
    loadSafeController();
148
    setInstantController(SAFE_CONTROLLER);
149
    // when do we finish? after some time with delay?
150
151
152

    // update variable that keeps track of current setpoint
    setCurrentSafeSetpoint(setpoint_msg);
153
154
155
156
157
158
159
160
}

void landCF(CrazyflieData& current_local_coordinates)
{
    // set the setpoint and call safe controller
    Setpoint setpoint_msg;
    setpoint_msg.x = current_local_coordinates.x;           // previous one
    setpoint_msg.y = current_local_coordinates.y;           //previous one
161
    setpoint_msg.z = landing_distance;           //previous one plus some offset
162
163
164
165
166
    setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
    safeControllerServiceSetpointPublisher.publish(setpoint_msg);

    // now, use safe controller to go to that setpoint
    loadSafeController();
167
    setInstantController(SAFE_CONTROLLER);
168
    setCurrentSafeSetpoint(setpoint_msg);
169
170
171
172
}

void changeFlyingStateTo(int new_state)
{
173
174
    if(crazyradio_status == CONNECTED)
    {
175
        ROS_INFO("[PPS CLIENT] Change state to: %d", new_state);
176
177
178
179
        flying_state = new_state;
    }
    else
    {
180
        ROS_INFO("[PPS CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
181
182
183
        flying_state = STATE_MOTORS_OFF;
    }

184
    changed_state_flag = true;
185
186
187
    std_msgs::Int32 flying_state_msg;
    flying_state_msg.data = flying_state;
    flyingStatePublisher.publish(flying_state_msg);
188
189
}

roangel's avatar
roangel committed
190
191
192
193
194
195
196
197
198
199
200

void takeOffTimerCallback(const ros::TimerEvent&)
{
    finished_take_off = true;
}

void landTimerCallback(const ros::TimerEvent&)
{
    finished_land = true;
}

201
void goToControllerSetpoint()
202
{
203
    safeControllerServiceSetpointPublisher.publish(controller_setpoint);
204
    setCurrentSafeSetpoint(controller_setpoint);
205
206
}

207

208
//is called when new data from Vicon arrives
209
210
void viconCallback(const ViconData& viconData)
{
211
	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
bucyril's avatar
bucyril committed
212
		CrazyflieData global = *it;
213

214
        if(global.crazyflieName == context.crazyflieName)
215
        {
216
217
218
219
220
            Controller controllerCall;
            CrazyflieData local = global;
            coordinatesToLocal(local);
            controllerCall.request.ownCrazyflie = local;

roangel's avatar
roangel committed
221
222
            ros::NodeHandle nodeHandle("~");

223
224
225
226
227
228
            switch(flying_state) //things here repeat every X ms, non-blocking stuff
            {
                case STATE_MOTORS_OFF: // here we will put the code of current disabled button
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
229
                        ROS_INFO("[PPS CLIENT] STATE_MOTORS_OFF");
230
231
232
233
234
235
236
237
                    }
                    break;
                case STATE_TAKE_OFF: //we need to move up from where we are now.
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
                        takeOffCF(local);
                        finished_take_off = false;
238
                        ROS_INFO("[PPS CLIENT] STATE_TAKE_OFF");
239
                        timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_take_off), takeOffTimerCallback, true);
240
241
242
243
                    }
                    if(finished_take_off)
                    {
                        finished_take_off = false;
244
                        setInstantController(getControllerUsed());
245
246
247
248
249
250
251
                        changeFlyingStateTo(STATE_FLYING);
                    }
                    break;
                case STATE_FLYING:
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
252
253
                        // need to change setpoint to the controller one
                        goToControllerSetpoint();
254
                        ROS_INFO("[PPS CLIENT] STATE_FLYING");
255
256
257
258
259
260
261
262
                    }
                    break;
                case STATE_LAND:
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
                        landCF(local);
                        finished_land = false;
263
                        ROS_INFO("[PPS CLIENT] STATE_LAND");
264
                        timer_takeoff = nodeHandle.createTimer(ros::Duration(duration_landing), landTimerCallback, true);
265
266
267
268
                    }
                    if(finished_land)
                    {
                        finished_land = false;
269
                        setInstantController(getControllerUsed());
270
271
272
273
274
275
                        changeFlyingStateTo(STATE_MOTORS_OFF);
                    }
                    break;
                default:
                    break;
            }
276

277
278
279
280
281
            // control part that should always be running, calls to controller, computation of control output
            if(flying_state != STATE_MOTORS_OFF)
            {
                if(!global.occluded)    //if it is not occluded, then proceed to compute the controller output.
                {
282
283
284
285
                    // Only call an "non-safe" controller if:
                    // 1) we are not currently using safe controller, AND
                    // 2) the flying state is FLYING
                    if( (getInstantController() != SAFE_CONTROLLER) && (flying_state == STATE_FLYING) )
286
                    {
287
288
289
290
291
292
293
294
295
296
297
298
299
300
                        // Initialise a local boolean success variable
                        bool success = false;

                        switch(getInstantController())
                        {
                            case DEMO_CONTROLLER:
                                success = demoController.call(controllerCall);
                                break;
                            case STUDENT_CONTROLLER:
                                success = studentController.call(controllerCall);
                                break;
                            case MPC_CONTROLLER:
                                success = mpcController.call(controllerCall);
                                break;
301
302
303
                            case REMOTE_CONTROLLER:
                                success = remoteController.call(controllerCall);
                                break;
304
305
306
                            case TUNING_CONTROLLER:
                                success = tuningController.call(controllerCall);
                                break;
307
308
309
                            case PICKER_CONTROLLER:
                                success = pickerController.call(controllerCall);
                                break;
pragash1's avatar
pragash1 committed
310
311
312
313
														//TODO
														case DRONEX_CONTROLLER:
																success = droneXController.call(controllerCall);
																break;
314
315
316
317
318
319
                            default:
                                ROS_ERROR("[PPS CLIENT] the current controller was not recognised");
                                break;
                        }

                        // Ensure success and enforce safety
320
321
                        if(!success)
                        {
322
323
324
                            ROS_ERROR("[PPS CLIENT] Failed to call a 'non-safe' controller, switching to safe controller");
                            //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller status: valid: " << demoController.isValid() << ", exists: " << demoController.exists());
                            //ROS_ERROR_STREAM("[PPS CLIENT] 'non-safe' controller name: " << demoController.getService());
325
                            setInstantController(SAFE_CONTROLLER);
326
327
328
                        }
                        else if(!safetyCheck(global, controllerCall.response.controlOutput))
                        {
329
                            setInstantController(SAFE_CONTROLLER);
330
                            ROS_INFO_STREAM("[PPS CLIENT] safety check failed, switching to safe controller");
331
                        }
332

pragash1's avatar
pragash1 committed
333

334
                    }
335
336
                    // SAFE_CONTROLLER and state is different from flying
                    else
337
                    {
338
                        calculateDistanceToCurrentSafeSetpoint(local); // update distance, it also updates the unit vector
roangel's avatar
roangel committed
339
                        // ROS_INFO_STREAM("distance: " << distance);
340
                        // here, detect if euclidean distance between setpoint and current position is higher than a threshold
341
                        if(distance > distance_threshold)
342
                        {
343
344
345
                            // DEBUGGING: display a message that the crazyflie is inside the thresholds
                            //ROS_INFO("inside threshold");
                            // Declare a local variable for the "setpoint message" to be published
346
347
                            Setpoint setpoint_msg;
                            // here, where we are now, or where we were in the beginning?
348
349
350
                            setpoint_msg.x = local.x + distance_threshold * unit_vector[0];
                            setpoint_msg.y = local.y + distance_threshold * unit_vector[1];
                            setpoint_msg.z = local.z + distance_threshold * unit_vector[2];
351
352
353
354

                            // yaw is better divided by the number of steps?
                            setpoint_msg.yaw = current_safe_setpoint.yaw;
                            safeControllerServiceSetpointPublisher.publish(setpoint_msg);
roangel's avatar
roangel committed
355
                            was_in_threshold = true;
356
357
358
                        }
                        else
                        {
roangel's avatar
roangel committed
359
360
361
362
363
364
                            if(was_in_threshold)
                            {
                                was_in_threshold = false;
                                safeControllerServiceSetpointPublisher.publish(current_safe_setpoint);
                                // goToControllerSetpoint(); //maybe this is a bit repetitive?
                            }
365
366
                        }

367
368
                        bool success = safeController.call(controllerCall);
                        if(!success) {
369
                            ROS_ERROR_STREAM("[PPS CLIENT] Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
370
371
372
373
374
375
                        }
                    }


                    controlCommandPublisher.publish(controllerCall.response.controlOutput);

376
377
378
                    // Putting data into the ROS "bag" for post-analysis
                    //bag.write("ViconData", ros::Time::now(), local);
                    //bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
379
380
                }
            }
381
382
383
            else
            {
                ControlCommand zeroOutput = ControlCommand(); //everything set to zero
Paul Beuchat's avatar
Paul Beuchat committed
384
                zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
385
                controlCommandPublisher.publish(zeroOutput);
386
387
388
389

                // Putting data into the ROS "bag" for post-analysis
                //bag.write("ViconData", ros::Time::now(), local);
                //bag.write("ControlOutput", ros::Time::now(), zeroOutput);
390
            }
391
        }
392
393
394
	}
}

395

396

397

398
void loadCrazyflieContext() {
399
	CMQuery contextCall;
400
	contextCall.request.studentID = agentID;
401
	ROS_INFO_STREAM("[PPS CLIENT] AgentID:" << agentID);
402

403
404
    CrazyflieContext new_context;

405
406
407
	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall)) {
408
		new_context = contextCall.response.crazyflieContext;
409
		ROS_INFO_STREAM("[PPS CLIENT] CrazyflieContext:\n" << new_context);
410

411
412
        if((context.crazyflieName != "") && (new_context.crazyflieName != context.crazyflieName)) //linked crazyflie name changed and it was not empty before
        {
413

414
            // Motors off is done in python script now everytime we disconnect
415

416
417
418
419
            // send motors OFF and disconnect before setting context = new_context
            // std_msgs::Int32 msg;
            // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
            // commandPublisher.publish(msg);
420

421
            ROS_INFO("[PPS CLIENT] CF is now different for this student. Disconnect and turn it off");
422

423
424
425
426
            std_msgs::Int32 msg;
            msg.data = CMD_DISCONNECT;
            crazyRadioCommandPublisher.publish(msg);
        }
427

428
        context = new_context;
429

430
431
432
433
434
        ros::NodeHandle nh("CrazyRadio");
        nh.setParam("crazyFlieAddress", context.crazyflieAddress);
	}
    else
    {
435
		ROS_ERROR("[PPS CLIENT] Failed to load context. Waiting for next Save in DB by teacher");
436
	}
437
}
438

439

440
441
442

void commandCallback(const std_msgs::Int32& commandMsg) {
	int cmd = commandMsg.data;
443

444
445
	switch(cmd) {
    	case CMD_USE_SAFE_CONTROLLER:
446
            ROS_INFO("[PPS CLIENT] USE_SAFE_CONTROLLER Command received");
447
            setControllerUsed(SAFE_CONTROLLER);
448
449
    		break;

Paul Beuchat's avatar
Paul Beuchat committed
450
    	case CMD_USE_DEMO_CONTROLLER:
451
            ROS_INFO("[PPS CLIENT] USE_DEMO_CONTROLLER Command received");
Paul Beuchat's avatar
Paul Beuchat committed
452
            setControllerUsed(DEMO_CONTROLLER);
453
454
    		break;

455
456
457
458
459
460
461
462
463
464
        case CMD_USE_STUDENT_CONTROLLER:
            ROS_INFO("[PPS CLIENT] USE_STUDENT_CONTROLLER Command received");
            setControllerUsed(STUDENT_CONTROLLER);
            break;

        case CMD_USE_MPC_CONTROLLER:
            ROS_INFO("[PPS CLIENT] USE_MPC_CONTROLLER Command received");
            setControllerUsed(MPC_CONTROLLER);
            break;

465
466
467
468
469
        case CMD_USE_REMOTE_CONTROLLER:
            ROS_INFO("[PPS CLIENT] USE_REMOTE_CONTROLLER Command received");
            setControllerUsed(REMOTE_CONTROLLER);
            break;

470
471
472
473
474
        case CMD_USE_TUNING_CONTROLLER:
            ROS_INFO("[PPS CLIENT] USE_TUNING_CONTROLLER Command received");
            setControllerUsed(TUNING_CONTROLLER);
            break;

475
476
477
478
479
        case CMD_USE_PICKER_CONTROLLER:
            ROS_INFO("[PPS CLIENT] USE_PICKER_CONTROLLER Command received");
            setControllerUsed(PICKER_CONTROLLER);
            break;

pragash1's avatar
pragash1 committed
480
481
482
483
484
						//TODO
				case CMD_USE_DRONEX_CONTROLLER:
						ROS_INFO("[PPS CLIENT] USE_DRONEX_CONTROLLER Command received");
						setControllerUsed(DRONEX_CONTROLLER);
						break;
485
    	case CMD_CRAZYFLY_TAKE_OFF:
486
487
488
489
            if(flying_state == STATE_MOTORS_OFF)
            {
                changeFlyingStateTo(STATE_TAKE_OFF);
            }
490
491
    		break;

492
    	case CMD_CRAZYFLY_LAND:
493
494
495
496
            if(flying_state != STATE_MOTORS_OFF)
            {
                changeFlyingStateTo(STATE_LAND);
            }
497
    		break;
498
499
500
        case CMD_CRAZYFLY_MOTORS_OFF:
            changeFlyingStateTo(STATE_MOTORS_OFF);
            break;
501

502
    	default:
503
    		ROS_ERROR_STREAM("[PPS CLIENT] unexpected command number: " << cmd);
504
    		break;
505
	}
506
507
}

508
509
510
511
512
void DBChangedCallback(const std_msgs::Int32& msg)
{
    loadCrazyflieContext();
}

513
514
515
516
517
void emergencyStopCallback(const std_msgs::Int32& msg)
{
    commandCallback(msg);
}

518
519
520
521
522
void commandAllAgentsCallback(const std_msgs::Int32& msg)
{
    commandCallback(msg);
}

523
524
525
void crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
    crazyradio_status = msg.data;
526
527
528
529
530
    // RESET THE BATTERY STATE IF DISCONNECTED
    if (crazyradio_status == DISCONNECTED)
    {
        setBatteryStateTo(BATTERY_STATE_NORMAL);
    }
531
532
}

533
534
535
536
537
538
539
540
541
542
543
544
void controllerSetPointCallback(const Setpoint& newSetpoint)
{
    // load in variable the setpoint
    controller_setpoint = newSetpoint;

    // if we are in flying, set it up NOW
    if(flying_state == STATE_FLYING)
    {
        goToControllerSetpoint();
    }
}

545
546
547
548
void safeSetPointCallback(const Setpoint& newSetpoint)
{
}

549

550
551


552
553
554
555
556
557
558
559
560
561
562
563
564
//    ----------------------------------------------------------------------------------
//    L       OOO     A    DDDD
//    L      O   O   A A   D   D
//    L      O   O  A   A  D   D
//    L      O   O  AAAAA  D   D
//    LLLLL   OOO   A   A  DDDD
//
//    PPPP     A    RRRR     A    M   M  EEEEE  TTTTT  EEEEE  RRRR    SSSS
//    P   P   A A   R   R   A A   MM MM  E        T    E      R   R  S
//    PPPP   A   A  RRRR   A   A  M M M  EEE      T    EEE    RRRR    SSS
//    P      AAAAA  R  R   AAAAA  M   M  E        T    E      R  R       S
//    P      A   A  R   R  A   A  M   M  EEEEE    T    EEEEE  R   R  SSSS
//    ----------------------------------------------------------------------------------
565
566


567
568
569
570
571
void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
    // Extract from the "msg" for which controller the and from where to fetch the YAML
    // parameters
    int controller_to_fetch_yaml = msg.data;
572

573
574
575
    // Switch between fetching for the different controllers and from different locations
    switch(controller_to_fetch_yaml)
    {
pragash1's avatar
pragash1 committed
576

577
        // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
578
        case FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT:
579
        {
580
            // Let the user know that this message was received
581
            ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from this machine.");
582
583
            // Create a node handle to the parameter service running on this agent's machine
            ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
584
            // Call the function that fetches the parameters
585
            fetchYamlParametersForSafeController(nodeHandle_to_own_agent_parameter_service);
586
            break;
587
        }
588

589
        // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
590
        case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
591
        {
592
            // Let the user know that this message was received
593
            // > and also from where the paramters are being fetched
594
            ROS_INFO("[PPS CLIENT] Received the message that YAML parameters were (re-)loaded for the Safe Controller. > Now fetching the parameter values from the coordinator.");
595
            // Create a node handle to the parameter service running on the coordinator machine
596
            ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
597
            // Call the function that fetches the parameters
598
            fetchYamlParametersForSafeController(nodeHandle_to_coordinator_parameter_service);
599
            break;
600
        }
601
602

        default:
603
        {
604
            // Let the user know that the command was not relevant
605
            //ROS_INFO("The PPSClient received the message that YAML parameters were (re-)loaded.\r> However the parameters do not relate to this service, hence nothing will be fetched.");
606
            break;
607
        }
608
609
610
    }
}

611
612
613


void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle)
614
{
615
    // Here we load the parameters that are specified in the SafeController.yaml file
616

Paul Beuchat's avatar
Paul Beuchat committed
617
    // Add the "SafeController" namespace to the "nodeHandle"
618
    ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
619
620
621

    if(!nodeHandle_for_safeController.getParam("takeOffDistance", take_off_distance))
    {
622
        ROS_ERROR("[PPS CLIENT] Failed to get takeOffDistance");
623
624
625
626
    }

    if(!nodeHandle_for_safeController.getParam("landingDistance", landing_distance))
    {
627
        ROS_ERROR("[PPS CLIENT] Failed to get landing_distance");
628
629
630
631
    }

    if(!nodeHandle_for_safeController.getParam("durationTakeOff", duration_take_off))
    {
632
        ROS_ERROR("[PPS CLIENT] Failed to get duration_take_off");
633
    }
634

635
636
    if(!nodeHandle_for_safeController.getParam("durationLanding", duration_landing))
    {
637
        ROS_ERROR("[PPS CLIENT] Failed to get duration_landing");
638
639
640
641
    }

    if(!nodeHandle_for_safeController.getParam("distanceThreshold", distance_threshold))
    {
642
        ROS_ERROR("[PPS CLIENT] Failed to get distance_threshold");
643
    }
644
645
}

646
647
648

// > Load the paramters from the Client Config YAML file
void fetchClientConfigParameters(ros::NodeHandle& nodeHandle)
649
{
650
    if(!nodeHandle.getParam("agentID", agentID)) {
651
        ROS_ERROR("[PPS CLIENT] Failed to get agentID");
652
653
    }
    if(!nodeHandle.getParam("strictSafety", strictSafety)) {
654
        ROS_ERROR("[PPS CLIENT] Failed to get strictSafety param");
655
656
657
        return;
    }
    if(!nodeHandle.getParam("angleMargin", angleMargin)) {
658
        ROS_ERROR("[PPS CLIENT] Failed to get angleMargin param");
659
660
661
        return;
    }
    if(!nodeHandle.getParam("battery_threshold_while_flying", m_battery_threshold_while_flying)) {
662
        ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_flying param");
663
664
665
        return;
    }
    if(!nodeHandle.getParam("battery_threshold_while_motors_off", m_battery_threshold_while_motors_off)) {
666
        ROS_ERROR("[PPS CLIENT] Failed to get battery_threshold_while_motors_off param");
667
668
669
670
671
        return;
    }
}


672
673
674
675
676
677






Paul Beuchat's avatar
Paul Beuchat committed
678
679


beuchatp's avatar
beuchatp committed
680
void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg)
681
682
683
684
685
686
687
688
689
690
691
692
{
    // The "msg" received can be directly published on the "crazyRadioCommandPublisher"
    // class variable because it is same format message
    // > NOTE: this may be inefficient to "just" pass on the message,
    //   the intention is that it is more transparent that the "coordinator"
    //   node requests all agents to (re/dis)-connect from, and the
    //   individual agents pass this along to their respective radio node.
    crazyRadioCommandPublisher.publish(msg);
}



Paul Beuchat's avatar
Paul Beuchat committed
693
694
695
696
697
698






699
700
701
702
703
704
705
706
707
708
709
710
int getBatteryState()
{
    return m_battery_state;
}


void setBatteryStateTo(int new_battery_state)
{
    switch(new_battery_state)
    {
        case BATTERY_STATE_NORMAL:
            m_battery_state = BATTERY_STATE_NORMAL;
711
            ROS_INFO("[PPS CLIENT] changed battery state to normal");
712
713
714
            break;
        case BATTERY_STATE_LOW:
            m_battery_state = BATTERY_STATE_LOW;
715
            ROS_INFO("[PPS CLIENT] changed battery state to low");
roangel's avatar
roangel committed
716
717
            if(flying_state != STATE_MOTORS_OFF)
                changeFlyingStateTo(STATE_LAND);
718
719
            break;
        default:
720
            ROS_INFO("[PPS CLIENT] Unknown battery state command, set to normal");
721
722
723
724
725
726
727
728
729
            m_battery_state = BATTERY_STATE_NORMAL;
            break;
    }

    std_msgs::Int32 battery_state_msg;
    battery_state_msg.data = getBatteryState();
    batteryStatePublisher.publish(battery_state_msg);
}

730
731
732
float movingAverageBatteryFilter(float new_input)
{
    const int N = 7;
733
734
    static float previous_output = 4.2f;
    static float inputs[N] = {4.2f,4.2f,4.2f,4.2f,4.2f,4.2f,4.2f};
735
736


737
738
739
    // imagine an array of an even number of samples, we will output the one
    // in the middle averaged with information from all of them.
    // For that, we only need to store some past of the signal
740
741
742
743
744
745
746
    float output = previous_output + new_input/N - inputs[N-1]/N;

    // update array of inputs
    for(int i = N - 1; i > 0; i--)
    {
        inputs[i] = inputs[i-1];
    }
roangel's avatar
roangel committed
747
748
    inputs[0] = new_input;

749
750
751
752
753
754

    // update previous output
    previous_output = output;
    return output;
}

755

756
void CFBatteryCallback(const std_msgs::Float32& msg)
757
758
759
760
{
    m_battery_voltage = msg.data;
    // filter and check if inside limits, and if, change status
    // need to do the filtering first
761
    float filtered_battery_voltage = movingAverageBatteryFilter(m_battery_voltage); //need to perform filtering here
roangel's avatar
roangel committed
762

roangel's avatar
roangel committed
763
    // ROS_INFO_STREAM("filtered data: " << filtered_battery_voltage);
764
765
766
767
768
    if(
        (flying_state != STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_flying))
        ||
        (flying_state == STATE_MOTORS_OFF && (filtered_battery_voltage < m_battery_threshold_while_motors_off))
    )
769
    {
roangel's avatar
roangel committed
770
        if(getBatteryState() != BATTERY_STATE_LOW)
771
        {
roangel's avatar
roangel committed
772
            setBatteryStateTo(BATTERY_STATE_LOW);
773
774
            ROS_INFO("[PPS CLIENT] low level battery triggered");
        }
pragash1's avatar
pragash1 committed
775

776
    }
777
    else
778
    {
779
780
781
782
783
784
785
        // TO AVOID BEING ABLE TO IMMEDIATELY TAKE-OFF AFTER A
        // "BATTERY LOW" EVENT IS TRIGGERED, WE DO NOT SET THE
        // BATTERY STATE BACK TO NORMAL
        // if(getBatteryState() != BATTERY_STATE_NORMAL)
        // {
        //     setBatteryStateTo(BATTERY_STATE_NORMAL);
        // }
786
787
788
    }
}

789

Paul Beuchat's avatar
Paul Beuchat committed
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807













void loadSafeController() {
    ros::NodeHandle nodeHandle("~");

    std::string safeControllerName;
    if(!nodeHandle.getParam("safeController", safeControllerName)) {
808
        ROS_ERROR("[PPS CLIENT] Failed to get safe controller name");
Paul Beuchat's avatar
Paul Beuchat committed
809
810
811
812
813
        return;
    }

    ros::service::waitForService(safeControllerName);
    safeController = ros::service::createClient<Controller>(safeControllerName, true);
814
    ROS_INFO_STREAM("[PPS CLIENT] loaded safe controller: " << safeController.getService());
Paul Beuchat's avatar
Paul Beuchat committed
815
816
817
818
819
820
821
822
823
}

void loadDemoController()
{
    ros::NodeHandle nodeHandle("~");

    std::string demoControllerName;
    if(!nodeHandle.getParam("demoController", demoControllerName))
    {
824
        ROS_ERROR("[PPS CLIENT] Failed to get demo controller name");
Paul Beuchat's avatar
Paul Beuchat committed
825
826
827
828
        return;
    }

    demoController = ros::service::createClient<Controller>(demoControllerName, true);
829
    ROS_INFO_STREAM("[PPS CLIENT] Loaded demo controller " << demoController.getService());
Paul Beuchat's avatar
Paul Beuchat committed
830
831
}

832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
void loadStudentController()
{
    ros::NodeHandle nodeHandle("~");

    std::string studentControllerName;
    if(!nodeHandle.getParam("studentController", studentControllerName))
    {
        ROS_ERROR("[PPS CLIENT] Failed to get student controller name");
        return;
    }

    studentController = ros::service::createClient<Controller>(studentControllerName, true);
    ROS_INFO_STREAM("[PPS CLIENT] Loaded student controller " << studentController.getService());
}

void loadMpcController()
{
    ros::NodeHandle nodeHandle("~");

    std::string mpcControllerName;
    if(!nodeHandle.getParam("mpcController", mpcControllerName))
    {
        ROS_ERROR("[PPS CLIENT] Failed to get mpc controller name");
        return;
    }

    mpcController = ros::service::createClient<Controller>(mpcControllerName, true);
    ROS_INFO_STREAM("[PPS CLIENT] Loaded mpc controller " << mpcController.getService());
}
Paul Beuchat's avatar
Paul Beuchat committed
861

862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
void loadRemoteController()
{
    ros::NodeHandle nodeHandle("~");

    std::string remoteControllerName;
    if(!nodeHandle.getParam("remoteController", remoteControllerName))
    {
        ROS_ERROR("[PPS CLIENT] Failed to get remote controller name");
        return;
    }

    remoteController = ros::service::createClient<Controller>(remoteControllerName, true);
    ROS_INFO_STREAM("[PPS CLIENT] Loaded remote controller " << remoteController.getService());
}

877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
void loadTuningController()
{
    ros::NodeHandle nodeHandle("~");

    std::string tuningControllerName;
    if(!nodeHandle.getParam("tuningController", tuningControllerName))
    {
        ROS_ERROR("[PPS CLIENT] Failed to get tuning controller name");
        return;
    }

    tuningController = ros::service::createClient<Controller>(tuningControllerName, true);
    ROS_INFO_STREAM("[PPS CLIENT] Loaded tuning controller " << tuningController.getService());
}

892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
void loadPickerController()
{
    ros::NodeHandle nodeHandle("~");

    std::string pickerControllerName;
    if(!nodeHandle.getParam("pickerController", pickerControllerName))
    {
        ROS_ERROR("[PPS CLIENT] Failed to get picker controller name");
        return;
    }

    pickerController = ros::service::createClient<Controller>(pickerControllerName, true);
    ROS_INFO_STREAM("[PPS CLIENT] Loaded picker controller " << pickerController.getService());
}

pragash1's avatar
pragash1 committed
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
	// TODO
void loadDroneXController(){

	ros::NodeHandle nodeHandle("~");

	std::string droneXControllerName;
	if(!nodeHandle.getParam("droneXController", droneXControllerName))
	{
			ROS_ERROR("[PPS CLIENT] Failed to get droneX controller name");
			return;
	}

	droneXController = ros::service::createClient<Controller>(droneXControllerName, true);
	ROS_INFO_STREAM("[PPS CLIENT] Loaded droneX controller " << droneXController.getService());
}

Paul Beuchat's avatar
Paul Beuchat committed
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
void sendMessageUsingController(int controller)
{
    // send a message in topic for the studentGUI to read it
    std_msgs::Int32 controller_used_msg;
    controller_used_msg.data = controller;
    controllerUsedPublisher.publish(controller_used_msg);
}

void setInstantController(int controller) //for right now, temporal use
{
    instant_controller = controller;
    sendMessageUsingController(controller);
    switch(controller)
    {
        case SAFE_CONTROLLER:
            loadSafeController();
            break;
        case DEMO_CONTROLLER:
            loadDemoController();
            break;
943
944
945
946
947
948
        case STUDENT_CONTROLLER:
            loadStudentController();
            break;
        case MPC_CONTROLLER:
            loadMpcController();
            break;
949
950
951
        case REMOTE_CONTROLLER:
            loadRemoteController();
            break;
952
953
        case TUNING_CONTROLLER:
            loadTuningController();
954
955
        case PICKER_CONTROLLER:
            loadPickerController();
956
            break;
pragash1's avatar
pragash1 committed
957
958
				case DRONEX_CONTROLLER:
						loadDroneXController();
Paul Beuchat's avatar
Paul Beuchat committed
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
        default:
            break;
    }
}

int getInstantController()
{
    return instant_controller;
}

void setControllerUsed(int controller) //for permanent configs
{
    controller_used = controller;

    if(flying_state == STATE_MOTORS_OFF || flying_state == STATE_FLYING)
    {
        setInstantController(controller); //if motors OFF or STATE FLYING, transparent, change is instant
    }
}

int getControllerUsed()
{
    return controller_used;
}









992
993
994
995
996
997
998
999
//    ----------------------------------------------------------------------------------
//    M   M    A    III  N   N
//    MM MM   A A    I   NN  N
//    M M M  A   A   I   N N N
//    M   M  AAAAA   I   N  NN
//    M   M  A   A  III  N   N
//    ----------------------------------------------------------------------------------

1000
int main(int argc, char* argv[])