PPSClient.h 9.93 KB
Newer Older
Paul Beuchat's avatar
Paul Beuchat committed
1
2
3
//    Copyright (C) 2017, ETH Zurich, D-ITET, Paul Beuchat, Angel Romero, Cyrill Burgener, Marco Mueller, Philipp Friedli
//
//    This file is part of D-FaLL-System.
pragash1's avatar
pragash1 committed
4
//
Paul Beuchat's avatar
Paul Beuchat committed
5
6
7
8
//    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.
pragash1's avatar
pragash1 committed
9
//
Paul Beuchat's avatar
Paul Beuchat committed
10
11
12
13
//    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.
pragash1's avatar
pragash1 committed
14
//
Paul Beuchat's avatar
Paul Beuchat committed
15
16
//    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/>.
pragash1's avatar
pragash1 committed
17
//
Paul Beuchat's avatar
Paul Beuchat committed
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
59
//
//    ----------------------------------------------------------------------------------
//    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.
//
//    ----------------------------------------------------------------------------------





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

#include "ros/ros.h"
#include <stdlib.h>
#include <std_msgs/String.h>
#include <ros/package.h>

#include "d_fall_pps/Controller.h"
#include "d_fall_pps/CMQuery.h"

#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/CrazyflieData.h"
#include "d_fall_pps/ControlCommand.h"
#include "d_fall_pps/CrazyflieContext.h"
#include "d_fall_pps/Setpoint.h"
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"

60
61
62
// Include the Parameter Service shared definitions
#include "nodes/ParameterServiceDefinitions.h"

Paul Beuchat's avatar
Paul Beuchat committed
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>

#include "d_fall_pps/ControlCommand.h"





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

// Types PPS firmware
#define CF_COMMAND_TYPE_MOTORS 6
#define CF_COMMAND_TYPE_RATE   7
#define CF_COMMAND_TYPE_ANGLE  8

// Types of controllers being used:
86
87
#define SAFE_CONTROLLER      1
#define DEMO_CONTROLLER      2
88
89
#define STUDENT_CONTROLLER   3
#define MPC_CONTROLLER       4
90
#define REMOTE_CONTROLLER    5
91
#define TUNING_CONTROLLER    6
92
#define PICKER_CONTROLLER    7
pragash1's avatar
pragash1 committed
93
#define DRONEX_CONTROLLER    8
Paul Beuchat's avatar
Paul Beuchat committed
94
95
96

// The constants that "command" changes in the
// operation state of this agent
97
98
99
100
#define CMD_USE_SAFE_CONTROLLER      1
#define CMD_USE_DEMO_CONTROLLER      2
#define CMD_USE_STUDENT_CONTROLLER   3
#define CMD_USE_MPC_CONTROLLER       4
101
#define CMD_USE_REMOTE_CONTROLLER    5
102
#define CMD_USE_TUNING_CONTROLLER    6
103
#define CMD_USE_PICKER_CONTROLLER    7
pragash1's avatar
pragash1 committed
104
#define CMD_USE_DRONEX_CONTROLLER    8
105

106
107
108
109

#define CMD_CRAZYFLY_TAKE_OFF        11
#define CMD_CRAZYFLY_LAND            12
#define CMD_CRAZYFLY_MOTORS_OFF      13
Paul Beuchat's avatar
Paul Beuchat committed
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149

// Flying states
#define STATE_MOTORS_OFF 1
#define STATE_TAKE_OFF   2
#define STATE_FLYING     3
#define STATE_LAND       4

// Battery states
#define BATTERY_STATE_NORMAL 0
#define BATTERY_STATE_LOW    1

// Commands for CrazyRadio
#define CMD_RECONNECT  0
#define CMD_DISCONNECT 1


// CrazyRadio states:
#define CONNECTED        0
#define CONNECTING       1
#define DISCONNECTED     2


// Universal constants
#define PI 3.141592653589

// Namespacing the package
using namespace d_fall_pps;





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

150
151
// "agentID", gives namespace and identifier in CentralManagerService
int agentID;
Paul Beuchat's avatar
Paul Beuchat committed
152

153
// The safe controller specified in the ClientConfig.yaml
Paul Beuchat's avatar
Paul Beuchat committed
154
ros::ServiceClient safeController;
155
// The Demo controller specified in the ClientConfig.yaml
Paul Beuchat's avatar
Paul Beuchat committed
156
ros::ServiceClient demoController;
157
// The Student controller specified in the ClientConfig.yaml
158
ros::ServiceClient studentController;
159
// The MPC controller specified in the ClientConfig.yaml
160
ros::ServiceClient mpcController;
161
162
// The Remote controller specified in the ClientConfig.yaml
ros::ServiceClient remoteController;
163
164
// The Tuning controller specified in the ClientConfig.yaml
ros::ServiceClient tuningController;
165
166
// The Picker controller specified in the ClientConfig.yaml
ros::ServiceClient pickerController;
pragash1's avatar
pragash1 committed
167
168
169
// The DroneX controller specified in the ClientConfig.yaml
//TODO
ros::ServiceClient dronexController;
170

Paul Beuchat's avatar
Paul Beuchat committed
171
172
173
174
175
176
177
178
179
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
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284

//values for safteyCheck
bool strictSafety;
float angleMargin;

// battery threshold
float m_battery_threshold_while_flying;
float m_battery_threshold_while_motors_off;


// battery values

int m_battery_state;
float m_battery_voltage;

Setpoint controller_setpoint;

// variables for linear trayectory
Setpoint current_safe_setpoint;
double distance;
double unit_vector[3];
bool was_in_threshold = false;
double distance_threshold;      //to be loaded from yaml


ros::ServiceClient centralManager;
ros::Publisher controlCommandPublisher;

// communicate with safeControllerService, setpoint, etc...
ros::Publisher safeControllerServiceSetpointPublisher;

// publisher for flying state
ros::Publisher flyingStatePublisher;

// publisher for battery state
ros::Publisher batteryStatePublisher;

// publisher to send commands to itself.
ros::Publisher commandPublisher;

// communication with crazyRadio node. Connect and disconnect
ros::Publisher crazyRadioCommandPublisher;


// Variable for the namespaces for the paramter services
// > For the paramter service of this agent
std::string namespace_to_own_agent_parameter_service;
// > For the parameter service of the coordinator
std::string namespace_to_coordinator_parameter_service;


// variables for the states:
int flying_state;
bool changed_state_flag;

// variable for crazyradio status
int crazyradio_status;

//describes the area of the crazyflie and other parameters
CrazyflieContext context;

//wheter to use safe of demo controller
int instant_controller;         //variable for the instant controller, e.g., we use safe controller for taking off and landing even if demo controller is enabled. This variable WILL change automatically

// controller used:
int controller_used;

ros::Publisher controllerUsedPublisher;

std::string ros_namespace;

float take_off_distance;
float landing_distance;
float duration_take_off;
float duration_landing;

bool finished_take_off = false;
bool finished_land = false;

ros::Timer timer_takeoff;
ros::Timer timer_land;

// A ROS "bag" to store data for post-analysis
//rosbag::Bag bag;




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


// > For the LOAD PARAMETERS
void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);

void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);





285
286
void viconCallback(const ViconData& viconData);

Paul Beuchat's avatar
Paul Beuchat committed
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
// > For the LOADING of YAML PARAMETERS
void yamlReadyForFetchCallback(const std_msgs::Int32& msg);
void fetchYamlParametersForSafeController(ros::NodeHandle& nodeHandle);
void fetchClientConfigParameters(ros::NodeHandle& nodeHandle);



// > For the {dis/re}-connect command received from the coordinator
void crazyRadioCommandAllAgentsCallback(const std_msgs::Int32& msg);


// > For the BATTERY
int getBatteryState();
void setBatteryStateTo(int new_battery_state);
float movingAverageBatteryFilter(float new_input);
void CFBatteryCallback(const std_msgs::Float32& msg);






void loadSafeController();
void loadDemoController();
311
312
void loadStudentController();
void loadMpcController();
313
void loadRemoteController();
314
void loadTuningController();
315
void loadPickerController();
pragash1's avatar
pragash1 committed
316
void loadDroneXController();
317

Paul Beuchat's avatar
Paul Beuchat committed
318
319
320
321
322
void sendMessageUsingController(int controller);
void setInstantController(int controller);
int getInstantController();
void setControllerUsed(int controller);
int getControllerUsed();