FlyingAgentClient.cpp 43.5 KB
Newer Older
1
//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
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
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,
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
//    
15
//    You should have received a copy of the GNU General Public License
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
//    along with D-FaLL-System.  If not, see <http://www.gnu.org/licenses/>.
//    
//
//    ----------------------------------------------------------------------------------
//    DDDD        FFFFF        L     L           SSSS  Y   Y   SSSS  TTTTT  EEEEE  M   M
//    D   D       F      aaa   L     L          S       Y Y   S        T    E      MM MM
//    D   D  ---  FFFF  a   a  L     L     ---   SSS     Y     SSS     T    EEE    M M M
//    D   D       F     a  aa  L     L              S    Y        S    T    E      M   M
//    DDDD        F      aa a  LLLL  LLLL       SSSS     Y    SSSS     T    EEEEE  M   M
//
//
//    DESCRIPTION:
//    ROS node that manages the student's setup.
//
//    ----------------------------------------------------------------------------------

32

33
34
35



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

39

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



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

57
58


roangel's avatar
roangel committed
59

60
61
62
63




64

65

phfriedl's avatar
phfriedl committed
66

bucyril's avatar
bucyril committed
67

68

69
70


71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
// void takeOffCF(CrazyflieData& current_local_coordinates) //local because the setpoint is in local coordinates
// {
//     // set the setpoint and call safe controller
//     Setpoint setpoint_msg;
//     setpoint_msg.x = current_local_coordinates.x;           // previous one
//     setpoint_msg.y = current_local_coordinates.y;           //previous one
//     setpoint_msg.z = current_local_coordinates.z + yaml_take_off_distance;           //previous one plus some offset
//     // setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
//     setpoint_msg.yaw = 0.0;
//     safeControllerServiceSetpointPublisher.publish(setpoint_msg);
//     ROS_INFO("[FLYING AGENT CLIENT] Take OFF:");
//     ROS_INFO("[FLYING AGENT CLIENT] ------Current coordinates:");
//     ROS_INFO("[FLYING AGENT CLIENT] X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
//     ROS_INFO("[FLYING AGENT CLIENT] ------New coordinates:");
//     ROS_INFO("[FLYING AGENT CLIENT] X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
86

87
88
89
90
//     // now, use safe controller to go to that setpoint
//     loadSafeController();
//     setInstantController(SAFE_CONTROLLER);
//     // when do we finish? after some time with delay?
91

92
93
94
//     // update variable that keeps track of current setpoint
//     setCurrentSafeSetpoint(setpoint_msg);
// }
95

96
97
98
99
100
101
102
103
104
// void landCF(CrazyflieData& current_local_coordinates)
// {
//     // set the setpoint and call safe controller
//     Setpoint setpoint_msg;
//     setpoint_msg.x = current_local_coordinates.x;           // previous one
//     setpoint_msg.y = current_local_coordinates.y;           //previous one
//     setpoint_msg.z = yaml_landing_distance;           //previous one plus some offset
//     setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
//     safeControllerServiceSetpointPublisher.publish(setpoint_msg);
105

106
107
108
109
110
//     // now, use safe controller to go to that setpoint
//     loadSafeController();
//     setInstantController(SAFE_CONTROLLER);
//     setCurrentSafeSetpoint(setpoint_msg);
// }
111

roangel's avatar
roangel committed
112
113
114



115
116
117
118
119
// void goToControllerSetpoint()
// {
//     safeControllerServiceSetpointPublisher.publish(controller_setpoint);
//     setCurrentSafeSetpoint(controller_setpoint);
// }
120

121

122
//is called when new data from Vicon arrives
123
124
void viconCallback(const ViconData& viconData)
{
125
126
127
128
    // NOTE: THIS FUNCTION IS CALL AT THE FREQUENCY OF THE MOTION
    //       CAPTURE SYSTEM. HENCE ANY OPTERATIONS PERFORMED IN
    //       THIS FUNCTION MUST BE NON-BLOCKING.

129
130
131
132
    // Initialise a counter of consecutive frames of motion
    // capture data where the agent is occuled
    static int number_of_consecutive_occulsions = 0;

133
134
135
136
137
138
139
140
141
    // Initilise a variable for the pose data of this agent
    CrazyflieData poseDataForThisAgent;

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


142
143
144
145
146
147
148
149
150
    // 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();
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165


    // Only continue if:
    // (1) the pose for this agent was found, and
    // (2) the flying state is something other than MOTORS-OFF
    if (
        (m_poseDataIndex >= 0)
        and
        (m_flying_state != STATE_MOTORS_OFF)
        and
        (m_controllers_avialble)
    )
    {
        // Initliase the "Contrller" service call variable
        Controller controllerCall;
166

167
168
        // Fill in the pose data for this agent
        controllerCall.request.ownCrazyflie = poseDataForThisAgent;
169

170
171
        // Initialise a node handle, needed for starting timers
        ros::NodeHandle nodeHandle("~");
roangel's avatar
roangel committed
172

173
174
175
176
177
        // If the object is NOT occluded,
        // then proceed to make the "Controller Service Call" that
        // compute the controller output.
        if(!poseDataForThisAgent.occluded)
        {
178
179
180
        	// Reset the "consecutive occulsions" flag
        	number_of_consecutive_occulsions = 0;

181
182
            // PERFORM THE SAFTY CHECK (IF NOT THE DEFAULT CONTROLLER)
            if ( getInstantController() != DEFAULT_CONTROLLER )
183
            {
184
185
186
187
188
                if ( !safetyCheck(poseDataForThisAgent) )
                {
                    setInstantController(DEFAULT_CONTROLLER);
                    ROS_INFO_STREAM("[FLYING AGENT CLIENT] safety check failed, switching to DEFAULT CONTROLLER");
                }
189
            }
190

191
192
193
194
195
196
197
198
199
            // Initialise a local boolean success variable
            bool isSuccessful_controllerCall = false;



            isSuccessful_controllerCall = m_instant_controller_service_client->call(controllerCall);

            // Ensure success and enforce safety
            if(!isSuccessful_controllerCall)
200
            {
201
202
203
204
205
206
                // 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 )
207
                {
208
209
210
211
212
213
                    // 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 )
214
                    {
215
                        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());
216
217
218
                    }
                }
            }
219
220
221
222
223

            // Send the command to the CrazyRadio
            // > IF SUCCESSFUL
            if (isSuccessful_controllerCall)
            {
224
                m_commaNdfOrsendiNgTocrazyflIepublisher.publish(controllerCall.response.controlOutput);
225
226
            }
            // > ELSE SEND ZERO OUTPUT COMMAND
227
228
            else
            {
229
                // Send the command to turn the motors off
230
                sendZeroOutputCommandForMotors();
231
232
                // And change the state to motor-off
                changeFlyingStateTo(STATE_MOTORS_OFF);
233
            }
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
		}
		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;
				// Send the command to turn the motors off
		        sendZeroOutputCommandForMotors();
		        // Update the flying state
		        changeFlyingStateTo( STATE_MOTORS_OFF );
			}
253
        } // END OF: "if(!global.occluded)"
254

255
256
257
258
    }
    else
    {
        // Send the command to turn the motors off
259
        sendZeroOutputCommandForMotors();
260

261
	} // END OF: "if ( (m_poseDataIndex >= 0) and (m_flying_state != STATE_MOTORS_OFF) )"
262

263
}
264

265
266


267
268


269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
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;
292
        }
293
    }
294

295
296
297
298
299
300
301
302
303
304
305
306
307
    // 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;
            }
        }
    }
308

309
310
311
312
313
    // If not found, then retrun, else fill in the pose data
    if (object_index < 0)
    {
        return object_index;
    }
314
315
    else
    {
316
317
318
319
        pose_to_fill_in = viconData.crazyflies[object_index];
        coordinatesToLocal(pose_to_fill_in);
        return object_index;
    }
320
}
321

322

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

332
333
334
335
    cf.x -= originX;
    cf.y -= originY;
    cf.z -= originZ;
}
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
void timerCallback_mocap_timeout_check(const ros::TimerEvent&)
{
	// Update the flag
	m_isAvailable_mocap_data = true;
	// 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
        changeFlyingStateTo( STATE_MOTORS_OFF );
	}
}


void sendZeroOutputCommandForMotors()
{
	ControlCommand zeroOutput = ControlCommand(); //everything set to zero
    zeroOutput.onboardControllerType = CF_COMMAND_TYPE_MOTORS; //set to motor_mode
    m_commaNdfOrsendiNgTocrazyflIepublisher.publish(zeroOutput);
}


363
364
365
366
367
368
369







370
371
372
373
374
void changeFlyingStateTo(int new_state)
{
    if(crazyradio_status == CRAZY_RADIO_STATE_CONNECTED)
    {
        ROS_INFO("[FLYING AGENT CLIENT] Change state to: %d", new_state);
375
        m_flying_state== = new_state;
376
377
378
379
380
    }
    else
    {
        ROS_INFO("[FLYING AGENT CLIENT] Disconnected and trying to change state. State goes to MOTORS OFF");
        m_flying_state = STATE_MOTORS_OFF;
381
    }
382

383
384
    // Set the class variable flag that the
    // flying state was changed
385
    //m_changed_flying_state_flag = true;
386

387
388
389
390
    // Publish a message with the new flying state
    std_msgs::Int32 flying_state_msg;
    flying_state_msg.data = m_flying_state;
    flyingStatePublisher.publish(flying_state_msg);
391
392
}

393

394
void takeOffTimerCallback(const ros::TimerEvent&)
395
{
396
    //finished_take_off = true;
397
398
}

399
void landTimerCallback(const ros::TimerEvent&)
400
{
401
    //finished_land = true;
402
403
}

404

405
406


407

408
409
410
411
412
413






Paul Beuchat's avatar
Paul Beuchat committed
414
415
416
417
418





419

420

Paul Beuchat's avatar
Paul Beuchat committed
421
422
423
424
425
426
427







428
//    ----------------------------------------------------------------------------------
429
430
431
432
433
434
435
//    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
//
//
436
437
438
439
440
441
//     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
//    ----------------------------------------------------------------------------------
Paul Beuchat's avatar
Paul Beuchat committed
442
443


444
445
446
// CREATE A "CONTROLLER" TYPE SERVICE CLIENT
// NOTE: that in the "ros::service::createClient" function the
//       second argument is a boolean that specifies whether the 
447
448
449
450
451
452
453
454
455
//       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 loadController( std::string paramter_name , ros::ServiceClient& serviceClient )
{
456
457
    ros::NodeHandle nodeHandle_to_own_agent_parameter_service(m_namespace_to_own_agent_parameter_service);
    ros::NodeHandle nodeHandle(nodeHandle_to_own_agent_parameter_service, "ClientConfig");
Paul Beuchat's avatar
Paul Beuchat committed
458

459
460
461
462
    std::string controllerName;
    if(!nodeHandle.getParam(paramter_name, controllerName))
    {
        ROS_ERROR_STREAM("[FLYING AGENT CLIENT] Failed to get \"" << paramter_name << "\" paramter");
Paul Beuchat's avatar
Paul Beuchat committed
463
464
465
        return;
    }

466
467
    serviceClient = ros::service::createClient<Controller>(controllerName, true);
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] Loaded service: " << serviceClient.getService() <<  ", valid: " << serviceClient.isValid() << ", exists: " << serviceClient.exists() );
Paul Beuchat's avatar
Paul Beuchat committed
468
469
470
471
}



472
473
474
475
476
477
478
479
480
481
482
483
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
509
510
511
512
513
514
void timerCallback_for_creaTeaLlcontrollerServiceClients(const ros::TimerEvent&)
{
    // INITIALISE ALL THE CONTROLLER SERVICE CLIENTS
    loadController( "defaultController"  , defaultController );
    loadController( "studentController"  , studentController );
    loadController( "tuningController"   , tuningController );
    loadController( "pickerController"   , pickerController );
    loadController( "templateController" , templateController );

    // Check that at least the default controller is available
    // > Setting the flag accordingly
    if (defaultController)
    {
        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.");
    }
}







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

515
516
517
518
519
520
521
522
523


void sendMessageUsingController(int controller)
{
    // Send a message on the topic for informing the Flying
    // Agent GUI about this update
    std_msgs::Int32 controller_used_msg;
    controller_used_msg.data = controller;
    controllerUsedPublisher.publish(controller_used_msg);
Paul Beuchat's avatar
Paul Beuchat committed
524
525
}

526
void setInstantController(int controller) //for right now, temporal use
527
{
528
529
530
531
    // Update the class variable
    m_instant_controller = controller;

    
532

533
    switch(controller)
534
    {
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
        case DEFAULT_CONTROLLER:
            m_instant_controller_service_client = &defaultController;
            break;
        case STUDENT_CONTROLLER:
            m_instant_controller_service_client = &studentController;
            break;
        case TUNING_CONTROLLER:
            m_instant_controller_service_client = &tuningController;
            break;
        case PICKER_CONTROLLER:
            m_instant_controller_service_client = &pickerController;
            break;
        case TEMPLATE_CONTROLLER:
            m_instant_controller_service_client = &templateController;
            break;
        default:
            break;
552
553
    }

554
    sendMessageUsingController(controller);
555
556
}

557
int getInstantController()
558
{
559
560
561
562
563
564
565
    return m_instant_controller;
}

void setControllerNominallySelected(int controller)
{
    // Update the class variable
    m_controller_nominally_selected = controller;
566

567
568
569
    // If in state "MOTORS-OFF" or "FLYING",
    // then the change is instant.
    if(m_flying_state == STATE_MOTORS_OFF || m_flying_state == STATE_FLYING)
570
571
    {

572
573
        setInstantController(controller); 
    }
574
}
Paul Beuchat's avatar
Paul Beuchat committed
575

576
int getControllerNominallySelected()
577
{
578
579
580
581
582
    return m_controller_nominally_selected;
}



583

584
585
586
587
588
589
590
591
592
593
594
595
596






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)
597
    {
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
        // 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");
                if(m_flying_state == STATE_MOTORS_OFF)
                {
                    changeFlyingStateTo(STATE_TAKE_OFF);
                }
                break;

            case CMD_CRAZYFLY_LAND:
                ROS_INFO("[FLYING AGENT CLIENT] LAND Command received");
                if(m_flying_state != STATE_MOTORS_OFF)
                {
                    changeFlyingStateTo(STATE_LAND);
                }
                break;
            case CMD_CRAZYFLY_MOTORS_OFF:
                ROS_INFO("[FLYING AGENT CLIENT] MOTORS_OFF Command received");
                changeFlyingStateTo(STATE_MOTORS_OFF);
                break;

            default:
                ROS_ERROR_STREAM("[FLYING AGENT CLIENT] unexpected command number: " << cmd);
                break;
        }
666
    }
667
668
669
}


670

671
672
673
674
675
676
677
678
679





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

682
683
684



685
686


687
688
689
690
void emergencyStopReceivedCallback(const IntWithHeader & msg)
{
    ROS_INFO("[FLYING AGENT CLIENT] Received message to EMERGENCY STOP");
    flyingStateRequestCallback(msg);
691
692
}

693
694
695



Paul Beuchat's avatar
Paul Beuchat committed
696
697


698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
//    ----------------------------------------------------------------------------------
//     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)
Paul Beuchat's avatar
Paul Beuchat committed
713
{
714
715
716
717
    // Put the flying state into the response variable
    response.data = m_flying_state;
    // Return
    return true;
Paul Beuchat's avatar
Paul Beuchat committed
718
719
720
721
}



722
723
724



Paul Beuchat's avatar
Paul Beuchat committed
725
726
727
728
729
730






731
//    ----------------------------------------------------------------------------------
732
733
734
735
736
//    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
737
738
//    ----------------------------------------------------------------------------------

739
740
741
742
void batteryMonitorStateChangedCallback(std_msgs::Int32 msg)
{
    // Extract the data
    int new_battery_state = msg.data;
743

744
    // Take action if changed to low battery state
745
    if (new_battery_state == BATTERY_STATE_LOW)
746
    {
747
        if (m_flying_state != STATE_MOTORS_OFF)
748
        {
749
            ROS_INFO("[FLYING AGENT CLIENT] low level battery triggered, now landing.");
750
751
752
753
            changeFlyingStateTo(STATE_LAND);
        }
        else
        {
754
            ROS_INFO("[FLYING AGENT CLIENT] low level battery triggered, please turn off the Crazyflie.");
755
        }
756
    }
757
    else if (new_battery_state == BATTERY_STATE_NORMAL)
758
    {
759
        ROS_INFO("[FLYING AGENT CLIENT] received message that battery state changed to normal");
760
    }
761
762
    else
    {
763
        ROS_INFO("[FLYING AGENT CLIENT] received message that battery state changed to something unknown");
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


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

// Checks if crazyflie is within allowed area
bool safetyCheck(CrazyflieData data)
790
{
791
792
    // Check on the X position
    if((data.x < m_context.localArea.xmin) or (data.x > m_context.localArea.xmax))
793
    {
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
        ROS_INFO_STREAM("[FLYING AGENT CLIENT] x safety failed");
        return false;
    }
    // Check on the Y position
    if((data.y < m_context.localArea.ymin) or (data.y > m_context.localArea.ymax))
    {
        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;
    }

    // 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(
            (data.roll > m_maxTiltAngle_for_strictSafety_redians)
            or
            (data.roll < -m_maxTiltAngle_for_strictSafety_redians)
        )
        {
            ROS_INFO_STREAM("[FLYING AGENT CLIENT] roll too big.");
            return false;
        }
        // Check on the PITCH angle
        if(
            (data.pitch > m_maxTiltAngle_for_strictSafety_redians)
            or
            (data.pitch < -m_maxTiltAngle_for_strictSafety_redians)
        )
        {
            ROS_INFO_STREAM("[FLYING AGENT CLIENT] pitch too big.");
            return false;
        }
838
839
    }

840
841
842
    // If the code makes it to here then all the safety checks passed,
    // Hence return "true"
    return true;
843
844
845
846
847
}




848
849
850
851
852
853
854
855
856
857
858
859
860
861

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


864
865
866
867
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg)
{
    ROS_INFO("[FLYING AGENT CLIENT] Received message that the Context Database Changed");
    loadCrazyflieContext();
868
869
870
}


871
872

void loadCrazyflieContext()
873
{
874
875
876
    CMQuery contextCall;
    contextCall.request.studentID = m_agentID;
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] AgentID:" << m_agentID);
877

878
    CrazyflieContext new_context;
879

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

882
883
884
    if(centralManager.call(contextCall)) {
        new_context = contextCall.response.crazyflieContext;
        ROS_INFO_STREAM("[FLYING AGENT CLIENT] CrazyflieContext:\n" << new_context);
885

886
887
        if((m_context.crazyflieName != "") && (new_context.crazyflieName != m_context.crazyflieName)) //linked crazyflie name changed and it was not empty before
        {
888

889
            // Motors off is done in python script now everytime we disconnect
890

891
892
893
894
            // send motors OFF and disconnect before setting m_context = new_context
            // std_msgs::Int32 msg;
            // msg.data = CMD_CRAZYFLY_MOTORS_OFF;
            // commandPublisher.publish(msg);
895

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

898
899
900
901
902
            IntWithHeader msg;
            msg.shouldCheckForAgentID = false;
            msg.data = CMD_DISCONNECT;
            crazyRadioCommandPublisher.publish(msg);
        }
903

904
        m_context = new_context;
905

906
907
908
909
910
911
912
        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");
    }
913
914
915
916
917
918
919
920
}







921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
//    ----------------------------------------------------------------------------------
//    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
//    ----------------------------------------------------------------------------------


void isReadyClientConfigYamlCallback(const IntWithHeader & msg)
{
    // Check whether the message is relevant
939
    bool isRevelant = checkMessageHeader( m_agentID , msg.shouldCheckForAgentID , msg.agentIDs );
940
941
942
943
944
945
946
947
948
949
950
951
952
953

    // 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:
            {
954
                ROS_INFO("[FLYING AGENT CLIENT] Now fetching the ClientConfig YAML parameter values from this agent.");
955
956
957
958
959
960
                namespace_to_use = m_namespace_to_own_agent_parameter_service;
                break;
            }
            // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
            case LOAD_YAML_FROM_COORDINATOR:
            {
961
                ROS_INFO("[FLYING AGENT CLIENT] Now fetching the ClientConfig YAML parameter values from this agent's coordinator.");
962
963
964
965
966
967
                namespace_to_use = m_namespace_to_coordinator_parameter_service;
                break;
            }

            default:
            {
968
                ROS_ERROR("[FLYING AGENT CLIENT] Paramter service to load from was NOT recognised.");
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
                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
        fetchClientConfigYamlParameters(nodeHandle_to_use);
    }
}



// > Load the paramters from the Client Config YAML file
void fetchClientConfigYamlParameters(ros::NodeHandle& nodeHandle)
{
    // Here we load the parameters that are specified in the file:
    // ClientConfig.yaml

    // Add the "ClientConfig" namespace to the "nodeHandle"
    ros::NodeHandle nodeHandle_for_paramaters(nodeHandle, "ClientConfig");

    // Flag for whether to use angle for switching to the Safe Controller
992
993
    yaml_isEnabled_strictSafety = getParameterBool(nodeHandle_for_paramaters,"isEnabled_strictSafety");
    yaml_maxTiltAngle_for_strictSafety_degrees = getParameterFloat(nodeHandle_for_paramaters,"maxTiltAngle_for_strictSafety_degrees");
994
995
    
    // DEBUGGING: Print out one of the parameters that was loaded
996
    ROS_INFO_STREAM("[FLYING AGENT CLIENT] DEBUGGING: the fetched ClientConfig/isEnabled_strictSafety = " << yaml_isEnabled_strictSafety);
997
998
999



1000
    // PROCESS THE PARAMTERS
For faster browsing, not all history is shown. View entire blame