FlyingAgentClient.cpp 53.7 KB
Newer Older
1
2
//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
3
//
4
5
6
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
7
8
9
//    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.
10
11
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
12
13
14
//    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.
15
//    
16
//    You should have received a copy of the GNU General Public License
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
//    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:
//    ROS node that manages the student's setup.
//
//    ----------------------------------------------------------------------------------

33

34
35
36



beuchatp's avatar
beuchatp committed
37
// INCLUDE THE HEADER
38
#include "nodes/FlyingAgentClient.h"
roangel's avatar
roangel committed
39

40

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



//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------
57

58
59


roangel's avatar
roangel committed
60

61
62


63
64
65
66
67
68
69
70
71
72
73
74
75
//    ----------------------------------------------------------------------------------
//    M   M   OOO   TTTTT  III   OOO   N   N
//    MM MM  O   O    T     I   O   O  NN  N
//    M M M  O   O    T     I   O   O  N N N
//    M   M  O   O    T     I   O   O  N  NN
//    M   M   OOO     T    III   OOO   N   N
//
//     CCCC    A    PPPP   TTTTT  U   U  RRRR   EEEEE
//    C       A A   P   P    T    U   U  R   R  E
//    C      A   A  PPPP     T    U   U  RRRR   EEE
//    C      AAAAA  P        T    U   U  R   R  E
//     CCCC  A   A  P        T     UUU   R   R  EEEEE
//    ----------------------------------------------------------------------------------
76

77
// CALLBACK RUN EVERY TIME NEW MOTION CAPTURE DATA IS RECEIVED
78
79
void viconCallback(const ViconData& viconData)
{
80
81
82
	// NOTE: THIS FUNCTION IS CALL AT THE FREQUENCY OF THE MOTION
	//       CAPTURE SYSTEM. HENCE ANY OPTERATIONS PERFORMED IN
	//       THIS FUNCTION MUST BE NON-BLOCKING.
83

84
85
86
	// Initialise a counter of consecutive frames of motion
	// capture data where the agent is occuled
	static int number_of_consecutive_occulsions = 0;
87

88
89
	// Initilise a variable for the pose data of this agent
	CrazyflieData poseDataForThisAgent;
90

91
92
93
94
	// Extract the pose data from the full motion capture array
	// NOTE: that if the return index is a negative then this
	//       indicates that the pose data was not found.
	m_poseDataIndex = getPoseDataForObjectNameWithExpectedIndex( viconData, m_context.crazyflieName , m_poseDataIndex , poseDataForThisAgent );
95
96


97
98
99
100
101
102
103
104
105
106
107
108
109
	// Detecting time-out of the motion capture data
	// > Update the flag
	m_isAvailable_mocap_data = true;
	// > Stop any previous instance that might still be running
	m_timer_mocap_timeout_check.stop();
	// > Set the period again (second argument is reset)
	m_timer_mocap_timeout_check.setPeriod( ros::Duration(yaml_mocap_timeout_duration), true);
	// > Start the timer again
	m_timer_mocap_timeout_check.start();


	// Only continue if:
	// (1) the pose for this agent was found, and
110
	// (2) the controllers are actually available
111
112
113
114
115
116
117
	// (2) the flying state is something other than MOTORS-OFF
	if (
		(m_poseDataIndex >= 0)
		and
		(m_controllers_avialble)
	)
	{
118

119
120
		// Only continue if:
		// (1) the agent is NOT occulded
121
122
		if(!poseDataForThisAgent.occluded)
		{
123
124
			// Update the flag
			m_isOcculded_mocap_data = false;
125
126
			// Reset the "consecutive occulsions" flag
			number_of_consecutive_occulsions = 0;
127

128
129
130
131

			// Only continue if:
			// (1) The state is different from MOTORS-OFF
			if (m_flying_state != STATE_MOTORS_OFF)
132
			{
133

134
135
				// Initliase the "Contrller" service call variable
				Controller controllerCall;
136

137
138
				// Fill in the pose data for this agent
				controllerCall.request.ownCrazyflie = poseDataForThisAgent;
139

140
141
				
				
142

143
				// PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
144
145
				if ( getInstantController() != DEFAULT_CONTROLLER )
				{
146
					if ( !safetyCheck_on_positionAndTilt(poseDataForThisAgent) )
147
					{
148
149
						setInstantController(DEFAULT_CONTROLLER);
						ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
150
151
					}
				}
152

153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
				// Initialise a local boolean success variable
				bool isSuccessful_controllerCall = false;

				// Call the controller service client
				isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);

				// Ensure success and enforce safety
				if(!isSuccessful_controllerCall)
				{
					// Let the user know that the controller call failed
					ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to call controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());

					// Switch to the default controller,
					// if it was not already the active controller
					if ( getInstantController() != DEFAULT_CONTROLLER )
					{
						// Set the DEFAULT controller to be active
						setInstantController(DEFAULT_CONTROLLER);
						// Try the controller call
						isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);
						// Inform the user is not successful
						if ( !isSuccessful_controllerCall )
						{
							ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Also failed to call the DEFAULT controller, valid: " << m_instant_controller_service_client->isValid() << ", exists: " << m_instant_controller_service_client->exists());
						}
					}
				}

				// Send the command to the CrazyRadio
				// > IF SUCCESSFUL
				if (isSuccessful_controllerCall)
				{
					m_commandForSendingToCrazyfliePublisher.publish(controllerCall.response.controlOutput);
				}
				// > ELSE SEND ZERO OUTPUT COMMAND
				else
				{
					// Send the command to turn the motors off
					sendZeroOutputCommandForMotors();
					// And change the state to motor-off
					requestChangeFlyingStateTo(STATE_MOTORS_OFF);
				}
195
196
197
198
199
			}
			else
			{
				// Send the command to turn the motors off
				sendZeroOutputCommandForMotors();
200
			} // END OF: "if (m_flying_state != STATE_MOTORS_OFF)"
201
202
203
204
205
206
207
208
209
210
211
212
213
214
		}
		else
		{
			// Increment the number of consective occulations
			number_of_consecutive_occulsions++;
			// Update the flag if this exceeds the threshold
			if (
				(!m_isOcculded_mocap_data)
				and
				(number_of_consecutive_occulsions > yaml_consecutive_occulsions_threshold)
			)
			{
				// Update the flag
				m_isOcculded_mocap_data = true;
215
216
				// Inform the user
				ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Agent was occluded for more than " << yaml_consecutive_occulsions_threshold << " consecutive frames." );
217
				// Send the command to turn the motors off
218
219
220
				sendZeroOutputCommandForMotors();
				// Update the flying state
				requestChangeFlyingStateTo( STATE_MOTORS_OFF );
221
			}
222
		} // END OF: "if(!poseDataForThisAgent.occluded)"
223

224
225
226
227
228
	}
	else
	{
		// Send the command to turn the motors off
		sendZeroOutputCommandForMotors();
229

230
	} // END OF: "if ( (m_poseDataIndex >= 0) and (m_controllers_avialble) )"
231

232
}
233

234
235


236
237


238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose_to_fill_in)
{
    // Initialise an integer for the index where the object
    // "name" is found
    // > Initialise an negative to indicate not found
    int object_index = -1;

    // Get the length of the "pose data array"
    int length_poseData = viconData.crazyflies.size();

    // If the "expected index" is non-negative and less than
    // the length of the data array, then attempt to check
    // for a name match
    if (
        (0 <= expected_index)
        and
        (expected_index < length_poseData)
    )
    {
        // Check if the names match
        if (viconData.crazyflies[expected_index].crazyflieName == m_context.crazyflieName)
        {
            object_index = expected_index;
261
        }
262
    }
263

264
265
266
267
268
269
270
271
272
273
274
275
276
    // If not found, then iterate the data array looking
    // for a name match
    if (object_index < 0)
    {
        for( int i=0 ; i<length_poseData ; i++ )
        {    
            // Check if the names match
            if(viconData.crazyflies[i].crazyflieName == m_context.crazyflieName)
            {
                object_index = i;
            }
        }
    }
277

278
279
280
281
282
    // If not found, then retrun, else fill in the pose data
    if (object_index < 0)
    {
        return object_index;
    }
283
284
    else
    {
285
286
287
288
        pose_to_fill_in = viconData.crazyflies[object_index];
        coordinatesToLocal(pose_to_fill_in);
        return object_index;
    }
289
}
290

291

292
293
void coordinatesToLocal(CrazyflieData& cf)
{
294
	// Get the area into a local variable
295
    AreaBounds area = m_context.localArea;
296
297

    // Shift the X-Y coordinates
298
299
300
301
    float originX = (area.xmin + area.xmax) / 2.0;
    float originY = (area.ymin + area.ymax) / 2.0;
    cf.x -= originX;
    cf.y -= originY;
302
303
304
305
306
307

    // Shift the Z coordinate
    // 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.z -= originZ;
308
}
309

310

311
312
313
void timerCallback_mocap_timeout_check(const ros::TimerEvent&)
{
	// Update the flag
314
	m_isAvailable_mocap_data = false;
315
316
317
318
319
320
321
322
	// Inform the user
	ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Motion Capture Data has been unavailable for " << yaml_mocap_timeout_duration << " seconds." );
	// Ensure that the motors are turned off
	if ( !(m_flying_state==STATE_MOTORS_OFF) )
	{
		// Send the command to turn the motors off
        sendZeroOutputCommandForMotors();
        // Update the flying state
323
        requestChangeFlyingStateTo( STATE_MOTORS_OFF );
324
325
326
327
328
329
330
331
	}
}


void sendZeroOutputCommandForMotors()
{
	ControlCommand zeroOutput = ControlCommand(); //everything set to zero
    zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
332
    m_commandForSendingToCrazyfliePublisher.publish(zeroOutput);
333
334
335
}


336
337
338



339
340
341
342
343
344
345
346
347
348
349
350
351
//    ----------------------------------------------------------------------------------
//     SSSS    A    FFFFF  EEEEE  TTTTT  Y   Y
//    S       A A   F      E        T     Y Y
//     SSS   A   A  FFF    EEE      T      Y
//        S  AAAAA  F      E        T      Y
//    SSSS   A   A  F      EEEEE    T      Y
//
//     CCCC  H   H  EEEEE   CCCC  K   K   SSSS
//    C      H   H  E      C      K  K   S
//    C      HHHHH  EEE    C      KKK     SSS
//    C      H   H  E      C      K  K       S
//     CCCC  H   H  EEEEE   CCCC  K   K  SSSS
//    ----------------------------------------------------------------------------------
352

353
354
// Checks if crazyflie is within allowed area
bool safetyCheck_on_positionAndTilt(CrazyflieData data)
355
{
356
357
    // Check on the X position
    if((data.x < m_context.localArea.xmin) or (data.x > m_context.localArea.xmax))
358
    {
359
360
        ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
        return false;
361
    }
362
363
    // Check on the Y position
    if((data.y < m_context.localArea.ymin) or (data.y > m_context.localArea.ymax))
364
    {
365
366
367
368
369
370
371
372
        ROS_INFO_STREAM("[FLYING AGENT CLIENT] y safety failed");
        return false;
    }
    // Check on the Z position
    if((data.z < m_context.localArea.zmin) or (data.z > m_context.localArea.zmax))
    {
        ROS_INFO_STREAM("[FLYING AGENT CLIENT] z safety failed");
        return false;
373
    }
374

375
376
377
378
379
380
381
382
383
384
    // Check the title angle (if required)
    // > The tilt anlge between the body frame and inertial frame z-axis is
    //   give by:
    //   tilt angle  =  1 / ( cos(roll)*cos(pitch) )
    // > But this would be too sensitve to a divide by zero error, so instead
    //   we just check if each angle separately exceeds the limit
    if(yaml_isEnabled_strictSafety)
    {
        // Check on the ROLL angle
        if(
385
            (data.roll > yaml_maxTiltAngle_for_strictSafety_radians)
386
            or
387
            (data.roll < -yaml_maxTiltAngle_for_strictSafety_radians)
388
389
390
391
392
393
394
        )
        {
            ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
            return false;
        }
        // Check on the PITCH angle
        if(
395
            (data.pitch > yaml_maxTiltAngle_for_strictSafety_radians)
396
            or
397
            (data.pitch < -yaml_maxTiltAngle_for_strictSafety_radians)
398
399
400
401
402
403
        )
        {
            ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
            return false;
        }
    }
404

405
406
407
    // If the code makes it to here then all the safety checks passed,
    // Hence return "true"
    return true;
408
409
}

410

411
412


413

414
415
416



417
418
419
420
421
422
423
424
425
426
427
428
429
//    ----------------------------------------------------------------------------------
//    FFFFF L      Y   Y  III  N   N   GGGG
//    F     L       Y Y    I   NN  N  G
//    FFF   L        Y     I   N N N  G
//    F     L        Y     I   N  NN  G   G
//    F     LLLLL    Y    III  N   N   GGGG
//
//     SSSS  TTTTT    A    TTTTT  EEEEE
//    S        T     A A     T    E
//     SSS     T    A   A    T    EEE
//        S    T    AAAAA    T    E
//    SSSS     T    A   A    T    EEEEE
//    ----------------------------------------------------------------------------------
430
431


432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
void requestChangeFlyingStateTo(int new_state)
{
    if(m_crazyradio_status != CRAZY_RADIO_STATE_CONNECTED)
    {
        ROS_INFO("[FLYING AGENT CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
        m_flying_state = STATE_MOTORS_OFF;
    }
    else
    {
        // Switch between the possible new states
        switch (new_state)
        {
        	case STATE_TAKE_OFF:
        	{
        		requestChangeFlyingStateToTakeOff();
        		break;
        	}

        	case STATE_FLYING:
        	{
        		// This should never be called
        		break;
        	}

        	case STATE_LAND:
        	{
        		requestChangeFlyingStateToLand();
        		break;
        	}

        	case STATE_MOTORS_OFF:
        	{
        		ROS_INFO("[FLYING AGENT CLIENT] Change state to MOTORS OFF");
        		m_flying_state = new_state;
        		break;
        	}

        	default:
        	{
        		ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Request state of " << new_state << " not recognised. Hence changing to MOTORS OFF.");
        		m_flying_state = new_state;
        		break;
        	}
        }
476

477
    }
beuchatp's avatar
beuchatp committed
478

479
480
481
    // Publish a message with the new flying state
    std_msgs::Int32 flying_state_msg;
    flying_state_msg.data = m_flying_state;
482
    m_flyingStatePublisher.publish(flying_state_msg);
483
}
beuchatp's avatar
beuchatp committed
484

485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
void requestChangeFlyingStateToTakeOff()
{
	// Only allow taking off from the MOTORS OFF state
	if (m_flying_state != STATE_MOTORS_OFF)
	{
		ROS_ERROR("[FLYING AGENT CLIENT] Request to TAKE OFF was not implemented because the current state is NOT the MOTORS OFF state.");
	}
	else
	{
		// Check that the Motion Capture data is available
		if ( m_isAvailable_mocap_data and !m_isOcculded_mocap_data )
		{
			// Check whether a "controller" take-off should
			// be performed, and that not already in the
			// "take-off" state
			if (
				(yaml_shouldPerfom_takeOff_with_defaultController)
				and
				(m_flying_state != STATE_TAKE_OFF)
			)
			{
				// Call the service offered by the default
				// controller for how long a take-off will take
				dfall_pkg::IntIntService requestManoeurveCall;
509
				requestManoeurveCall.request.data = DEFAULT_CONTROLLER_REQUEST_TAKEOFF;
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
				if(m_defaultController_requestManoeuvre.call(requestManoeurveCall))
				{
					// Extract the duration
					float take_off_duration = float(requestManoeurveCall.response.data) / 1000.0;
					// Start the timer
					// > Stop any previous instance
					m_timer_takeoff_complete.stop();
					// > Set the period again (second argument is reset)
					m_timer_takeoff_complete.setPeriod( ros::Duration(take_off_duration), true);
					// > Start the timer
					m_timer_takeoff_complete.start();
					// Inform the user
					ROS_INFO_STREAM("[FLYING AGENT CLIENT] Changed state to STATE_TAKE_OFF for a duration of " << take_off_duration << " seconds.");
					// Update the class variable
					m_flying_state = STATE_TAKE_OFF;
525
526
					// Set the Default controller as the instant controller
					setInstantController(DEFAULT_CONTROLLER);
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
				}
				else
				{
					// Inform the user
					ROS_INFO("[FLYING AGENT CLIENT] Failed to get take-off duration from Default controller. Switching to MOTORS-OFF.");
					// Update the class variable
					m_flying_state = STATE_MOTORS_OFF;
				}
			}
			// Otherwise, just switch straight to the
			// "flying" state
			else
			{
				// Inform the user
				ROS_INFO("[FLYING AGENT CLIENT] Changed state directly to STATE_FLYING");
				// Update the class variable
				m_flying_state = STATE_FLYING;
			}
		}
		else
		{
			// Inform the user of the problem
			if (!m_isAvailable_mocap_data)
			{
				ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the motion capture data is unavailable.");
			}
			if (m_isOcculded_mocap_data)
			{
				ROS_ERROR("[FLYING AGENT CLIENT] Take-off not possible because the object is \"long-term\" occuled.");
			}
		}
	}
}
beuchatp's avatar
beuchatp committed
560
561


562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
void requestChangeFlyingStateToLand()
{
	// Only allow landing from the TAKE-OFF and FLYING state
	if (
		(m_flying_state != STATE_TAKE_OFF)
		and
		(m_flying_state != STATE_FLYING)
	)
	{
		ROS_ERROR("[FLYING AGENT CLIENT] Request to LAND was not implemented because the current state is NOT the TAKE-OFF or FLYING state.");
	}
	else
	{
		// Check whether a "controller" take-off should
		// be performed, and that not already in the
		// "land" state
		if (
			(yaml_shouldPerfom_landing_with_defaultController)
			and
			(m_flying_state != STATE_LAND)
		)
		{
			// Call the service offered by the default
			// controller for how long a landing will take
			dfall_pkg::IntIntService requestManoeurveCall;
			requestManoeurveCall.request.data = DEFAULT_CONTROLLER_REQUEST_LANDING;
			if(m_defaultController_requestManoeuvre.call(requestManoeurveCall))
			{
				// Extract the duration
				float land_duration = float(requestManoeurveCall.response.data) / 1000.0;
				// Start the timer
				// > Stop any previous instance
				m_timer_land_complete.stop();
				// > Set the period again (second argument is reset)
596
				m_timer_land_complete.setPeriod( ros::Duration(land_duration), true);
597
598
599
				// > Start the timer
				m_timer_land_complete.start();
				// Inform the user
600
				ROS_INFO_STREAM("[FLYING AGENT CLIENT] Changed state to STATE_LAND for a duration of " << land_duration << " seconds.");
601
602
				// Update the class variable
				m_flying_state = STATE_LAND;
603
604
				// Set the Default controller as the instant controller
				setInstantController(DEFAULT_CONTROLLER);
605
606
607
608
			}
			else
			{
				// Inform the user
609
				ROS_INFO("[FLYING AGENT CLIENT] Failed to get land duration from Default controller. Switching to MOTORS-OFF.");
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
				// Update the class variable
				m_flying_state = STATE_MOTORS_OFF;
			}
		}
		// Otherwise, just switch straight to the
		// "motors off" state
		else
		{
			// Inform the user
			ROS_INFO("[FLYING AGENT CLIENT] Changed state directly to STATE_MOTORS_OFF");
			// Update the class variable
			m_flying_state = STATE_MOTORS_OFF;
		}
	}
}
beuchatp's avatar
beuchatp committed
625

626

627
628
629
630
631
632
633
634
635
void timerCallback_takeoff_complete(const ros::TimerEvent&)
{
    // Only change to flying if still in the take-off state
    if (m_flying_state == STATE_TAKE_OFF)
	{
		// Inform the user
		ROS_INFO("[FLYING AGENT CLIENT] Take-off complete, changed state to STATE_FLYING");
		// Update the class variable
		m_flying_state = STATE_FLYING;
636
637
638
639
		// Publish a message with the new flying state
		std_msgs::Int32 flying_state_msg;
		flying_state_msg.data = m_flying_state;
		m_flyingStatePublisher.publish(flying_state_msg);
640
641
		// Change back to the nominal controller
		setInstantController( m_controller_nominally_selected );
642
643
644
645
646
647
648
	}
	else
	{
		// Inform the user
		ROS_INFO("[FLYING AGENT CLIENT] Take-off duration elapsed but the agent is no longer in STATE_TAKE_OFF.");
	}
}
649

650
void timerCallback_land_complete(const ros::TimerEvent&)
651
{
652
653
654
655
656
657
658
    // Only change to flying if still in the take-off state
    if (m_flying_state == STATE_LAND)
	{
		// Inform the user
		ROS_INFO("[FLYING AGENT CLIENT] Land complete, changed state to STATE_MOTORS_OFF");
		// Update the class variable
		m_flying_state = STATE_MOTORS_OFF;
659
660
661
662
		// Publish a message with the new flying state
		std_msgs::Int32 flying_state_msg;
		flying_state_msg.data = m_flying_state;
		m_flyingStatePublisher.publish(flying_state_msg);
663
664
		// Change back to the nominal controller
		setInstantController( m_controller_nominally_selected );
665
666
667
668
669
670
671
672
	}
	else
	{
		// Inform the user
		ROS_INFO("[FLYING AGENT CLIENT] Land duration elapsed but the agent is no longer in STATE_LAND.");
	}
}

beuchatp's avatar
beuchatp committed
673
674
675
676
677





678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699








//    ----------------------------------------------------------------------------------
//     SSSS  EEEEE  L      EEEEE   CCCC  TTTTT
//    S      E      L      E      C        T
//     SSS   EEE    L      EEE    C        T
//        S  E      L      E      C        T
//    SSSS   EEEEE  LLLLL  EEEEE   CCCC    T
//
//     CCCC   OOO   N   N  TTTTT  RRRR    OOO   L      L      EEEEE  RRRR
//    C      O   O  NN  N    T    R   R  O   O  L      L      E      R   R
//    C      O   O  N N N    T    RRRR   O   O  L      L      EEE    RRRR
//    C      O   O  N  NN    T    R   R  O   O  L      L      E      R   R
//     CCCC   OOO   N   N    T    R   R   OOO   LLLLL  LLLLL  EEEEE  R   R
//    ----------------------------------------------------------------------------------

700

701
702
703
704
705
706
// THIS SETS THE CONTROLLER THAT IT ACTUALLY BEING USED
// > During take-off and landing this function is
//   called to switch to the "Default" controller
// > The highlighting in the "Flying Agent GUI" should
//   reflect the instant controller   
void setInstantController(int controller)
707
{
708
709
710
    // Update the class variable
    m_instant_controller = controller;

711
    // Point to the correct service client
712
    switch(controller)
713
    {
714
        case DEFAULT_CONTROLLER:
715
            m_instant_controller_service_client = &m_defaultController;
716
717
            break;
        case STUDENT_CONTROLLER:
718
            m_instant_controller_service_client = &m_studentController;
719
720
            break;
        case TUNING_CONTROLLER:
721
            m_instant_controller_service_client = &m_tuningController;
722
723
            break;
        case PICKER_CONTROLLER:
724
            m_instant_controller_service_client = &m_pickerController;
725
726
            break;
        case TEMPLATE_CONTROLLER:
727
            m_instant_controller_service_client = &m_templateController;
728
729
730
            break;
        default:
            break;
731
732
    }

733
734
735
736
    // Publish a message that informs the "Flying Agent
    // GUI" about this update to the instant controller
    std_msgs::Int32 controller_used_msg;
    controller_used_msg.data = controller;
737
    m_controllerUsedPublisher.publish(controller_used_msg);
738
739
}

740
741

// THIS SIMPLY RETRIEVES THE CLASS VARIABLE
742
int getInstantController()
743
{
744
745
746
    return m_instant_controller;
}

747
748
749
750
751
752
753
754

// THIS SETS THE CONTROLLER THAT IS NOMINALLY SELECTED
// > This is the controller that will be use as the
//   "instant controller" when in the "flying" state.
// > But during take-off and landing, the "Default"
//   controller is used, and this keeps track of which
//   controller to switch to after those phases are
//   complete
755
756
757
758
void setControllerNominallySelected(int controller)
{
    // Update the class variable
    m_controller_nominally_selected = controller;
759

760
761
    // If in state "MOTORS-OFF" or "FLYING",
    // then the change is instant.
762
763
764
765
766
    if (
    	(m_flying_state == STATE_MOTORS_OFF)
    	or
    	(m_flying_state == STATE_FLYING)
	)
767
768
    {

769
770
        setInstantController(controller); 
    }
771
}
beuchatp's avatar
beuchatp committed
772

773
774

// THIS SIMPLY RETRIEVES THE CLASS VARIABLE
775
int getControllerNominallySelected()
776
{
777
778
779
780
781
    return m_controller_nominally_selected;
}



782

783
784
785
786
787
788






789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
//    ----------------------------------------------------------------------------------
//     SSSS  TTTTT    A    TTTTT  EEEEE
//    S        T     A A     T    E
//     SSS     T    A   A    T    EEE
//        S    T    AAAAA    T    E
//    SSSS     T    A   A    T    EEEEE
//
//     CCCC    A    L      L      BBBB     A     CCCC  K   K
//    C       A A   L      L      B   B   A A   C      K  K
//    C      A   A  L      L      BBBB   A   A  C      KKK
//    C      AAAAA  L      L      B   B  AAAAA  C      K  K
//     CCCC  A   A  LLLLL  LLLLL  BBBB   A   A   CCCC  K   K
//
//    FFFFF  RRRR    OOO   M   M      GGGG  U   U  III
//    F      R   R  O   O  MM MM     G      U   U   I
//    FFF    RRRR   O   O  M M M     G      U   U   I
//    F      R   R  O   O  M   M     G   G  U   U   I
//    F      R   R   OOO   M   M      GGGG   UUU   III
//    ----------------------------------------------------------------------------------



// THE CALLBACK THAT A NEW FLYING STATE WAS REQUESTED
// > These requests come from the "Flying Agent GUI"
// > The options are: {take-off,land,motor-off,controller}
814
815
816
817
818
819
820
void flyingStateRequestCallback(const IntWithHeader & msg) {

    // Check whether the message is relevant
    bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );

    // Continue if the message is relevant
    if (isRevelant)
821
    {
822
823
824
825
826
827
828
829
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
861
862
863
864
865
866
867
        // Extract the data
        int cmd = msg.data;

        switch(cmd) {
            case CMD_USE_DEFAULT_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_DEFAULT_CONTROLLER Command received");
                setControllerNominallySelected(DEFAULT_CONTROLLER);
                break;

            case CMD_USE_DEMO_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_DEMO_CONTROLLER Command received");
                setControllerNominallySelected(DEMO_CONTROLLER);
                break;

            case CMD_USE_STUDENT_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_STUDENT_CONTROLLER Command received");
                setControllerNominallySelected(STUDENT_CONTROLLER);
                break;

            case CMD_USE_MPC_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_MPC_CONTROLLER Command received");
                setControllerNominallySelected(MPC_CONTROLLER);
                break;

            case CMD_USE_REMOTE_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_REMOTE_CONTROLLER Command received");
                setControllerNominallySelected(REMOTE_CONTROLLER);
                break;

            case CMD_USE_TUNING_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_TUNING_CONTROLLER Command received");
                setControllerNominallySelected(TUNING_CONTROLLER);
                break;

            case CMD_USE_PICKER_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_PICKER_CONTROLLER Command received");
                setControllerNominallySelected(PICKER_CONTROLLER);
                break;

            case CMD_USE_TEMPLATE_CONTROLLER:
                ROS_INFO("[FLYING AGENT CLIENT] USE_TEMPLATE_CONTROLLER Command received");
                setControllerNominallySelected(TEMPLATE_CONTROLLER);
                break;

            case CMD_CRAZYFLY_TAKE_OFF:
                ROS_INFO("[FLYING AGENT CLIENT] TAKE_OFF Command received");
868
                requestChangeFlyingStateTo(STATE_TAKE_OFF);
869
870
871
872
                break;

            case CMD_CRAZYFLY_LAND:
                ROS_INFO("[FLYING AGENT CLIENT] LAND Command received");
873
                requestChangeFlyingStateTo(STATE_LAND);
874
875
876
                break;
            case CMD_CRAZYFLY_MOTORS_OFF:
                ROS_INFO("[FLYING AGENT CLIENT] MOTORS_OFF Command received");
877
                requestChangeFlyingStateTo(STATE_MOTORS_OFF);
878
879
880
881
882
883
                break;

            default:
                ROS_ERROR_STREAM("[FLYING AGENT CLIENT] unexpected command number: " << cmd);
                break;
        }
884
    }
885
886
887
}


888

889
890
891
892
893
894
895
896





void crazyRadioStatusCallback(const std_msgs::Int32& msg)
{
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] Received message with Crazy Radio Status = " << msg.data );
897
    m_crazyradio_status = msg.data;
898
899
}

900
901
902



903
904


905
906
907
908
void emergencyStopReceivedCallback(const IntWithHeader & msg)
{
    ROS_INFO("[FLYING AGENT CLIENT] Received message to EMERGENCY STOP");
    flyingStateRequestCallback(msg);
909
910
}

911
912
913



beuchatp's avatar
beuchatp committed
914
915


916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
//    ----------------------------------------------------------------------------------
//     GGGG  EEEEE  TTTTT      SSSS  TTTTT    A    TTTTT  U   U   SSSS
//    G      E        T       S        T     A A     T    U   U  S
//    G      EEE      T        SSS     T    A   A    T    U   U   SSS
//    G   G  E        T           S    T    AAAAA    T    U   U      S
//     GGGG  EEEEE    T       SSSS     T    A   A    T     UUU   SSSS
//
//     CCCC    A    L      L      BBBB     A     CCCC  K   K   SSSS
//    C       A A   L      L      B   B   A A   C      K  K   S
//    C      A   A  L      L      BBBB   A   A  C      KKK     SSS
//    C      AAAAA  L      L      B   B  AAAAA  C      K  K       S
//     CCCC  A   A  LLLLL  LLLLL  BBBB   A   A   CCCC  K   K  SSSS
//    ----------------------------------------------------------------------------------

bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response)
beuchatp's avatar
beuchatp committed
931
{
932
933
934
935
    // Put the flying state into the response variable
    response.data = m_flying_state;
    // Return
    return true;
beuchatp's avatar
beuchatp committed
936
937
938
}


939
940
941
942
943
944
945
946
bool getInstantControllerServiceCallback(IntIntService::Request &request, IntIntService::Response &response)
{
    // Put the flying state into the response variable
    response.data = getInstantController();
    // Return
    return true;
}

beuchatp's avatar
beuchatp committed
947

948
949


950
//    ----------------------------------------------------------------------------------
951
952
953
954
955
//    BBBB     A    TTTTT  TTTTT  EEEEE  RRRR   Y   Y
//    B   B   A A     T      T    E      R   R   Y Y
//    BBBB   A   A    T      T    EEE    RRRR     Y
//    B   B  AAAAA    T      T    E      R   R    Y
//    BBBB   A   A    T      T    EEEEE  R   R    Y
956
957
//    ----------------------------------------------------------------------------------

958
959
960
961
void batteryMonitorStateChangedCallback(std_msgs::Int32 msg)
{
    // Extract the data
    int new_battery_state = msg.data;
962

963
    // Take action if changed to low battery state
964
    if (new_battery_state == BATTERY_STATE_LOW)
965
    {
966
        if (m_flying_state != STATE_MOTORS_OFF)
967
        {
968
            ROS_INFO("[FLYING AGENT CLIENT] low level battery triggered, now landing.");
969
            requestChangeFlyingStateTo(STATE_LAND);
970
971
972
        }
        else
        {
973
            ROS_INFO("[FLYING AGENT CLIENT] low level battery triggered, please turn off the Crazyflie.");
974
        }
975
    }
976
    else if (new_battery_state == BATTERY_STATE_NORMAL)
977
    {
978
        ROS_INFO("[FLYING AGENT CLIENT] received message that battery state changed to normal");
979
    }
980
981
    else
    {
982
        ROS_INFO("[FLYING AGENT CLIENT] received message that battery state changed to something unknown");
983
984
    }
    
985
}
986

987

988
989


990

991
992


993
994
995
996
997





998
999
1000
1001
1002
1003
1004
1005
1006
1007
1008
1009
1010
1011

//    ----------------------------------------------------------------------------------
//    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
//
//     CCCC   OOO   N   N  TTTTT  EEEEE  X   X  TTTTT
//    C      O   O  NN  N    T    E       X X     T
//    C      O   O  N N N    T    EEE      X      T
//    C      O   O  N  NN    T    E       X X     T
//     CCCC   OOO   N   N    T    EEEEE  X   X    T
//    ----------------------------------------------------------------------------------
1012
1013


1014
1015
1016
1017
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
{
    ROS_INFO("[FLYING AGENT CLIENT] Received message that the Context Database Changed");
    loadCrazyflieContext();
1018
1019
1020
}


1021
1022

void loadCrazyflieContext()
1023
{
1024
1025
1026
    CMQuery contextCall;
    contextCall.request.studentID = m_agentID;
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] AgentID:" << m_agentID);
1027

1028
    CrazyflieContext new_context;
1029

1030
    m_centralManager.waitForExistence(ros::Duration(-1));
1031

1032
    if(m_centralManager.call(contextCall)) {
1033
1034
        new_context = contextCall.response.crazyflieContext;
        ROS_INFO_STREAM("[FLYING AGENT CLIENT] CrazyflieContext:\n" << new_context);
1035

1036
1037
        if((m_context.crazyflieName != "") && (new_context.crazyflieName != m_context.crazyflieName)) //linked crazyflie name changed and it was not empty before
        {
1038

1039
            // Motors off is done in python script now everytime we disconnect
1040

1041
1042
1043
1044
            // send motors OFF and disconnect before setting m_context = new_context
            // std_msgs::Int32 msg;
            // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
            // commandPublisher.publish(msg);
1045

1046
            ROS_INFO("[FLYING AGENT CLIENT] CF is now different for this student. Disconnect and turn it off");
1047

1048
1049
1050
            IntWithHeader msg;
            msg.shouldCheckForAgentID = false;
            msg.data = CMD_DISCONNECT;
1051
            m_crazyRadioCommandPublisher.publish(msg);
1052
        }
1053

1054
        m_context = new_context;
1055

1056
1057
1058
1059
1060
1061
1062
        ros::NodeHandle nh("CrazyRadio");
        nh.setParam("crazyFlieAddress", m_context.crazyflieAddress);
    }
    else
    {
        ROS_ERROR("[FLYING AGENT CLIENT] Failed to load context. Waiting for next Save in DB by teacher");
    }
1063
1064
1065
1066
1067
1068
}





1069
1070
1071
1072
1073
1074
1075
1076
1077
1078
1079
1080
1081
1082
1083
1084
1085
1086
1087
1088
1089
1090
1091
1092
1093
1094
1095
1096
1097
1098
1099
1100
1101
1102
//    ----------------------------------------------------------------------------------
//     CCCC  RRRR   EEEEE    A    TTTTT  EEEEE
//    C      R   R  E       A A     T    E
//    C      RRRR   EEE    A   A    T    EEE
//    C      R   R  E      AAAAA    T    E
//     CCCC  R   R  EEEEE  A   A    T    EEEEE
//
//     SSSS  EEEEE  RRRR   V   V  III   CCCC  EEEEE
//    S      E      R   R  V   V   I   C      E
//     SSS   EEE    RRRR   V   V   I   C      EEE
//        S  E      R   R   V V    I   C      E
//    SSSS   EEEEE  R   R    V    III   CCCC  EEEEE
//
//     CCCC  L      III  EEEEE  N   N  TTTTT
//    C      L       I   E      NN  N    T
//    C      L       I   EEE    N N N    T
//    C      L       I   E      N  NN    T
//     CCCC  LLLLL  III  EEEEE  N   N    T
//    ----------------------------------------------------------------------------------


// CREATE A "CONTROLLER" TYPE SERVICE CLIENT
// NOTE: that in the "ros::service::createClient" function the
//       second argument is a boolean that specifies whether the 
//       service is persistent or not. In the ROS documentation a
//       persistent connection is described as:
//   "Persistent connections should be used carefully. They greatly
//    improve performance for repeated requests, but they also make
//    your client more fragile to service failures. Clients using
//    persistent connections should implement their own reconnection
//    logic in the event that the persistent connection fails."
void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
{
    ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
1103
    ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "FlyingAgentClientConfig");
1104
1105
1106
1107
1108
1109
1110
1111
1112
1113
1114
1115
1116

    std::string controllerName;
    if(!nodeHandle.getParam(paramter_name, controllerName))
    {
        ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
        return;
    }

    serviceClient = ros::service::createClient<Controller>(controllerName, true);
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() <<  ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
}


1117
1118
1119
void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient )
{
    ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
1120
    ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "FlyingAgentClientConfig");
1121
1122
1123
1124
1125
1126
1127
1128
1129
1130
1131
1132
1133

    std::string controllerName;
    if(!nodeHandle.getParam(paramter_name, controllerName))
    {
        ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
        return;
    }

    serviceClient = ros::service::createClient<IntIntService>(controllerName, true);
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() <<  ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
}


1134
1135
1136
1137
1138
1139
1140
1141
1142
1143
1144
1145

void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&)
{
    // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
    createControllerServiceClientFromParameterName( "defaultController"  , m_defaultController );
    createControllerServiceClientFromParameterName( "studentController"  , m_studentController );
    createControllerServiceClientFromParameterName( "tuningController"   , m_tuningController );
    createControllerServiceClientFromParameterName( "pickerController"   , m_pickerController );
    createControllerServiceClientFromParameterName( "templateController" , m_templateController );

    // INITIALISE THE SERVICE FOR REQUESTING THE DEFAULT
    // CONTROLLER TO PERFORM MANOEUVRES
1146
    createIntIntServiceClientFromParameterName( "defaultController_requestManoeuvre" , m_defaultController_requestManoeuvre );
1147
1148
1149

    // Check that at least the default controller is available
    // > Setting the flag accordingly
1150
    if (m_defaultController)
1151
1152
1153
1154
1155
1156
1157
1158
1159
1160
1161
1162
1163
    {
        m_controllers_avialble = true;
    }
    else
    {
        m_controllers_avialble = false;
        // Inform the user of the problem
        ROS_ERROR("[FLYING AGENT CLIENT] The default controller service client (and pressumably all other controllers) could NOT be created.");
    }
}



1164
1165


1166
1167
1168
1169
1170
1171
1172
1173
1174
1175
1176
1177
1178
1179
1180
//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------


1181
void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg)
1182
1183
{
    // Check whether the message is relevant
1184
    bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
1185
1186
1187
1188
1189
1190
1191
1192
1193
1194
1195
1196
1197
1198

    // Continue if the message is relevant
    if (isRevelant)
    {
        // Extract the data
        int parameter_service_to_load_from = msg.data;
        // Initialise a local variable for the namespace
        std::string namespace_to_use;
        // Load from the respective parameter service
        switch(parameter_service_to_load_from)
        {
            // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
            case LOAD_YAML_FROM_AGENT:
            {
1199
                ROS_INFO("[FLYING AGENT CLIENT] Now fetching the FlyingAgentClientConfig YAML parameter values from this agent.");
1200
1201
1202
1203
1204
1205
                namespace_to_use = m_namespace_to_own_agent_parameter_service;
                break;
            }
            // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
            case LOAD_YAML_FROM_COORDINATOR:
            {
1206
                ROS_INFO("[FLYING AGENT CLIENT] Now fetching the FlyingAgentClientConfig YAML parameter values from this agent's coordinator.");
1207
1208
1209
1210
1211
1212
                namespace_to_use = m_namespace_to_coordinator_parameter_service;
                break;
            }

            default:
            {
1213
                ROS_ERROR("[FLYING AGENT CLIENT] Paramter service to load from was NOT recognised.");
1214
1215
1216
1217
1218
1219
1220
                namespace_to_use = m_namespace_to_own_agent_parameter_service;
                break;
            }
        }
        // Create a node handle to the selected parameter service
        ros::NodeHandle nodeHandle_to_use(namespace_to_use);
        // Call the function that fetches the parameters
1221
        fetchFlyingAgentClientConfigYamlParameters(nodeHandle_to_use);
1222
1223
1224
1225
1226
1227
    }
}



// > Load the paramters from the Client Config YAML file
1228
void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
1229
1230
{
    // Here we load the parameters that are specified in the file:
1231
    // FlyingAgentClientConfig.yaml
1232

1233
1234
    // Add the "FlyingAgentClientConfig" namespace to the "nodeHandle"
    ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "FlyingAgentClientConfig");
1235
1236

    // Flag for whether to use angle for switching to the Safe Controller
1237
1238
    yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
    yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
1239
1240
1241

    // Number of consecutive occulsions that will deem the
    // object as "long-term" occuled
1242
1243
1244
1245
	yaml_consecutive_occulsions_threshold = getParameterInt(nodeHandle_for_paramaters,"consecutive_occulsions_threshold");

	// Time out duration after which Mocap is considered unavailable
	yaml_mocap_timeout_duration = getParameterFloat(nodeHandle_for_paramaters,"mocap_timeout_duration");
1246
1247
1248
1249
1250
1251
1252
1253

	// Flag for whether the take-off should be performed
	//  with the default controller
	yaml_shouldPerfom_takeOff_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_takeOff_with_defaultController");

	// Flag for whether the landing should be performed
	// with the default controller
	yaml_shouldPerfom_landing_with_defaultController = getParameterBool(nodeHandle_for_paramaters,"shouldPerfom_landing_with_defaultController");
1254
1255
    
    // DEBUGGING: Print out one of the parameters that was loaded
1256
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched FlyingAgentClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
1257
1258
1259



1260
1261
    // PROCESS THE PARAMTERS
    // Convert from degrees to radians
1262
    yaml_maxTiltAngle_for_strictSafety_radians = DEG2RAD * yaml_maxTiltAngle_for_strictSafety_degrees;
1263

1264
    // DEBUGGING: Print out one of the processed values
1265
    ROS_INFO_STREAM("[FLYING AGENT CLIENT CONTROLLER] DEBUGGING: after processing yaml_maxTiltAngle_for_strictSafety_radians = " << yaml_maxTiltAngle_for_strictSafety_radians);
1266
1267
1268
1269
1270
}




1271

1272
1273
1274
1275
1276
1277
1278
1279
1280
1281
1282

//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------

int main(int argc, char* argv[])
{
1283
	// Starting the ROS-node
1284
	ros::init(argc, argv, "FlyingAgentClient");
1285

1286
1287
1288
	// Create a "ros::NodeHandle" type local variable "nodeHandle"
	// as the current node, the "~" indcates that "self" is the
	// node handle assigned to this variable.
1289
1290
	ros::NodeHandle nodeHandle("~");

1291
1292
1293
	// Get the namespace of this node
	std::string m_namespace = ros::this_node::getNamespace();
	ROS_INFO_STREAM("[FLYING AGENT CLIENT] ros::this_node::getNamespace() =  " << m_namespace);
1294
1295
1296



1297
	// AGENT ID AND COORDINATOR ID
1298

1299
1300
1301
1302
1303
1304
1305
1306
1307
	// NOTES:
	// > If you look at the "Agent.launch" file in the "launch" folder,
	//   you will see the following line of code:
	//   <param name="agentID" value="$(optenv ROS_NAMESPACE)" />
	//   This line of code adds a parameter named "agentID" to the
	//   "FlyingAgentClient" node.
	// > Thus, to get access to this "agentID" paremeter, we first
	//   need to get a handle to the "FlyingAgentClient" node within which this
	//   controller service is nested.
1308
1309


1310
1311
	// Get the ID of the agent and its coordinator
	bool isValid_IDs = getAgentIDandCoordIDfromClientNode( m_namespace + "/FlyingAgentClient" , &m_agentID , &m_coordID);
1312

1313
1314
1315
1316
1317
1318
1319
1320
1321
1322
	// Stall the node IDs are not valid
	if ( !isValid_IDs )
	{
		ROS_ERROR("[FLYING AGENT CLIENT] Node NOT FUNCTIONING :-)");
		ros::spin();
	}
	else
	{
		ROS_INFO_STREAM("[FLYING AGENT CLIENT] loaded agentID = " << m_agentID << ", and coordID = " << m_coordID);
	}
1323
1324
1325



1326
	// PARAMETER SERVICE NAMESPACE AND NODEHANDLES:
1327

1328
1329
1330
1331
1332
1333
1334
1335
1336
1337
1338
1339
	// NOTES:
	// > The parameters that are specified thorugh the *.yaml files
	//   are managed by a separate node called the "Parameter Service"
	// > A separate node is used for reasons of speed and generality
	// > To allow for a distirbuted architecture, there are two
	//   "ParamterService" nodes that are relevant:
	//   1) the one that is nested under the "m_agentID" namespace
	//   2) the one that is nested under the "m_coordID" namespace
	// > The following lines of code create the namespace (as strings)
	//   to there two relevant "ParameterService" nodes.
	// > The node handles are also created because they are needed
	//   for the ROS Subscriptions that follow.
1340

1341
1342
1343
1344
1345
1346
1347
	// Set the class variable "m_namespace_to_own_agent_parameter_service",
	// i.e., the namespace of parameter service for this agent
	m_namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";

	// Set the class variable "m_namespace_to_coordinator_parameter_service",
	// i.e., the namespace of parameter service for this agent's coordinator
	constructNamespaceForCoordinatorParameterService( m_coordID, m_namespace_to_coordinator_parameter_service );
1348