FlyingAgentClient.h 13 KB
Newer Older
1
//    Copyright (C) 2019, ETH Zurich, D-ITET, Paul Beuchat
Paul Beuchat's avatar
Paul Beuchat committed
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
//    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.
//    
//    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:
//    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>

50
51
52
53
// Include the standard message types
#include "std_msgs/Int32.h"
#include "std_msgs/Float32.h"
//#include <std_msgs/String.h>
Paul Beuchat's avatar
Paul Beuchat committed
54

55
// Include the DFALL message types
56
57
58
59
60
61
#include "dfall_pkg/IntWithHeader.h"
#include "dfall_pkg/ViconData.h"
#include "dfall_pkg/CrazyflieData.h"
#include "dfall_pkg/ControlCommand.h"
#include "dfall_pkg/CrazyflieContext.h"
#include "dfall_pkg/Setpoint.h"
62
63

// Include the DFALL service types
64
65
66
#include "dfall_pkg/Controller.h"
#include "dfall_pkg/CMQuery.h"
#include "dfall_pkg/IntIntService.h"
Paul Beuchat's avatar
Paul Beuchat committed
67

68
// Include the shared definitions
69
#include "nodes/Constants.h"
70
#include "nodes/DefaultControllerConstants.h"
71

72
73
74
// Include other classes
#include "classes/GetParamtersAndNamespaces.h"

Paul Beuchat's avatar
Paul Beuchat committed
75
76
77
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>

78
79
80
81
82




// Namespacing the package
83
using namespace dfall_pkg;
Paul Beuchat's avatar
Paul Beuchat committed
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98





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



99

Paul Beuchat's avatar
Paul Beuchat committed
100
101
102
103
104
105
106
107
108
109
110
111
112





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

113
114
115
116
117
118
119
120
121
122
123
124
// 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;

Paul Beuchat's avatar
Paul Beuchat committed
125

126

127
128
129
130
131
132
133
134
135
136
137
// VARIABLES FOR THE MOTION CAPTURE DATA
// > The index for which element in the "motion capture
//   data" array is expected to match the name in
//   "m_context", (negative numbers indicate unknown)
int m_poseDataIndex = -1;
// > Boolen indicating if the Mocap data is availble
bool m_isAvailable_mocap_data = false;
// > Boolen indicating if the object is "long term" occuled
bool m_isOcculded_mocap_data = false;
// > Number of consecutive occulsions that trigger the
//   "m_isOcculded_mocap_data" variable to be "true"
138
int yaml_consecutive_occulsions_threshold = 30;
139
140
141
// > Timer that when triggered determines that the
//   "m_isAvailable_mocap_data" variable becomes true
ros::Timer m_timer_mocap_timeout_check;
142
143
// > Time out duration after which Mocap is considered unavailable
float yaml_mocap_timeout_duration = 1.0;
144
145
146
147
148
149
150
151
152
153



// VARIABLES FOR STORING THE PARAMTER OF THE POSITION
// AND TILT ANGLE SAFTY CHECKS
// > Boolean indicating whether the tilt angle check
//   should be performed
bool yaml_isEnabled_strictSafety = true;
// > The maximum allowed tilt angle, in degrees and radians
float yaml_maxTiltAngle_for_strictSafety_degrees = 70;
154
float yaml_maxTiltAngle_for_strictSafety_radians = 70 * DEG2RAD;
155
156
157



158
159
160
161
162
163
// VARIABLES FOR MANAGING THE FLYING STATE
// > Integer that is the current flying state
int m_flying_state;
// > Booleans for whether the {take-off,landing} should
//   be performed with the default controller
bool yaml_shouldPerfom_takeOff_with_defaultController = true;
164
bool yaml_shouldPerfom_landing_with_defaultController = true;
165
166
167
168
169
170
171
172
173
174
175
176
// > Service Clients for requesting the Default controller
//   to perform a {take-off,landing} maneouvre
ros::ServiceClient m_defaultController_requestManoeuvre;
// > Timer that fire when the {take-off,landing} is complete
ros::Timer m_timer_takeoff_complete;
ros::Timer m_timer_land_complete;



// VARIABLES RELATING TO CONTROLLER SELECTION
// > Integer indicating the controller that is to be
//   used in when motion capture data is received
177
int m_instant_controller;
178
179
// > Pointer to the controller service client that
//   agrees with the "m_instant_controller" variable
180
ros::ServiceClient* m_instant_controller_service_client;
181
182
// > Boolean indicating that the controller service clients
//   have been successfully created
183
bool m_controllers_avialble = false;
184
185
// > Timer for creating the controller service client after
//   some delay
186
ros::Timer m_timer_for_createAllControllerServiceClients;
187
188
189
190
// > Integer indicating the controller that has been
//   requested. This controller is used during the "flying"
//   state, and the "Default" controller is used during the
//   "take-off" and "landing" states.
191
192
193
int m_controller_nominally_selected;


194
195

// VARIABLES FOR THE CONTROLER SERVIVCE CLIENTS
196
// The default controller specified in the FlyingAgentClientConfig.yaml
197
ros::ServiceClient m_defaultController;
198
// The Student controller specified in the FlyingAgentClientConfig.yaml
199
ros::ServiceClient m_studentController;
200
// The Tuning controller specified in the FlyingAgentClientConfig.yaml
201
ros::ServiceClient m_tuningController;
202
// The Picker controller specified in the FlyingAgentClientConfig.yaml
203
ros::ServiceClient m_pickerController;
204
// The Template controller specified in the FlyingAgentClientConfig.yaml
205
ros::ServiceClient m_templateController;
206

Paul Beuchat's avatar
Paul Beuchat committed
207

208
209
210



Paul Beuchat's avatar
Paul Beuchat committed
211
212


213
214
// variable for crazyradio status
int m_crazyradio_status;
Paul Beuchat's avatar
Paul Beuchat committed
215

216
217
//describes the area of the crazyflie and other parameters
CrazyflieContext m_context;
Paul Beuchat's avatar
Paul Beuchat committed
218

219
// Service Client from which the "CrazyflieContext" is loaded
220
ros::ServiceClient m_centralManager;
221
222
223
224
225

// Publisher for the control actions to be sent on
// to the Crazyflie (the CrazyRadio node listen to this
// publisher and actually send the commands)
// {onboardControllerType,roll,pitch,yaw,motorCmd1,motorCmd2,motorCmd3,motorCmd4}
226
ros::Publisher m_commandForSendingToCrazyfliePublisher;
Paul Beuchat's avatar
Paul Beuchat committed
227

228
// Publisher for the current flying state of this Flying Agent Client
229
ros::Publisher m_flyingStatePublisher;
Paul Beuchat's avatar
Paul Beuchat committed
230

231
232
233
// Publisher for the commands of:
// {take-off,land,motors-off, and which controller to use}
//ros::Publisher commandPublisher;
Paul Beuchat's avatar
Paul Beuchat committed
234

235
// Publisher Communication with crazyRadio node. Connect and disconnect
236
ros::Publisher m_crazyRadioCommandPublisher;
Paul Beuchat's avatar
Paul Beuchat committed
237

238
// Publisher for which controller is currently being used
239
ros::Publisher m_controllerUsedPublisher;
Paul Beuchat's avatar
Paul Beuchat committed
240
241


242

Paul Beuchat's avatar
Paul Beuchat committed
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260


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



261
262
263
// FUNCTIONS FOR HANDLING THE MOTION CAPTURE DATA
// > Callback run every time new motion capture
//   data is available
264
void viconCallback(const ViconData& viconData);
265
266
// > For extracting the pose data of an specific
//   object by name
267
int getPoseDataForObjectNameWithExpectedIndex(const ViconData& viconData, std::string name , int expected_index , CrazyflieData& pose);
268
269
// > For converting the global frame motion capture
//   data to the local frame of this agent
270
void coordinatesToLocal(CrazyflieData& cf);
271
272
273
274
275
276
// > Callback run when motion capture data has not
//   been receive for a specified time
void timerCallback_mocap_timeout_check(const ros::TimerEvent&);
// > For sending a command, via the CrazyRadio, that
//   the motors should be set to zero
void sendZeroOutputCommandForMotors();
277

278

279

280
281
// FOR THE SAFETY CHECKS ON POSITION AND TILT ANGLE
bool safetyCheck_on_positionAndTilt(CrazyflieData data);
Paul Beuchat's avatar
Paul Beuchat committed
282

283

284

285
286
287
288
289
290
291
292
293
// FUNCTIONS THAT MANAGE THE CHANGES TO THE FLYING STATE
// > For changing between possible state of:
//   {take-off,flying,take-off,motors-off}
void requestChangeFlyingStateTo(int new_state);
// > For changing to take-off
void requestChangeFlyingStateToTakeOff();
// > For changing to land
void requestChangeFlyingStateToLand();
// > Callback that the take off phase is complete
294
void takeOffTimerCallback(const ros::TimerEvent&);
295
// > Callback that the landing phase is complete
296
void landTimerCallback(const ros::TimerEvent&);
Paul Beuchat's avatar
Paul Beuchat committed
297
298


299

300
301
// FUNCTIONS FOR SELECTING WHICH CONTROLLER TO USE
// > For setting the controller that is actually used
Paul Beuchat's avatar
Paul Beuchat committed
302
void setInstantController(int controller);
303
// > For retrieving the value of the class variable
Paul Beuchat's avatar
Paul Beuchat committed
304
int getInstantController();
305
306
// > For setting the controller that it to be used
//   during the normal "flying" state
307
void setControllerNominallySelected(int controller);
308
// > For retrieving the value of the class variable
309
310
311
312
int getControllerNominallySelected();



313
314
315
316
317
318
319
// THE CALLBACK THAT A NEW FLYING STATE WAS REQUESTED
// > These requests come from the "Flying Agent GUI"
// > The options are: {take-off,land,motor-off,controller}
void flyingStateOrControllerRequestCallback(const IntWithHeader & commandMsg);



320
321
// THE CALLBACK THAT THE CRAZY RADIO STATUS CHANGED
void crazyRadioStatusCallback(const std_msgs::Int32& msg);
Paul Beuchat's avatar
Paul Beuchat committed
322

323
324


325
326
// THE CALLBACK THAT AN EMERGENCY STOP MESSAGE WAS RECEIVED
void emergencyStopReceivedCallback(const IntWithHeader & msg);
Paul Beuchat's avatar
Paul Beuchat committed
327

328
329


330
331
332
// THE SERVICE CALLBACK REQUESTING THE CURRENT FLYING STATE
bool getCurrentFlyingStateServiceCallback(IntIntService::Request &request, IntIntService::Response &response);

333
334


335
// FOR THE BATTERY STATE CALLBACK
336
337
void batteryMonitorStateChangedCallback(std_msgs::Int32 msg);

338

339

340
341
342
343
// 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
344
345
void loadCrazyflieContext();

346
347
348
349
350
351


// FUNCTIONS FOR CREATING THE CONTROLLER SERVICE CLIENT
// > For creating a service client from the name
//   of the YAML parameter
void createControllerServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
352
353
354
// > For creating an IntInt service client from the
//   name of a YAML paramter
void createIntIntServiceClientFromParameterName( std::string paramter_name , ros::ServiceClient& serviceClient );
355
356
357
358
359
360
// > Callback for the timer so that all services servers
//   exists before we try to create the clients
void timerCallback_for_createAllcontrollerServiceClients(const ros::TimerEvent&);



361
// FOR LOADING THE YAML PARAMETERS
362
363
void isReadyFlyingAgentClientConfigYamlCallback(const IntWithHeader & msg);
void fetchFlyingAgentClientConfigYamlParameters(ros::NodeHandle& nodeHandle);