SafeControllerService.cpp 23 KB
Newer Older
1
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Cyrill Burgener, Marco Mueller, Philipp Friedli
bucyril's avatar
bucyril 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
bucyril's avatar
bucyril 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,
bucyril's avatar
bucyril 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
//    
bucyril's avatar
bucyril 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
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:
//    The fall-back controller that keeps the Crazyflie safe
//
//    ----------------------------------------------------------------------------------

bucyril's avatar
bucyril committed
32

33
34


35

36
37
// INCLUDE THE HEADER
#include "nodes/SafeControllerService.h"
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
59




60
61
62
63
64
65
66
//    ------------------------------------------------------------------------------
//     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
//    ----------------------------------------------------------------------------------
67

68
//simple derivative
69
70
/*
void estimateState(Controller::Request &request, float (&est)[9]) {
71
72
73
    est[0] = request.ownCrazyflie.x;
    est[1] = request.ownCrazyflie.y;
    est[2] = request.ownCrazyflie.z;
74

75
76
77
    est[3] = (request.ownCrazyflie.x - previousLocation.x) / request.ownCrazyflie.acquiringTime;
    est[4] = (request.ownCrazyflie.y - previousLocation.y) / request.ownCrazyflie.acquiringTime;
    est[5] = (request.ownCrazyflie.z - previousLocation.z) / request.ownCrazyflie.acquiringTime;
78

79
80
81
    est[6] = request.ownCrazyflie.roll;
    est[7] = request.ownCrazyflie.pitch;
    est[8] = request.ownCrazyflie.yaw;
82
83
}
*/
phfriedl's avatar
phfriedl committed
84

85

roangel's avatar
roangel committed
86
87
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
88
    
roangel's avatar
roangel committed
89
    float yaw_measured = request.ownCrazyflie.yaw;
90

91
    //move coordinate system to make setpoint origin
92
93
94
95
    request.ownCrazyflie.x -= setpoint[0];
    request.ownCrazyflie.y -= setpoint[1];
    request.ownCrazyflie.z -= setpoint[2];
    float yaw = request.ownCrazyflie.yaw - setpoint[3];
roangel's avatar
roangel committed
96

97
    
roangel's avatar
roangel committed
98
    //bag.write("Offset", ros::Time::now(), request.ownCrazyflie);
99

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

104
105
    float est[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
    estimateState(request, est);
phfriedl's avatar
phfriedl committed
106
    float state[9]; //px, py, pz, vx, vy, vz, roll, pitch, yaw
107
    convertIntoBodyFrame(est, state, yaw_measured);
108

109
110
111
112
    //calculate feedback
    float outRoll = 0;
    float outPitch = 0;
    float outYaw = 0;
phfriedl's avatar
phfriedl committed
113
114
    float thrustIntermediate = 0;
    for(int i = 0; i < 9; ++i) {
115
116
117
118
        outRoll -= gainMatrixRoll[i] * state[i];
        outPitch -= gainMatrixPitch[i] * state[i];
        outYaw -= gainMatrixYaw[i] * state[i];
        thrustIntermediate -= gainMatrixThrust[i] * state[i];
phfriedl's avatar
phfriedl committed
119
    }
roangel's avatar
roangel committed
120
    // ROS_INFO_STREAM("thrustIntermediate = " << thrustIntermediate);
121
    //INFORMATION: this ugly fix was needed for the older firmware
122
    //outYaw *= 0.5;
123

124
125
126
127
128
    //ROS_INFO_STREAM("y-error      = " << state[1]);
    //ROS_INFO_STREAM("y-velocity   = " << state[4]);
    //ROS_INFO_STREAM("roll         = " << state[6]);
    //ROS_INFO_STREAM("rollRate     = " << outRoll );

129
130
131
    response.controlOutput.roll = outRoll;
    response.controlOutput.pitch = outPitch;
    response.controlOutput.yaw = outYaw;
phfriedl's avatar
phfriedl committed
132

133
134
135
136
137
    if(thrustIntermediate > saturationThrust)
        thrustIntermediate = saturationThrust;
    else if(thrustIntermediate < -saturationThrust)
        thrustIntermediate = -saturationThrust;

roangel's avatar
roangel committed
138
139
140
141
    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);
142

roangel's avatar
roangel committed
143
144
145
146
147
    // ROS_INFO_STREAM("ffThrust" << ffThrust[0]);
    // ROS_INFO_STREAM("controlOutput.cmd1 = " << response.controlOutput.motorCmd1);
    // ROS_INFO_STREAM("controlOutput.cmd3 = " << response.controlOutput.motorCmd2);
    // ROS_INFO_STREAM("controlOutput.cmd2 = " << response.controlOutput.motorCmd3);
    // ROS_INFO_STREAM("controlOutput.cmd4 = " << response.controlOutput.motorCmd4);
roangel's avatar
roangel committed
148

149
    response.controlOutput.onboardControllerType = RATE_MODE;
150

151
    previousLocation = request.ownCrazyflie;
152

153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
    return true;
}

void convertIntoBodyFrame(float est[9], float (&state)[9], float yaw_measured)
{
    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];
}
roangel's avatar
roangel committed
173

174
175
176
177
178


float computeMotorPolyBackward(float thrust)
{
    return (-motorPoly[1] + sqrt(motorPoly[1] * motorPoly[1] - 4 * motorPoly[2] * (motorPoly[0] - thrust))) / (2 * motorPoly[2]);
179
}
bucyril's avatar
bucyril committed
180

181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245

//Kalman
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];

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





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


// This function DOES NOT NEED TO BE edited for successful completion of the PPS exercise
void yamlReadyForFetchCallback(const std_msgs::Int32& msg)
{
    // Extract from the "msg" for which controller the and from where to fetch the YAML
    // parameters
    int controller_to_fetch_yaml = msg.data;

    // Switch between fetching for the different controllers and from different locations
    switch(controller_to_fetch_yaml)
    {

246
        // > FOR FETCHING FROM THE AGENT'S OWN PARAMETER SERVICE
247
        case FETCH_YAML_SAFE_CONTROLLER_FROM_OWN_AGENT:
248
        {
249
            // Let the user know that this message was received
250
            ROS_INFO("[SAFE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from this machine.");
251
            // Create a node handle to the parameter service running on this agent's machine
252
            ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
253
254
255
            // Call the function that fetches the parameters
            fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);
            break;
256
        }
257

258
        // > FOR FETCHING FROM THE COORDINATOR'S PARAMETER SERVICE
259
        case FETCH_YAML_SAFE_CONTROLLER_FROM_COORDINATOR:
260
261
        {
            // Let the user know that this message was received
262
            ROS_INFO("[SAFE CONTROLLER] Received the message that YAML parameters were (re-)loaded. > Now fetching the parameter values from the coordinator.");
263
            // Create a node handle to the parameter service running on the coordinator machine
264
            ros::NodeHandle nodeHandle_to_coordinator_parameter_service(namespace_to_coordinator_parameter_service);
265
266
267
268
269
            // Call the function that fetches the parameters
            fetchYamlParameters(nodeHandle_to_coordinator_parameter_service);
            break;
        }

270
        default:
271
        {
272
            // Let the user know that the command was not relevant
273
            //ROS_INFO("The SafeControllerService received the message that YAML parameters were (re-)loaded.\r> However the parameters do not relate to this controller, hence nothing will be fetched.");
274
            break;
275
        }
276
277
278
279
280
281
282
283
284
    }
}

void fetchYamlParameters(ros::NodeHandle& nodeHandle)
{

    // Here we load the parameters that are specified in the SafeController.yaml file

    // Add the "CustomController" namespace to the "nodeHandle"
285
    ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
286
287
288
289
290
291

    // > The mass of the crazyflie
    cf_mass = getParameterFloat(nodeHandle_for_safeController, "mass");

    // > The co-efficients of the quadratic conversation from 16-bit motor command to
    //   thrust force in Newtons
292
    getParameterFloatVector(nodeHandle_for_safeController, "motorPoly", motorPoly, 3);
293
294
295
    
    
    // > The row of the LQR matrix that commands body frame roll rate
296
    getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixRoll", gainMatrixRoll, 9);
297
    // > The row of the LQR matrix that commands body frame pitch rate
298
    getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixPitch", gainMatrixPitch, 9);
299
    // > The row of the LQR matrix that commands body frame yaw rate
300
    getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixYaw", gainMatrixYaw, 9);
301
    // > The row of the LQR matrix that commands thrust adjustment
302
    getParameterFloatVector(nodeHandle_for_safeController, "gainMatrixThrust", gainMatrixThrust, 9);
303
304

    // > The gains for the point-mass Kalman filter
305
306
    getParameterFloatVector(nodeHandle_for_safeController, "filterGain", filterGain, 6);
    getParameterFloatVector(nodeHandle_for_safeController, "estimatorMatrix", estimatorMatrix, 2);
307
308

    // > The defailt setpoint of the controller
309
    getParameterFloatVector(nodeHandle_for_safeController, "defaultSetpoint", defaultSetpoint, 4);
310

311
    // DEBUGGING: Print out one of the parameters that was loaded
312
    ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << cf_mass);
313

314
315
316
317
318
319
320
321
322
323
324
325
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
    // Call the function that computes details an values that are needed from these
    // parameters loaded above
    processFetchedParameters();
}

void processFetchedParameters()
{
    // force that we need to counteract gravity (mg)
    gravity_force = cf_mass * 9.81/(1000*4); // in N
    // The maximum possible thrust
    saturationThrust = motorPoly[2] * 12000 * 12000 + motorPoly[1] * 12000 + motorPoly[0];
}




//    ----------------------------------------------------------------------------------
//     GGGG  EEEEE  TTTTT  PPPP     A    RRRR     A    M   M   ( )
//    G      E        T    P   P   A A   R   R   A A   MM MM  (   )
//    G      EEE      T    PPPP   A   A  RRRR   A   A  M M M  (   )
//    G   G  E        T    P      AAAAA  R  R   AAAAA  M   M  (   )
//     GGGG  EEEEE    T    P      A   A  R   R  A   A  M   M   ( )
//    ----------------------------------------------------------------------------------


float getParameterFloat(ros::NodeHandle& nodeHandle, std::string name)
{
    float val;
    if(!nodeHandle.getParam(name, val))
    {
        ROS_ERROR_STREAM("missing parameter '" << name << "'");
    }
    return val;
}

void getParameterFloatVector(ros::NodeHandle& nodeHandle, std::string name, std::vector<float>& val, int length)
{
    if(!nodeHandle.getParam(name, val)){
        ROS_ERROR_STREAM("missing parameter '" << name << "'");
    }
    if(val.size() != length) {
        ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
    }
}

int getParameterInt(ros::NodeHandle& nodeHandle, std::string name)
{
    int val;
    if(!nodeHandle.getParam(name, val))
    {
        ROS_ERROR_STREAM("missing parameter '" << name << "'");
    }
    return val;
}
roangel's avatar
roangel committed
368

369
370
371
372
373
374
375
376
void getParameterIntVectorWithKnownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val, int length)
{
    if(!nodeHandle.getParam(name, val)){
        ROS_ERROR_STREAM("missing parameter '" << name << "'");
    }
    if(val.size() != length) {
        ROS_ERROR_STREAM("parameter '" << name << "' has wrong array length, " << length << " needed");
    }
377
}
bucyril's avatar
bucyril committed
378

379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
int getParameterIntVectorWithUnknownLength(ros::NodeHandle& nodeHandle, std::string name, std::vector<int>& val)
{
    if(!nodeHandle.getParam(name, val)){
        ROS_ERROR_STREAM("missing parameter '" << name << "'");
    }
    return val.size();
}

bool getParameterBool(ros::NodeHandle& nodeHandle, std::string name)
{
    bool val;
    if(!nodeHandle.getParam(name, val))
    {
        ROS_ERROR_STREAM("missing parameter '" << name << "'");
    }
    return val;
}





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

415
416
417
418
419
420
421
void setpointCallback(const Setpoint& newSetpoint) {
    setpoint[0] = newSetpoint.x;
    setpoint[1] = newSetpoint.y;
    setpoint[2] = newSetpoint.z;
    setpoint[3] = newSetpoint.yaw;
}

422

423

424
425
426
427
428
429
430
431
432


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

bucyril's avatar
bucyril committed
434
int main(int argc, char* argv[]) {
435
436
437
    ros::init(argc, argv, "SafeControllerService");

    ros::NodeHandle nodeHandle("~");
438
439
440
441
442


    // *********************************************************************************
    // EVERYTHING THAT RELATES TO FETCHING PARAMETERS FROM A YAML FILE

443
444
445

    // EVERYTHING FOR THE CONNECTION TO THIS AGENT's OWN PARAMETER SERVICE:

446
447
448
    // Get the namespace of this "SafeControllerService" node
    std::string m_namespace = ros::this_node::getNamespace();

449
450
451
    // Set the class variable "namespace_to_own_agent_parameter_service" to be a the
    // namespace string for the parameter service that is running on the machine of this
    // agent
452
    namespace_to_own_agent_parameter_service = m_namespace + "/ParameterService";
453
454
    
    // Create a node handle to the parameter service running on this agent's machine
455
    ros::NodeHandle nodeHandle_to_own_agent_parameter_service(namespace_to_own_agent_parameter_service);
456
457
458
459
460
461
462
463

    // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
    // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
    // and calls the class function "yamlReadyForFetchCallback" each time a message is
    // received on this topic and the message is passed as an input argument to the
    // "yamlReadyForFetchCallback" class function.
    ros::Subscriber controllerYamlReadyForFetchSubscriber_to_agent = nodeHandle_to_own_agent_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);

464
465
466
467
468

    // EVERYTHING FOR THE CONNECTION THE COORDINATOR'S PARAMETER SERVICE:

    // Set the class variable "nodeHandle_to_coordinator_parameter_service" to be a node handle
    // for the parameter service that is running on the coordinate machine
469
470
471
    // NOTE: the backslash here (i.e., "/") before the name of the node ("ParameterService")
    //       is very important because it specifies that the name is global
    namespace_to_coordinator_parameter_service = "/ParameterService";
472
473
474
475
476
477

    // Create a node handle to the parameter service running on the coordinator machine
    ros::NodeHandle nodeHandle_to_coordinator = ros::NodeHandle();
    //ros::NodeHandle nodeHandle_to_coordinator_parameter_service = ros::NodeHandle(namespace_to_own_agent_parameter_service);
    

478
479
480
481
482
    // Instantiate the local variable "controllerYamlReadyForFetchSubscriber" to be a
    // "ros::Subscriber" type variable that subscribes to the "controllerYamlReadyForFetch" topic
    // and calls the class function "yamlReadyForFetchCallback" each time a message is
    // received on this topic and the message is passed as an input argument to the
    // "yamlReadyForFetchCallback" class function.
483
484
485
486
487
    ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator.subscribe("/ParameterService/controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);
    //ros::Subscriber controllerYamlReadyForFetchSubscriber_to_coordinator = nodeHandle_to_coordinator_parameter_service.subscribe("controllerYamlReadyForFetch", 1, yamlReadyForFetchCallback);


    // PRINT OUT SOME INFORMATION
488

489
    // Let the user know what namespaces are being used for linking to the parameter service
490
491
492
    ROS_INFO_STREAM("[SAFE CONTROLLER] The namespace string for accessing the Paramter Services are:");
    ROS_INFO_STREAM("[SAFE CONTROLLER] namespace_to_own_agent_parameter_service    =  " << namespace_to_own_agent_parameter_service);
    ROS_INFO_STREAM("[SAFE CONTROLLER] namespace_to_coordinator_parameter_service  =  " << namespace_to_coordinator_parameter_service);
493

494
495

    // FINALLY, FETCH ANY PARAMETERS REQUIRED FROM THESE "PARAMETER SERVICES"
496

497
    // Call the class function that loads the parameters for this class.
498
499

    // Sleep for some time (in seconds)
500
501
502
503
504
505
    // ros::Duration(1.0).sleep();

    // // Ask the Paramter Service to load the respective YAML file
    // std::string namespace_to_own_agent_loadYamlFiles_service = namespace_to_own_agent_parameter_service + "/LoadYamlFiles";
    // loadYamlFilesService_own_agent = ros::service::createClient<LoadYamlFiles>(namespace_to_own_agent_loadYamlFiles_service, true);
    // ROS_INFO_STREAM("[SAFE CONTROLLER] Loaded service: " << loadYamlFilesService_own_agent.getService());
506

507
508
509
510
    // LoadYamlFiles loadYamlFilesService;
    // std::vector<std::string> yamlFileNames_to_load = {"SafeController"};
    // loadYamlFilesService.request.yamlFileNames = yamlFileNames_to_load;
    // bool success = loadYamlFilesService_own_agent.call(loadYamlFilesService);
511

512
    // ROS_INFO_STREAM("[SAFE CONTROLLER] called Laod Yaml File service with success = " << success);
513

514
    // ros::Duration(2.0).sleep();
515
516


517
    // Call the class function that loads the parameters for this class.
518
519
    fetchYamlParameters(nodeHandle_to_own_agent_parameter_service);

520
521
522
523
524
525
526
527
528
529

    // DEBUGGING
    // Add the "CustomController" namespace to the "nodeHandle"
    //ros::NodeHandle nodeHandle_for_safeController(nodeHandle, "SafeController");
    // > The mass of the crazyflie
    //float temp_mass = getParameterFloat(nodeHandle_for_safeController, "mass");
    //ROS_INFO_STREAM("[SAFE CONTROLLER] DEBUGGING: the fetched SafeController/mass = " << temp_mass);



530
531
532
    // *********************************************************************************

    
roangel's avatar
roangel committed
533
    setpoint = defaultSetpoint; // only first time setpoint is equal to default setpoint
534

bucyril's avatar
bucyril committed
535
    ros::Subscriber setpointSubscriber = nodeHandle.subscribe("Setpoint", 1, setpointCallback);
536

537
538
539
    ros::NodeHandle namespace_nodeHandle(ros::this_node::getNamespace());


540
    ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
541
    ROS_INFO("[SAFE CONTROLLER] Service ready :-)");
542
    
543
544
545
546
547
	// std::string package_path;
	// package_path = ros::package::getPath("d_fall_pps") + "/";
	// ROS_INFO_STREAM(package_path);
	// std::string record_file = package_path + "LoggingSafeController.bag";
	// bag.open(record_file, rosbag::bagmode::Write);
548
549


550
    ros::spin();
551
	// bag.close();
552
553
	
	
554
555

    return 0;
bucyril's avatar
bucyril committed
556
}