DefaultControllerService.h 18.1 KB
Newer Older
beuchatp's avatar
beuchatp committed
1
//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
//
//    This file is part of D-FaLL-System.
//    
//    D-FaLL-System is free software: you can redistribute it and/or modify
//    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.
//    
//    D-FaLL-System is distributed in the hope that it will be useful,
//    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.
//    
//    You should have received a copy of the GNU General Public License
//    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
//
//    ----------------------------------------------------------------------------------





//    ----------------------------------------------------------------------------------
//    III  N   N   CCCC  L      U   U  DDDD   EEEEE   SSSS
//     I   NN  N  C      L      U   U  D   D  E      S
//     I   N N N  C      L      U   U  D   D  EEE     SSS
//     I   N  NN  C      L      U   U  D   D  E          S
//    III  N   N   CCCC  LLLLL   UUU   DDDD   EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// These various headers need to be included so that this controller service can be
// connected with the D-FaLL system.

//some useful libraries
#include <math.h>
#include <stdlib.h>
#include "ros/ros.h"
#include <ros/package.h>

// Include the standard message types
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
#include <std_msgs/String.h>

// Include the DFALL message types
59
60
61
62
63
64
65
66
67
#include "dfall_pkg/IntWithHeader.h"
//#include "dfall_pkg/StringWithHeader.h"
#include "dfall_pkg/SetpointWithHeader.h"
#include "dfall_pkg/CustomButtonWithHeader.h"
#include "dfall_pkg/ViconData.h"
#include "dfall_pkg/Setpoint.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/Controller.h"
#include "dfall_pkg/DebugMsg.h"
68
69

// Include the DFALL service types
70
#include "dfall_pkg/IntIntService.h"
71
72
#include "dfall_pkg/LoadYamlFromFilename.h"
#include "dfall_pkg/GetSetpointService.h"
73
#include "dfall_pkg/CMQuery.h"
74
75
76

// Include the shared definitions
#include "nodes/Constants.h"
77
#include "nodes/DefaultControllerConstants.h"
78
79
80
81
82
83
84
85
86
87
88
89

// Include other classes
#include "classes/GetParamtersAndNamespaces.h"

// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>





// Namespacing the package
90
using namespace dfall_pkg;
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105





//    ----------------------------------------------------------------------------------
//    DDDD   EEEEE  FFFFF  III  N   N  EEEEE   SSSS
//    D   D  E      F       I   NN  N  E      S
//    D   D  EEE    FFF     I   N N N  EEE     SSS
//    D   D  E      F       I   N  NN  E          S
//    DDDD   EEEEE  F      III  N   N  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

// These constants are defined to make the code more readable and adaptable.

106
107
// NOTE: many constants are already defined in the "Constant.h" header file

108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
// These constants define the method used for computing
// the control actions from the state error estimates.
// The following is a short description about each mode:
//
// CONTROLLER_METHOD_RATES
//       Uses the poisition, linear velocity and angle
//       error estimates to compute the rates
//
// CONTROLLER_METHOD_RATE_ANGLE_NESTED
//       Uses the position and linear velocity error
//       estimates to compute an angle, and then uses
//       this as a reference to construct an angle error
//       estimate and compute from that the rates
//
#define CONTROLLER_METHOD_RATES               1
#define CONTROLLER_METHOD_RATE_ANGLE_NESTED   2   // (DEFAULT)


// These constants define the method used for estimating
// the Inertial frame state.
// All methods are run at all times, this flag indicates
// which estimate is passed onto the controller.
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
// The following is a short description about each mode:
//
// ESTIMATOR_METHOD_FINITE_DIFFERENCE
//       Takes the poisition and angles directly as measured,
//       and estimates the velocities as a finite different to the
//       previous measurement
//
// ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION
//       Uses a 2nd order random walk estimator independently for
//       each of (x,y,z,roll,pitch,yaw)
//
// ESTIMATOR_METHOD_QUADROTOR_MODEL_BASED
//       Uses the model of the quad-rotor and the previous inputs
//
#define ESTIMATOR_METHOD_FINITE_DIFFERENCE          1
#define ESTIMATOR_METHOD_POINT_MASS_PER_DIMENSION   2   // (DEFAULT)


148
149
150
151
// These constants deine the behaviour of the intergrator
#define INTEGRATOR_FLAG_ON       1
#define INTEGRATOR_FLAG_OFF      2
#define INTEGRATOR_FLAG_RESET    3
152
153
154
155
156
157
158
159
160
161
162
163




//    ----------------------------------------------------------------------------------
//    V   V    A    RRRR   III    A    BBBB   L      EEEEE   SSSS
//    V   V   A A   R   R   I    A A   B   B  L      E      S
//    V   V  A   A  RRRR    I   A   A  BBBB   L      EEE     SSS
//     V V   AAAAA  R  R    I   AAAAA  B   B  L      E          S
//      V    A   A  R   R  III  A   A  BBBB   LLLLL  EEEEE  SSSS
//    ----------------------------------------------------------------------------------

164
165
166

// VARIABLES FOR PERFORMING THE TAKE-OFF AND LANDING MANOEUVRES

167
168
169
// The current state of the Default Controller
int m_current_state = DEFAULT_CONTROLLER_STATE_STANDBY;

170
171
172
173
174
// A flag for when the state is changed, this is used
// so that a "one-off" operation can be performed
// the first time after changing that state
bool m_current_state_changed = false;

175
176
177
178
// The elapased time, incremented by counting the motion
// capture callbacks
float m_time_in_seconds = 0.0;

179
180
181
// PARAMETERS FROM THE YAML FILE

// Max setpoint change per second
182
float yaml_max_setpoint_change_per_second_horizontal = 0.60;
183
float yaml_max_setpoint_change_per_second_vertical   = 0.40;
184

185
186
187
// Max error for z
float yaml_max_setpoint_error_z = 0.4;

188
189
// Max error for xy
float yaml_max_setpoint_error_xy = 0.3;
190

191
192
193
194
// Max {roll,pitch} angle request
float yaml_max_roll_pitch_request_degrees = 30.0;
float yaml_max_roll_pitch_request_radians = 30.0 * DEG2RAD;

195
196
197
198
// Max error for yaw angle
float yaml_max_setpoint_error_yaw_degrees = 60.0;
float yaml_max_setpoint_error_yaw_radians = 60.0 * DEG2RAD;

199
200
201
202
203
// Theshold for {roll,pitch} angle beyond
// which the motors are turned off
float yaml_threshold_roll_pitch_for_turn_off_degrees = 70.0;
float yaml_threshold_roll_pitch_for_turn_off_radians = 70.0 * DEG2RAD;

204
205
// The thrust for take off spin motors
float yaml_takeoff_spin_motors_thrust = 8000;
206
207
// The time for: take off spin(-up) motors
float yaml_takoff_spin_motors_time = 0.8;
208
209
210
211

// Height change for the take off move-up
float yaml_takeoff_move_up_start_height = 0.1;
float yaml_takeoff_move_up_end_height   = 0.4;
212
// The time for: take off spin motors
213
float yaml_takoff_move_up_time = 2.0;
214

215
// Minimum and maximum allowed time for: take off goto setpoint
216
217
float yaml_takoff_goto_setpoint_nearby_time = 1.0;
float yaml_takoff_goto_setpoint_max_time    = 4.0;
218
219
220
221
222
223
224

// Box within which to keep the integrator on
// > Units of [meters]
// > The box consider is plus/minus this value
float yaml_takoff_integrator_on_box_horizontal = 0.25;
float yaml_takoff_integrator_on_box_vertical   = 0.15;
// The time for: take off integrator-on
225
float yaml_takoff_integrator_on_time = 2.0;
226
227
228
229
230
231


// Height change for the landing move-down
float yaml_landing_move_down_end_height_setpoint  = 0.05;
float yaml_landing_move_down_end_height_threshold = 0.10;
// The time for: landing move-down
232
float yaml_landing_move_down_time_max = 5.0;
233
234
235
236

// The thrust for landing spin motors
float yaml_landing_spin_motors_thrust = 10000;
// The time for: landing spin motors
237
float yaml_landing_spin_motors_time = 1.5;
238
239


240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258


// The setpoint to be tracked, the ordering is (x,y,z,yaw),
// with units [meters,meters,meters,radians]
float m_setpoint[4] = {0.0,0.0,0.4,0.0};

// The setpoint that is actually used by the controller, this
// differs from the setpoint when smoothing is turned on
float m_setpoint_for_controller[4] = {0.0,0.0,0.4,0.0};

// Boolean for whether to limit rate of change of the setpoint
bool m_shouldSmoothSetpointChanges = true;





// ------------------------------------------------------
// VARIABLES THAT ARE STANDARD FOR A "CONTROLLER SERVICE"
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274

// The ID of the agent that this node is monitoring
int m_agentID;

// The ID of the agent that can coordinate this node
int m_coordID;

// NAMESPACES FOR THE PARAMETER SERVICES
// > For the paramter service of this agent
std::string m_namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string m_namespace_to_coordinator_parameter_service;




275
276
// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
// > the mass of the crazyflie, in [grams]
277
float yaml_cf_mass_in_grams = 25.0;
278
// > the weight of the Crazyflie in Newtons, i.e., mg
279
float m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81 / 1000.0;
280

281
// > the frequency at which the controller is running
282
float yaml_control_frequency = 200.0;
283
float m_control_deltaT = (1.0/200.0);
284

285
286
287
// > the coefficients of the 16-bit command to thrust conversion
std::vector<float> yaml_motorPoly = {5.484560e-4, 1.032633e-6, 2.130295e-11};

288
// > the min and max for saturating 16 bit thrust commands
289
290
291
float yaml_command_sixteenbit_min = 1000;
float yaml_command_sixteenbit_max = 65000;

292
293
// > the default setpoint, the ordering is (x,y,z,yaw),
//   with units [meters,meters,meters,radians]
294
295
std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};

296
297
298
// Boolean indiciating whether the "Debug Message" of this
// agent should be published or not
bool yaml_shouldPublishDebugMessage = false;
299

300
301
302
// Boolean indiciating whether the debugging ROS_INFO_STREAM
// should be displayed or not
bool yaml_shouldDisplayDebugInfo = false;
303
304
305



306
307
// VARIABLES FOR THE CONTROLLER

308
// > A flag for which controller to use:
309
int yaml_controller_method = CONTROLLER_METHOD_RATE_ANGLE_NESTED;
310

311
312
313
314
315
316
// The LQR Controller parameters for z-height
std::vector<float> yaml_gainMatrixThrust_2StateVector     =  { 0.98, 0.25};
// The LQR Controller parameters for "CONTROLLER_METHOD_RATES"
std::vector<float> yaml_gainMatrixRollRate_3StateVector   =  {-6.20,-3.00, 5.20};
std::vector<float> yaml_gainMatrixPitchRate_3StateVector  =  { 6.20, 3.00, 5.20};
// The LQR Controller parameters for "CONTROLLER_METHOD_RATE_ANGLE_NESTED"
317
318
std::vector<float> yaml_gainMatrixRollAngle_2StateVector  =  {-0.80,-0.50};
std::vector<float> yaml_gainMatrixPitchAngle_2StateVector =  { 0.80, 0.50};
319
320
321
322
float yaml_gainRollRate_fromAngle   =  4.00;
float yaml_gainPitchRate_fromAngle  =  4.00;
// The LQR Controller parameters for yaw
float yaml_gainYawRate_fromAngle    =  2.30;
323
324
325
326
// Integrator gains
float yaml_integratorGain_forThrust = 0.0;
float yaml_integratorGain_forTauXY  = 0.0;
float yaml_integratorGain_forTauYaw = 0.0;
327
328
329
330
331
332


// VARIABLES FOR THE ESTIMATOR

// Frequency at which the controller is running
float m_estimator_frequency = 200.0;
333

334
335
// > A flag for which estimator to use:
int yaml_estimator_method = ESTIMATOR_METHOD_FINITE_DIFFERENCE;
336

337
338
// > The current state interial estimate,
//   for use by the controller
339
float m_current_stateInertialEstimate[9];
340

341
342
343
// > The measurement of the Crazyflie at the "current" time step,
//   to avoid confusion
float m_current_xzy_rpy_measurement[6];
344

345
346
347
348
349
350
// > The measurement of the Crazyflie at the "previous" time step,
//   used for computing finite difference velocities
float m_previous_xzy_rpy_measurement[6];

// > The full 12 state estimate maintained by the finite
//   difference state estimator
351
float m_stateInterialEstimate_viaFiniteDifference[9];
352
353
354

// > The full 12 state estimate maintained by the point mass
//   kalman filter state estimator
355
float m_stateInterialEstimate_viaPointMassKalmanFilter[9];
356
357
358
359
360
361
362
363
364

// THE POINT MASS KALMAN FILTER (PMKF) GAINS AND ERROR EVOLUATION
// > For the (x,y,z) position
std::vector<float> yaml_PMKF_Ahat_row1_for_positions  =  {  0.6723, 0.0034};
std::vector<float> yaml_PMKF_Ahat_row2_for_positions  =  {-12.9648, 0.9352};
std::vector<float> yaml_PMKF_Kinf_for_positions       =  {  0.3277,12.9648};


// VARIABLES RELATING TO PUBLISHING
365
366
367
368

// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;

369
370
// ROS Publisher to inform the network about
// changes to the setpoint
371
372
ros::Publisher m_setpointChangedPublisher;

373
374
375
// ROS Publisher to inform the flying agent client
// when a requested manoeuvre is complete
ros::Publisher m_manoeuvreCompletePublisher;
376

377
378
379
380
// ROS Publisher for sending motors-off command
// to the flying agent client
ros::Publisher m_motorsOffToFlyingAgentClientPublisher;

381
382
// Describes the area of the crazyflie and other parameters
//CrazyflieContext m_context;
383

384
385
386
387
388
389
390
391
392
393
// Flag for whether a context is available or not
bool m_isAvailableContext = false;

// Variable for each coordinate
float m_symmetric_area_bounds_x = 0.5;
float m_symmetric_area_bounds_y = 0.5;
float m_symmetric_area_bounds_z = 0.5;

// Service Client from which the "CrazyflieContext" is loaded
ros::ServiceClient m_centralManager;
394
395
396



397
398
399
400




401
402
403
404
405
406
407
408
409
410
411
412
413
414
//    ----------------------------------------------------------------------------------
//    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
//
//    PPPP   RRRR    OOO   TTTTT   OOO   TTTTT  Y   Y  PPPP   EEEEE   SSSS
//    P   P  R   R  O   O    T    O   O    T     Y Y   P   P  E      S
//    PPPP   RRRR   O   O    T    O   O    T      Y    PPPP   EEE     SSS
//    P      R  R   O   O    T    O   O    T      Y    P      E          S
//    P      R   R   OOO     T     OOO     T      Y    P      EEEEE  SSSS
//    ----------------------------------------------------------------------------------

415
416
417
418
419
420
// These function prototypes are not strictly required for this code
// to complile, but adding the function prototypes here means the
// functions can be written in any order in the ".cpp" file.
// (If the function prototypes are not included then the functions
// need to written below in an order that ensure each function is
// implemented before it is called from another function)
421

422
423
424
// CALLBACK FOR THE REQUEST MANOEUVRE SERVICE
bool requestManoeuvreCallback(IntIntService::Request &request, IntIntService::Response &response);

425
426
// CONTROLLER COMPUTATIONS
bool calculateControlOutput(Controller::Request &request, Controller::Response &response);
427

428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
// > For the normal state
void computeResponse_for_normal(Controller::Response &response);
// > For the standby state (also used for unknown state)
void computeResponse_for_standby(Controller::Response &response);
// > For the take-off phases
void computeResponse_for_takeoff_move_up(Controller::Response &response);
void computeResponse_for_takeoff_spin_motors(Controller::Response &response);
void computeResponse_for_takeoff_goto_setpoint(Controller::Response &response);
void computeResponse_for_takeoff_integrator_on(Controller::Response &response);
// > For the landing phases
void computeResponse_for_landing_move_down(Controller::Response &response);
void computeResponse_for_landing_spin_motors(Controller::Response &response);

// SMOOTHING SETPOINT CHANGES
void smoothSetpointChanges( float target_setpoint[4] , float (&current_setpoint)[4] );

444
445
// > This function constructs the error in the body frame
//   before calling the appropriate control function
446
void calculateControlOutput_viaLQR_givenSetpoint(float setpoint[4], float stateInertial[9], Controller::Response &response, int integrator_flag);
447
// > The various functions that implement an LQR controller
448
void calculateControlOutput_viaLQR_givenError(float stateErrorBody[9], Controller::Response &response, int integrator_flag);
449
450
451
452
453
454
455
456

// ESTIMATOR COMPUTATIONS
void performEstimatorUpdate_forStateInterial(Controller::Request &request);
void performEstimatorUpdate_forStateInterial_viaFiniteDifference();
void performEstimatorUpdate_forStateInterial_viaPointMassKalmanFilter();

// PUBLISHING OF THE DEBUG MESSAGE
void construct_and_publish_debug_message(Controller::Request &request, Controller::Response &response);
457

458
459
// TRANSFORMATION OF THE (x,y) INERTIAL FRAME ERROR
// INTO AN (x,y) BODY FRAME ERROR
460
461
462
463
464
465
466
467
468
469
470
void convertIntoBodyFrame(float stateInertial[9], float (&stateBody)[9], float yaw_measured);

// CONVERSION FROM THRUST IN NEWTONS TO 16-BIT COMMAND
float computeMotorPolyBackward(float thrust);

// REQUEST SETPOINT CHANGE CALLBACK
void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint);

// CHANGE SETPOINT FUNCTION
void setNewSetpoint(float x, float y, float z, float yaw);

471
472
// GET CURRENT SETPOINT SERVICE CALLBACK
bool getCurrentSetpointCallback(GetSetpointService::Request &request, GetSetpointService::Response &response);
473

474
475
476
// PUBLISH THE CURRENT SETPOINT AND STATE
void publishCurrentSetpointAndState();

477
478
479
// CLIP TO MIN AND MAX
float clipToBounds(float val, float val_min, float val_max);

480
481
// CUSTOM COMMAND RECEIVED CALLBACK
void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived);
482

483
484
485
// PUBLISH MOTORS-OFF MESSAGE TO FLYING AGENT CLIENT
void publish_motors_off_to_flying_agent_client();

486
487
488
489
490
491
// FUNCTIONS FOR THE CONTEXT OF THIS AGENT
// > Callback that the context database changed
void crazyflieContextDatabaseChangedCallback(const std_msgs::Int32& msg);
// > For loading the context of this agent
void loadCrazyflieContext();

492
// LOADING OF YAML PARAMETERS
493
void timerCallback_initial_load_yaml(const ros::TimerEvent&);
494
495
void isReadyDefaultControllerYamlCallback(const IntWithHeader & msg);
void fetchDefaultControllerYamlParameters(ros::NodeHandle& nodeHandle);