StudentControllerService.cpp 63.5 KB
Newer Older
beuchatp's avatar
beuchatp committed
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
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
//    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:
//    Place for students to implement their controller
//
//    ----------------------------------------------------------------------------------
31
32


33
34
35



36
37
// INCLUDE THE HEADER
#include "nodes/StudentControllerService.h"
38

39

40
41
42
43
44
45
46
47
48
49
50
51
52
//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------
roangel's avatar
roangel committed
53

54

roangel's avatar
roangel committed
55
56


roangel's avatar
roangel committed
57

58
59
60
61
62
63
64
65
66
67
68
69
70
//    ------------------------------------------------------------------------------
//     OOO   U   U  TTTTT  EEEEE  RRRR 
//    O   O  U   U    T    E      R   R
//    O   O  U   U    T    EEE    RRRR
//    O   O  U   U    T    E      R  R
//     OOO    UUU     T    EEEEE  R   R
//
//     CCCC   OOO   N   N  TTTTT  RRRR    OOO   L           L       OOO    OOO   PPPP
//    C      O   O  NN  N    T    R   R  O   O  L           L      O   O  O   O  P   P
//    C      O   O  N N N    T    RRRR   O   O  L           L      O   O  O   O  PPPP
//    C      O   O  N  NN    T    R  R   O   O  L           L      O   O  O   O  P
//     CCCC   OOO   N   N    T    R   R   OOO   LLLLL       LLLLL   OOO    OOO   P
//    ----------------------------------------------------------------------------------
roangel's avatar
roangel committed
71

72
// This function is the callback that is linked to the "StudentController" service that
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
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
150
151
152
153
154
155
156
// is advertised in the main function. This must have arguments that match the
// "input-output" behaviour defined in the "Controller.srv" file (located in the "srv"
// folder)
//
// The arument "request" is a structure provided to this service with the following two
// properties:
// request.ownCrazyflie
// This property is itself a structure of type "CrazyflieData",  which is defined in the
// file "CrazyflieData.msg", and has the following properties
// string crazyflieName
//     float64 x                         The x position of the Crazyflie [metres]
//     float64 y                         The y position of the Crazyflie [metres]
//     float64 z                         The z position of the Crazyflie [metres]
//     float64 roll                      The roll component of the intrinsic Euler angles [radians]
//     float64 pitch                     The pitch component of the intrinsic Euler angles [radians]
//     float64 yaw                       The yaw component of the intrinsic Euler angles [radians]
//     float64 acquiringTime #delta t    The time elapsed since the previous "CrazyflieData" was received [seconds]
//     bool occluded                     A boolean indicted whether the Crazyflie for visible at the time of this measurement
// The values in these properties are directly the measurement taken by the Vicon
// motion capture system of the Crazyflie that is to be controlled by this service
//
// request.otherCrazyflies
// This property is an array of "CrazyflieData" structures, what allows access to the
// Vicon measurements of other Crazyflies.
//
// The argument "response" is a structure that is expected to be filled in by this
// service by this function, it has only the following property
// response.ControlCommand
// This property is iteself a structure of type "ControlCommand", which is defined in
// the file "ControlCommand.msg", and has the following properties:
//     float32 roll                      The command sent to the Crazyflie for the body frame x-axis
//     float32 pitch                     The command sent to the Crazyflie for the body frame y-axis
//     float32 yaw                       The command sent to the Crazyflie for the body frame z-axis
//     uint16 motorCmd1                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd2                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd3                  The command sent to the Crazyflie for motor 1
//     uint16 motorCmd4                  The command sent to the Crazyflie for motor 1
//     uint8 onboardControllerType       The flag sent to the Crazyflie for indicating how to implement the command
// 
// IMPORTANT NOTES FOR "onboardControllerType"  AND AXIS CONVENTIONS
// > The allowed values for "onboardControllerType" are in the "Defines" section at the
//   top of this file, they are MOTOR_MODE, RATE_MODE, OR ANGLE_MODE.
// > In the PPS exercise we will only use the RATE_MODE.
// > In RATE_MODE the ".roll", ".ptich", and ".yaw" properties of "response.ControlCommand"
//   specify the angular rate in [radians/second] that will be requested from the
//   PID controllers running in the Crazyflie 2.0 firmware.
// > In RATE_MODE the ".motorCmd1" to ".motorCmd4" properties of "response.ControlCommand"
//   are the baseline motor commands requested from the Crazyflie, with the adjustment
//   for body rates being added on top of this in the firmware (i.e., as per the code
//   of the "distribute_power" function provided in exercise sheet 2).
// > In RATE_MODE the axis convention for the roll, pitch, and yaw body rates returned
//   in "response.ControlCommand" should use right-hand coordinate axes with x-forward
//   and z-upwards (i.e., the positive z-axis is aligned with the direction of positive
//   thrust). To assist, teh following is an ASCII art of this convention:
//
// ASCII ART OF THE CRAZYFLIE 2.0 LAYOUT
//
//  > This is a top view,
//  > M1 to M4 stand for Motor 1 to Motor 4,
//  > "CW"  indicates that the motor rotates Clockwise,
//  > "CCW" indicates that the motor rotates Counter-Clockwise,
//  > By right-hand axis convention, the positive z-direction points our of the screen,
//  > This being a "top view" means tha the direction of positive thrust produced
//    by the propellers is also out of the screen.
//
//        ____                         ____
//       /    \                       /    \
//  (CW) | M4 |           x           | M1 | (CCW)
//       \____/\          ^          /\____/
//            \ \         |         / /
//             \ \        |        / /
//              \ \______ | ______/ /
//               \        |        /
//                |       |       |
//        y <-------------o       |
//                |               |
//               / _______________ \
//              / /               \ \
//             / /                 \ \
//        ____/ /                   \ \____
//       /    \/                     \/    \
// (CCW) | M3 |                       | M2 | (CW)
//       \____/                       \____/
//
157
//
158
//
159
// The distributed controller 
160
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
161
162
163
164
165
166
167
168
{	
	switch(controller_state)
	{
		case 0:
		{
			// --------------------------------------
			// Motors Off
			// --------------------------------------
169
170
171
172
173
174
			if (outer_loop_counter == 0) 
			{
				ROS_INFO("[STUDENT CONTROLLER] computed Motors Off control command");
			}
			turn_motors_off(response);
			break;
175
176
177
178
179
180
		}
		case 1:
		{
			// --------------------------------------
			// Take-off Copters
			// --------------------------------------
Tobias's avatar
Tobias committed
181
182
183
184
185
			if (outer_loop_counter == 0) 
			{
				// current load position is origin
				load_setpoint[0] 	= request.otherCrazyflies[3].x;
				load_setpoint[1] 	= request.otherCrazyflies[3].y;
186
				load_setpoint[2] 	= 0.0;
187
188
189
				float yaw = request.otherCrazyflies[0].yaw;
				while(yaw > PI) {yaw -= 2 * PI;}
    			while(yaw < -PI) {yaw += 2 * PI;}
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
    			load_setpoint[3] 	= yaw;

    			// convert agent position w.r.t. the load into world frame
    			std::vector<float> distance_to_load_world;
    			if (my_agentID == 1)
    			{
    				distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P1 , 0.0, 0.0, yaw );
    			}
    			else if (my_agentID == 2)
    			{
    				distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P2 , 0.0, 0.0, yaw );
    			}
    			else if (my_agentID == 3)
    			{
    				distance_to_load_world = transformation_frame_load_to_world(attachmentPoint.P3 , 0.0, 0.0, yaw );
    			}

    			agent_setpoint[0] 	= load_setpoint[0] + distance_to_load_world[0];
				agent_setpoint[1] 	= load_setpoint[1] + distance_to_load_world[1];
				agent_setpoint[2] 	= load_setpoint[2] + takeOffinitial;
				agent_setpoint[3] 	= 0.0 ;
Tobias's avatar
Tobias committed
211
			}
212
			if (outer_loop_counter <= (int)(control_frequency*durationTakeOff) ) {
213
214
				agent_setpoint[2] += takeOffDistance_increment;
			}
215
216
217
218
			else
			{
				agent_setpoint[2] = takeOffDistance;
			}
219
220
			// compute agent's lqr command for the setpoint
			agent_lqr_command( request , response );
221
			
222
			break;
223
224
		}
		case 2:
225
226
227
228
229
		{
			// --------------------------------------
			// Distributed controller copter and load
			// --------------------------------------
			if (outer_loop_counter == 0) {
230
231
232
				// distributed controller was enabled
				ROS_INFO("[STUDENT CONTROLLER] Distributed controller enabled");	

233
				// load z-setpoint is current load z-position
234
235
				load_setpoint[0] = request.otherCrazyflies[3].x;	
				load_setpoint[1] = request.otherCrazyflies[3].y;	
236
				load_setpoint[2] = request.otherCrazyflies[3].z;			
237
238
			}

239
240
241
242
			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
				// set equilibrium load position to current load set point
243
244
245
246
247
248
249
250
251
				augmented_state_eq(7,0) 	= load_setpoint[0];
				augmented_state_eq(8,0) 	= load_setpoint[1];
				augmented_state_eq(9,0) 	= load_setpoint[2];
				augmented_state_eq(26,0) 	= load_setpoint[0];
				augmented_state_eq(27,0) 	= load_setpoint[1];
				augmented_state_eq(28,0) 	= load_setpoint[2];			
				augmented_state_eq(45,0) 	= load_setpoint[0];
				augmented_state_eq(46,0) 	= load_setpoint[1];
				augmented_state_eq(47,0) 	= load_setpoint[2];
252

253
				// update measurement vector using vicon Data
254
				get_y_from_CFData( request );
255

256
			}
257
258


259
			// compute distributed controller commands
260
			distributed_control_command( request, response );
Tobias's avatar
Tobias committed
261
262

			// compute agent's lqr command for the setpoint
263
			//agent_lqr_command( request , response );
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278

			break;
		}	
		case 3:
		{
			float delta_z_load_current = request.otherCrazyflies[3].z - landingDistance_load ;
			// --------------------------------------
			// Prepare to land the load
			// --------------------------------------
			if (outer_loop_counter == 0) {
				ROS_INFO("[STUDENT CONTROLLER] Prepare to land the load");	
				z_load_decrement = delta_z_load_current / ( control_frequency * durationLanding_load);

			}

Tobias's avatar
Tobias committed
279
			if (delta_z_load_current > 0.0)
280
281
			{
				// load has not reached landing distance
282
				if (outer_loop_counter <= (int)(control_frequency*durationLanding_load)) {
283
284
285
286
					load_setpoint[2] -= landingDistance_decrement;
				}
				else {
					// switch to landing copters
Tobias's avatar
Tobias committed
287
					controller_state = 4;
288
				}
289

290
291
			}

292
293
294
295
			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
				// set equilibrium load position to current load set point
296
297
298
299
300
301
302
303
304
				augmented_state_eq(7,0) 	= load_setpoint[0];
				augmented_state_eq(8,0) 	= load_setpoint[1];
				augmented_state_eq(9,0) 	= load_setpoint[2];
				augmented_state_eq(26,0) 	= load_setpoint[0];
				augmented_state_eq(27,0) 	= load_setpoint[1];
				augmented_state_eq(28,0) 	= load_setpoint[2];			
				augmented_state_eq(45,0) 	= load_setpoint[0];
				augmented_state_eq(46,0) 	= load_setpoint[1];
				augmented_state_eq(47,0) 	= load_setpoint[2];
305
306

				// update measurement vector using vicon Data
307
				get_y_from_CFData( request );
308

309
			}
310
311

			// compute distributed controller commands
312
			distributed_control_command ( request, response );
313

314
			break;
315
		}			
316
		case 4:
317
		{
318

319
			float delta_z_current = request.ownCrazyflie.z - landingDistance;
320
321
322
			// --------------------------------------
			// Landing Copters
			// --------------------------------------
323
324
325
			if ( outer_loop_counter == 0 ) {
				// Landing was initiated
				landingDistance_decrement = delta_z_current / ( control_frequency * durationLanding );
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
				// current load position is origin
				load_setpoint[0] 	= request.otherCrazyflies[3].x;
				load_setpoint[1] 	= request.otherCrazyflies[3].y;
				load_setpoint[2] 	= request.otherCrazyflies[3].z;
				float yaw = request.otherCrazyflies[0].yaw;
				while(yaw > PI) {yaw -= 2 * PI;}
    			while(yaw < -PI) {yaw += 2 * PI;}
    			load_setpoint[3] 	= yaw;

    			// convert agent position w.r.t. the load into world frame
    			std::vector<float> distance_to_load_world;
    			std::vector<float> landing_delta; 			// for safely flying the copters next to the laod 
    			if (my_agentID == 1)
    			{
    				landing_delta = {0.1 , 0.0, 0.0};
    				landing_delta[0] += attachmentPoint.P1[0];
    				landing_delta[1] += attachmentPoint.P1[1];
    				landing_delta[2] += attachmentPoint.P1[2];
    				distance_to_load_world = transformation_frame_load_to_world(landing_delta , 0.0, 0.0, yaw );
				
    			}
    			else if (my_agentID == 2)
    			{
    				landing_delta = {-0.1 , 0.1, 0.0};
     				landing_delta[0] += attachmentPoint.P2[0];
    				landing_delta[1] += attachmentPoint.P2[1];
    				landing_delta[2] += attachmentPoint.P2[2];
    				distance_to_load_world = transformation_frame_load_to_world(landing_delta, 0.0, 0.0, yaw );
    			}
    			else if (my_agentID == 3)
    			{
    				landing_delta = {-0.1 , -0.1, 0.0};
     				landing_delta[0] += attachmentPoint.P3[0];
    				landing_delta[1] += attachmentPoint.P3[1];
    				landing_delta[2] += attachmentPoint.P3[2];   				
    				distance_to_load_world = transformation_frame_load_to_world(landing_delta, 0.0, 0.0, yaw );
    			}

    			agent_setpoint[0] 	= load_setpoint[0] + distance_to_load_world[0];
				agent_setpoint[1] 	= load_setpoint[1] + distance_to_load_world[1];
				agent_setpoint[2] 	= request.ownCrazyflie.z;
				agent_setpoint[3] 	= 0.0 ;		
368

369
370
			}
			// check if copter is within landing distance
Tobias's avatar
Tobias committed
371
			if ( delta_z_current > 0.0 ) 
372
			{
373
				/// decrease z-setpoint
374
375
				if (outer_loop_counter <= (int)( control_frequency*durationLanding) ) 
				{
376
					agent_setpoint[2] -= landingDistance_decrement;
377
378
					// compute agent's lqr command for new setpoint
					agent_lqr_command( request , response );					
379
				}
380
381
				else 
				{
382
383
384
385
386
					// landing duration is over
					turn_motors_off( response );
					// set controller state to zero
					controller_state = 0;					
				}
387
			}
388
389
			else 
			{
390
391
				// copter is within landing distance
				turn_motors_off( response );
392
				// set controller state to zero
393
394
395
				controller_state = 0;
			}
			break;			
396
		}
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
		case 5:
		{
			// --------------------------------------
			// Centralized LQG controller copter and load
			// --------------------------------------
			if (outer_loop_counter == 0) {
				// distributed controller was enabled
				ROS_INFO("[STUDENT CONTROLLER] Centralized LQG enabled");	

				// load z-setpoint is current load z-position
				load_setpoint[0] = request.otherCrazyflies[3].x;	
				load_setpoint[1] = request.otherCrazyflies[3].y;	
				load_setpoint[2] = request.otherCrazyflies[3].z;			
			}

			// Distributed Controller runs at 100Hz --> downsampling by factor 2
			if (outer_loop_counter % 2 == 0)
			{
				// set equilibrium load position to current load set point
				state_eq(21,0) 	= load_setpoint[0];
				state_eq(22,0) 	= load_setpoint[1];
				state_eq(23,0) 	= load_setpoint[2];

				// update measurement vector using vicon Data
				get_y_from_CFData( request );

			}


			// compute centralized controller commands
			//centralized_control_command( request, response );

			// compute agent's lqr command for the setpoint
			agent_lqr_command( request , response );

			break;
		}			
434
435
436
437
438
	}
	outer_loop_counter += 1;
	return true;
}

439

440
void distributed_control_command( Controller::Request &request, Controller::Response &response)
441
{
442

443
444
445
	// Control output
	Eigen::Matrix<float,12,1> u = overlappingController.K * overlapping_estimator_state_prev;
	// Estimator update
446
	overlapping_estimator_state_prev = overlappingController.Phi * overlapping_estimator_state_prev + overlappingController.Gamma * y_to_controller;
447

448
	// extract motor command from u:
449
450
451
452
	float thrustSum 	= 0.0;
	float outRoll		= 0.0;
	float outPitch		= 0.0;
	float outYaw		= 0.0;
453

Tobias's avatar
same    
Tobias committed
454
455
	//if ( request.ownCrazyflie.crazyflieName == request.otherCrazyflies[0].crazyflieName )
	if (my_agentID == 1 )
456
457
	{
		// Controller for crazyflie 01
458
459
460
461
		thrustSum 	= u(0,0);
		outRoll		= u(1,0);
		outPitch	= u(2,0);
		outYaw		= u(3,0);
462
	}
Tobias's avatar
same    
Tobias committed
463
464
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[1].crazyflieName )
	else if (my_agentID == 2 )
465
466
	{
		// Controller for crazyflie 02
467
468
469
470
		thrustSum 	= u(4,0);
		outRoll		= u(5,0);
		outPitch	= u(6,0);
		outYaw		= u(7,0);
471
	}
Tobias's avatar
same    
Tobias committed
472
473
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[2].crazyflieName )
	else if (my_agentID == 3 )
474
475
	{
		// Controller for crazyflie 03
476
477
478
479
		thrustSum 	= u(8,0);
		outRoll		= u(9,0);
		outPitch	= u(10,0);
		outYaw		= u(11,0);	
480
481
	}

482
483
484
485
	if (outer_loop_counter % 50 == 0)
	{
		ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
	}
486
	
487
	// PREPARE AND RETURN THE VARIABLE "response"
488
	
Tobias's avatar
Tobias committed
489
490
491
492
493
494
495
    response.controlOutput.roll 		= outRoll;
    response.controlOutput.pitch 		= outPitch;
    response.controlOutput.yaw 			= outYaw;
    response.controlOutput.motorCmd1 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd2 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd3 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd4 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
496
  
497
498
	// choosing the Crazyflie onBoard controller type.
	// it can either be Motor, Rate or Angle based 
499
500
501
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;
502
503
504
}


505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
void centralized_control_command( Controller::Request &request, Controller::Response &response)
{

	// Control output
	Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev;
	// Estimator update
	estimator_state_prev = centralized_controller.Phi * estimator_state_prev + centralized_controller.Gamma * y_to_controller;

	// extract motor command from u:
	float thrustSum 	= 0.0;
	float outRoll		= 0.0;
	float outPitch		= 0.0;
	float outYaw		= 0.0;

	//if ( request.ownCrazyflie.crazyflieName == request.otherCrazyflies[0].crazyflieName )
	if (my_agentID == 1 )
	{
		// Controller for crazyflie 01
		thrustSum 	= u(0,0);
		outRoll		= u(1,0);
		outPitch	= u(2,0);
		outYaw		= u(3,0);
	}
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[1].crazyflieName )
	else if (my_agentID == 2 )
	{
		// Controller for crazyflie 02
		thrustSum 	= u(4,0);
		outRoll		= u(5,0);
		outPitch	= u(6,0);
		outYaw		= u(7,0);
	}
	//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[2].crazyflieName )
	else if (my_agentID == 3 )
	{
		// Controller for crazyflie 03
		thrustSum 	= u(8,0);
		outRoll		= u(9,0);
		outPitch	= u(10,0);
		outYaw		= u(11,0);	
	}

	if (outer_loop_counter % 50 == 0)
	{
		ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
	}
	
	// PREPARE AND RETURN THE VARIABLE "response"
	
    response.controlOutput.roll 		= outRoll;
    response.controlOutput.pitch 		= outPitch;
    response.controlOutput.yaw 			= outYaw;
    response.controlOutput.motorCmd1 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd2 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd3 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
    response.controlOutput.motorCmd4 	= computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
  
	// choosing the Crazyflie onBoard controller type.
	// it can either be Motor, Rate or Angle based 
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;
}

569
void get_y_from_CFData( Controller::Request &request  )
570
571
{
	// get load orientation for transformation to world frame
572
	float roll 	= request.otherCrazyflies[3].roll;
573
574
	float pitch = request.otherCrazyflies[3].pitch;
	float yaw 	= request.otherCrazyflies[3].yaw;
575

576

577
	std::vector<float> P1_world;
578
	P1_world = transformation_frame_load_to_world( attachmentPoint.P1, roll , pitch , yaw );
579
	std::vector<float> P2_world;
580
	P2_world = transformation_frame_load_to_world( attachmentPoint.P2, roll , pitch , yaw );	
581
	std::vector<float> P3_world;
582
	P3_world = transformation_frame_load_to_world( attachmentPoint.P3, roll , pitch , yaw );
583
584

	// y1 = [CF01.x , CF01.y , CF01.z, CF01.roll, CF01.pitch, CF01.yaw , len_cable*phi1 , len_cable*theta1 ]
585
	yaw = request.otherCrazyflies[0].yaw;
Tobias's avatar
Tobias committed
586
587
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}
588
589
590

    Eigen::Matrix<float,24,1> y_meas;

591
592
593
594
595
596
	y_meas(0,0) 	= request.otherCrazyflies[0].roll;
	y_meas(1,0) 	= request.otherCrazyflies[0].pitch;
	y_meas(2,0) 	= yaw;
	y_meas(3,0) 	= request.otherCrazyflies[0].x;
	y_meas(4,0) 	= request.otherCrazyflies[0].y;
	y_meas(5,0) 	= request.otherCrazyflies[0].z;
597
598
	y_meas(6,0) 	= request.otherCrazyflies[0].x - request.otherCrazyflies[3].x - P1_world[0];
	y_meas(7,0) 	= request.otherCrazyflies[0].y - request.otherCrazyflies[3].y - P1_world[1];
599
600

	// y2 = [CF02.x , CF02.y , CF02.z, CF02.roll, CF02.pitch, CF02.yaw , len_cable*phi2 , len_cable*theta2 ]
Tobias's avatar
Tobias committed
601
	yaw = request.otherCrazyflies[1].yaw;
Tobias's avatar
Tobias committed
602
603
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}
604
605
606
607
608
609
	y_meas(8,0) 	= request.otherCrazyflies[1].roll;
	y_meas(9,0) 	= request.otherCrazyflies[1].pitch;
	y_meas(10,0) 	= yaw;
	y_meas(11,0) 	= request.otherCrazyflies[1].x;
	y_meas(12,0) 	= request.otherCrazyflies[1].y;
	y_meas(13,0) 	= request.otherCrazyflies[1].z;
610
611
	y_meas(14,0) 	= request.otherCrazyflies[1].x - request.otherCrazyflies[3].x - P2_world[0];
	y_meas(15,0) 	= request.otherCrazyflies[1].y - request.otherCrazyflies[3].y - P2_world[1];
612
613

	// y3 = [CF03.x , CF03.y , CF03.z, CF03.roll, CF03.pitch, CF03.yaw , len_cable*phi3 , len_cable*theta3 ]
Tobias's avatar
Tobias committed
614
	yaw = request.otherCrazyflies[2].yaw;
Tobias's avatar
Tobias committed
615
616
	while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}	
617
618
619
620
621
622
	y_meas(16,0) 	= request.otherCrazyflies[2].roll;
	y_meas(17,0) 	= request.otherCrazyflies[2].pitch;
	y_meas(18,0) 	= yaw;
	y_meas(19,0) 	= request.otherCrazyflies[2].x;
	y_meas(20,0) 	= request.otherCrazyflies[2].y;
	y_meas(21,0) 	= request.otherCrazyflies[2].z;
623
624
625
	y_meas(22,0) 	= request.otherCrazyflies[2].x - request.otherCrazyflies[3].x - P3_world[0];
	y_meas(23,0) 	= request.otherCrazyflies[2].y - request.otherCrazyflies[3].y - P3_world[1];

626

627
628
	// compute measurement difference measured vs. set point
	// 								    y_meas - C * xi_equiliburium
629
630
631
632
633
634
635
636
637
638
639
640
	//Eigen::Matrix<float,24,1> y_eq = outputMatrix * augmented_state_eq;
	Eigen::Matrix<float,24,1> y_eq;
	y_eq.setZero();
	y_eq(3,0) 	= load_setpoint[0] + P1_world[0]; 				// equilibrium x-pose copter 1
	y_eq(4,0) 	= load_setpoint[1] + P1_world[1]; 				// equilibrium y-pose copter 1
	y_eq(5,0) 	= load_setpoint[2] + P1_world[2] + len_cable; 	// equilibrium z-pose copter 1
	y_eq(11,0) 	= load_setpoint[0] + P2_world[0]; 				// equilibrium x-pose copter 2
	y_eq(12,0) 	= load_setpoint[1] + P2_world[1]; 				// equilibrium y-pose copter 2
	y_eq(13,0) 	= load_setpoint[2] + P2_world[2] + len_cable;	// equilibrium z-pose copter 2
	y_eq(19,0) 	= load_setpoint[0] + P3_world[0]; 				// equilibrium x-pose copter 3
	y_eq(20,0) 	= load_setpoint[1] + P3_world[1]; 				// equilibrium y-pose copter 3
	y_eq(21,0) 	= load_setpoint[2] + P3_world[2] + len_cable;	// equilibrium z-pose copter 3
641
642

	// compute y w.r.t. equilibrium position, passed to controller
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
	y_to_controller = y_meas - y_eq;

	// Debugging
	if (outer_loop_counter % 50 == 0)
	{
		ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent :  " << my_agentID);
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );				
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );		
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_to_controller(3,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_to_controller(4,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_to_controller(5,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_to_controller(11,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_to_controller(12,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_to_controller(13,0) );		
		ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_to_controller(19,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_to_controller(20,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_to_controller(21,0) );
		ROS_INFO_STREAM("[STUDENT CONTROLLER] y_to_controller^T = " << y_to_controller.transpose() );		
	}	


}

666
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw )
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
{
	// get load orientation for transformation to world frame
	float cos_roll_load 	= cos(roll);
	float sin_roll_load 	= sin(roll);
	float cos_pitch_load 	= cos(pitch);
	float sin_pitch_load 	= sin(pitch);
	float cos_yaw_load 		= cos(yaw);
	float sin_yaw_load 		= sin(yaw);
	// xP
	float P_x_world = 0.0;
	P_x_world += cos_yaw_load*cos_pitch_load * P_load[0];
	P_x_world += ( -sin_yaw_load*cos_roll_load + cos_yaw_load*sin_pitch_load*sin_roll_load ) * P_load[1];
	P_x_world += ( sin_yaw_load*sin_roll_load + cos_yaw_load*sin_pitch_load*cos_roll_load ) * P_load[2];
	// yP
	float P_y_world = 0.0;
	P_y_world += sin_yaw_load*cos_pitch_load*P_load[0];
	P_y_world += ( cos_yaw_load*cos_roll_load + sin_yaw_load*sin_pitch_load*sin_roll_load ) * P_load[1];
	P_y_world += ( -cos_yaw_load*sin_yaw_load + sin_yaw_load*sin_pitch_load*cos_roll_load ) * P_load[2];
	// zP
	float P_z_world = 0.0;
	P_z_world += -sin_pitch_load * P_load[0];
	P_z_world +=  cos_pitch_load * sin_roll_load * P_load[1];
	P_z_world +=  cos_pitch_load * cos_roll_load * P_load[2];	

691
	std::vector<float> P_world = {0 ,0 ,0};
692
693
694
	P_world[0] = P_x_world;
	P_world[1] = P_y_world;
	P_world[2] = P_z_world;
695
696

	return P_world;
697
}
698
699
700


Eigen::MatrixXf readMatrix(std::string filename)
701
{
702
    int cols = 0, rows = 0;
Tobias's avatar
Tobias committed
703
    float buff[10000];
704
705
706
707
708
709
710
711
712
713

    // Read numbers from file into buffer.
    std::ifstream infile;
    infile.open(filename);
    while (! infile.eof())
        {
        std::string line;
        std::getline(infile, line);

        int temp_cols = 0;
714
        float temp_bufferVal = 0;
715
716
        std::stringstream stream(line);
        while(! stream.eof())
717
            stream >> buff[cols*rows+temp_cols++] ;
718
719
720
721
722
723
724
725
726
727
728
729

        if (temp_cols == 0)
            continue;

        if (cols == 0)
            cols = temp_cols;

        rows++;
        }

    infile.close();

730
    //rows--;
731
732
733
734
735
736
737
738

    // Populate matrix with numbers.
    Eigen::MatrixXf result(rows,cols);
    for (int i = 0; i < rows; i++)
        for (int j = 0; j < cols; j++)
            result(i,j) = buff[ cols*i+j ];

    return result;
739
 }
740

741
742
void turn_motors_off( Controller::Response &response ) 
{
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
	// Put the computed roll rate into the "response" variable
	response.controlOutput.roll = 0;
	response.controlOutput.pitch = 0;
	response.controlOutput.yaw = 0;
	response.controlOutput.motorCmd1 = 0;
	response.controlOutput.motorCmd2 = 0;
	response.controlOutput.motorCmd3 = 0;
	response.controlOutput.motorCmd4 = 0;

	
	// PREPARE AND RETURN THE VARIABLE "response"

	/*choosing the Crazyflie onBoard controller type.
	it can either be Motor, Rate or Angle based */
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;
760
}
761

762
763
void agent_lqr_command( Controller::Request &request , Controller::Response &response ) 
{
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
	float yaw_measured = request.ownCrazyflie.yaw;

    //move coordinate system to make setpoint origin
    request.ownCrazyflie.x -= agent_setpoint[0];
    request.ownCrazyflie.y -= agent_setpoint[1];
    request.ownCrazyflie.z -= agent_setpoint[2];
    float yaw = request.ownCrazyflie.yaw - agent_setpoint[3];

    while(yaw > PI) {yaw -= 2 * PI;}
    while(yaw < -PI) {yaw += 2 * PI;}
    request.ownCrazyflie.yaw = yaw;

    float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
    estimateState(request, est);
    float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
    convertIntoBodyFrame(est, state, yaw_measured);

    //calculate feedback
    float outRoll = 0;
    float outPitch = 0;
    float outYaw = 0;
    float thrustIntermediate = 0;
    for(int i = 0; i < 9; ++i) {
        outRoll -= gainMatrixRollRate[i] * state[i];
        outPitch -= gainMatrixPitchRate[i] * state[i];
        outYaw -= gainMatrixYawRate[i] * state[i];
        thrustIntermediate -= gainMatrixThrust[i] * state[i];
    }
792

793
	// PREPARE AND RETURN THE VARIABLE "response"
794
 	
795
796
797
    response.controlOutput.roll = outRoll;
    response.controlOutput.pitch = outPitch;
    response.controlOutput.yaw = outYaw;
798

799
800
801
802
    response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
    response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
    response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
    response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustIntermediate + gravity_force);
803
    
804

805
806
807
808
809
	/*choosing the Crazyflie onBoard controller type.
	it can either be Motor, Rate or Angle based */
	// response.controlOutput.onboardControllerType = MOTOR_MODE;
	response.controlOutput.onboardControllerType = RATE_MODE;
	// response.controlOutput.onboardControllerType = ANGLE_MODE;
810
811
}

812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828


//    ------------------------------------------------------------------------------
//    RRRR    OOO   TTTTT    A    TTTTT  EEEEE       III  N   N  TTTTT   OOO
//    R   R  O   O    T     A A     T    E            I   NN  N    T    O   O
//    RRRR   O   O    T    A   A    T    EEE          I   N N N    T    O   O
//    R  R   O   O    T    AAAAA    T    E            I   N  NN    T    O   O
//    R   R   OOO     T    A   A    T    EEEEE       III  N   N    T     OOO
//
//    BBBB    OOO   DDDD   Y   Y       FFFFF  RRRR     A    M   M  EEEEE
//    B   B  O   O  D   D   Y Y        F      R   R   A A   MM MM  E
//    BBBB   O   O  D   D    Y         FFF    RRRR   A   A  M M M  EEE
//    B   B  O   O  D   D    Y         F      R  R   AAAAA  M   M  E
//    BBBB    OOO   DDDD     Y         F      R   R  A   A  M   M  EEEEE
//    ----------------------------------------------------------------------------------

// The arguments for this function are as follows:
829
// stateInertial
830
831
// This is an array of length 9 with the estimates the error of of the following values
// relative to the sepcifed setpoint:
832
833
834
835
836
837
838
839
840
//     stateInertial[0]    x position of the Crazyflie relative to the inertial frame origin [meters]
//     stateInertial[1]    y position of the Crazyflie relative to the inertial frame origin [meters]
//     stateInertial[2]    z position of the Crazyflie relative to the inertial frame origin [meters]
//     stateInertial[3]    x-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
//     stateInertial[4]    y-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
//     stateInertial[5]    z-axis component of the velocity of the Crazyflie in the inertial frame [meters/second]
//     stateInertial[6]    The roll  component of the intrinsic Euler angles [radians]
//     stateInertial[7]    The pitch component of the intrinsic Euler angles [radians]
//     stateInertial[8]    The yaw   component of the intrinsic Euler angles [radians]
841
// 
842
// stateBody
843
// This is an empty array of length 9, this function should fill in all elements of this
844
845
// array with the same ordering as for the "stateInertial" argument, expect that the (x,y)
// position and (x,y) velocities are rotated into the body frame.
846
847
848
849
850
//
// yaw_measured
// This is the yaw component of the intrinsic Euler angles in [radians] as measured by
// the Vicon motion capture system
//
851
852

void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured)
853
{
854
855
856
857
858
859
860
861
862
863
864
865
866
867
    float sinYaw = sin(yaw_measured);
    float cosYaw = cos(yaw_measured);

    state[0] = est[0] * cosYaw + est[1] * sinYaw;
    state[1] = -est[0] * sinYaw + est[1] * cosYaw;
    state[2] = est[2];

    state[3] = est[3] * cosYaw + est[4] * sinYaw;
    state[4] = -est[3] * sinYaw + est[4] * cosYaw;
    state[5] = est[5];

    state[6] = est[6];
    state[7] = est[7];
    state[8] = est[8];
868
869
870
}


871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
//    ------------------------------------------------------------------------------
//    KALMAN FILTER
//    ----------------------------------------------------------------------------------
void estimateState(Controller::Request &request, float (&est)[9])
{
    // attitude
    est[6] = request.ownCrazyflie.roll;
    est[7] = request.ownCrazyflie.pitch;
    est[8] = request.ownCrazyflie.yaw;

    //velocity & filtering
    float ahat_x[6]; //estimator matrix times state (x, y, z, vx, vy, vz)
    ahat_x[0] = 0; ahat_x[1]=0; ahat_x[2]=0;
    ahat_x[3] = estimatorMatrix[0] * prevEstimate[0] + estimatorMatrix[1] * prevEstimate[3];
    ahat_x[4] = estimatorMatrix[0] * prevEstimate[1] + estimatorMatrix[1] * prevEstimate[4];
    ahat_x[5] = estimatorMatrix[0] * prevEstimate[2] + estimatorMatrix[1] * prevEstimate[5];
887

888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
    
    float k_x[6]; //filterGain times state
    k_x[0] = request.ownCrazyflie.x * filterGain[0];
    k_x[1] = request.ownCrazyflie.y * filterGain[1];
    k_x[2] = request.ownCrazyflie.z * filterGain[2];
    k_x[3] = request.ownCrazyflie.x * filterGain[3];
    k_x[4] = request.ownCrazyflie.y * filterGain[4];
    k_x[5] = request.ownCrazyflie.z * filterGain[5];
   
    est[0] = ahat_x[0] + k_x[0];
    est[1] = ahat_x[1] + k_x[1];
    est[2] = ahat_x[2] + k_x[2];
    est[3] = ahat_x[3] + k_x[3];
    est[4] = ahat_x[4] + k_x[4];
    est[5] = ahat_x[5] + k_x[5];

    memcpy(prevEstimate, est, 9 * sizeof(float));
}
906

Tobias's avatar
Tobias committed
907
908
909
910
911
912
std::string getcwd_string( void ) {
   char buff[PATH_MAX];
   getcwd( buff, PATH_MAX );
   std::string cwd( buff );
   return cwd;
}
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
//    ------------------------------------------------------------------------------
//    N   N  EEEEE  W     W   TTTTT   OOO   N   N        CCCC  M   M  DDDD
//    NN  N  E      W     W     T    O   O  NN  N       C      MM MM  D   D
//    N N N  EEE    W     W     T    O   O  N N N  -->  C      M M M  D   D
//    N  NN  E       W W W      T    O   O  N  NN       C      M   M  D   D
//    N   N  EEEEE    W W       T     OOO   N   N        CCCC  M   M  DDDD
//
//     CCCC   OOO   N   N  V   V  EEEEE  RRRR    SSSS  III   OOO   N   N
//    C      O   O  NN  N  V   V  E      R   R  S       I   O   O  NN  N
//    C      O   O  N N N  V   V  EEE    RRRR    SSS    I   O   O  N N N
//    C      O   O  N  NN   V V   E      R  R       S   I   O   O  N  NN
//     CCCC   OOO   N   N    V    EEEEE  R   R  SSSS   III   OOO   N   N
//    ----------------------------------------------------------------------------------

// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
float computeMotorPolyBackward(float thrust)
{
    return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
931
932
}

933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951




//    ----------------------------------------------------------------------------------
//    N   N  EEEEE  W     W        SSSS  EEEEE  TTTTT  PPPP    OOO   III  N   N  TTTTT
//    NN  N  E      W     W       S      E        T    P   P  O   O   I   NN  N    T
//    N N N  EEE    W     W        SSS   EEE      T    PPPP   O   O   I   N N N    T
//    N  NN  E       W W W            S  E        T    P      O   O   I   N  NN    T
//    N   N  EEEEE    W W         SSSS   EEEEE    T    P       OOO   III  N   N    T
//
//     GGG   U   U  III        CCCC    A    L      L      BBBB     A     CCCC  K   K
//    G   G  U   U   I        C       A A   L      L      B   B   A A   C      K  K
//    G      U   U   I        C      A   A  L      L      BBBB   A   A  C      KKK
//    G   G  U   U   I        C      AAAAA  L      L      B   B  AAAAA  C      K  K
//     GGGG   UUU   III        CCCC  A   A  LLLLL  LLLLL  BBBB   A   A   CCCC  K   K
//    ----------------------------------------------------------------------------------

// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
952
953
954
955
956
void setpointCallback(const Setpoint& newSetpoint)
{
    load_setpoint[0] = newSetpoint.x;
    load_setpoint[1] = newSetpoint.y;
    load_setpoint[2] = newSetpoint.z;
957
958
959
960
    load_setpoint[3] = newSetpoint.yaw;
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, x: "<< load_setpoint[0]  );
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, y: "<< load_setpoint[1]  );
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, z: "<< load_setpoint[2]  );    
Tobias's avatar
Tobias committed
961
	ROS_INFO_STREAM("[STUDENT CONTROLLER] new load setpoint, yaw: "<< load_setpoint[3]  ); 
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
992
993
994
995
996
997
998
999
1000
//    ----------------------------------------------------------------------------------
//     CCCC  U   U   SSSS  TTTTT   OOO   M   M
//    C      U   U  S        T    O   O  MM MM
//    C      U   U   SSS     T    O   O  M M M
//    C      U   U      S    T    O   O  M   M
//     CCCC   UUU   SSSS     T     OOO   M   M
//
//     CCCC   OOO   M   M  M   M    A    N   N  DDDD
//    C      O   O  MM MM  MM MM   A A   NN  N  D   D
//    C      O   O  M M M  M M M  A   A  N N N  D   D
//    C      O   O  M   M  M   M  AAAAA  N  NN  D   D
//     CCCC   OOO   M   M  M   M  A   A  N   N  DDDD
//    ----------------------------------------------------------------------------------

// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButton& commandReceived)
{
	// Extract the data from the message
	int custom_button_index   = commandReceived.button_index;
	float custom_command_code = commandReceived.command_code;

	// Switch between the button pressed
	switch(custom_button_index)
	{

		// > FOR CUSTOM BUTTON 1
		case 1:
		{
			// Let the user know that this part of the code was triggered
			ROS_INFO("[STUDENT CONTROLLER] Button 1 received in controller.");
			// Code here to respond to custom button 1
			break;
		}

		// > FOR CUSTOM BUTTON 2
		case 2:
For faster browsing, not all history is shown. View entire blame