PPSClient.cpp 16.8 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//    ROS node that manages the student's setup.
//    Copyright (C) 2017  Cyrill Burgener, Marco Mueller, Philipp Friedli
//
//    This program 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.
//
//    This program 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 this program.  If not, see <http://www.gnu.org/licenses/>.

#include "ros/ros.h"
muelmarc's avatar
muelmarc committed
18
#include <stdlib.h>
19
#include <std_msgs/String.h>
20
#include <rosbag/bag.h>
muelmarc's avatar
muelmarc committed
21
#include <ros/package.h>
22

23
#include "d_fall_pps/Controller.h"
24
#include "d_fall_pps/CMQuery.h"
25
26

#include "d_fall_pps/ViconData.h"
27
#include "d_fall_pps/CrazyflieData.h"
28
#include "d_fall_pps/ControlCommand.h"
muelmarc's avatar
muelmarc committed
29
#include "d_fall_pps/CrazyflieContext.h"
30
#include "d_fall_pps/Setpoint.h"
31
#include "std_msgs/Int32.h"
32

muelmarc's avatar
muelmarc committed
33

34
#include "d_fall_pps/ControlCommand.h"
35

36
#define CMD_USE_SAFE_CONTROLLER   1
37
#define CMD_USE_CUSTOM_CONTROLLER 2
38
39
40
41
42
43
44
45
46
47
#define CMD_CRAZYFLY_TAKE_OFF     3
#define CMD_CRAZYFLY_LAND         4
#define CMD_CRAZYFLY_MOTORS_OFF   5

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

48
49
50
51
#define TAKE_OFF_OFFSET  1    //in meters
#define LANDING_DISTANCE 0.15    //in meters
#define DURATION_TAKE_OFF  300   //100 is 1 second
#define DURATION_LANDING   300   //100 is 1 second
52

53
54
#define PI 3.141592653589

55
using namespace d_fall_pps;
56

57
58
//studentID, gives namespace and identifier in CentralManagerService
int studentID;
59

60
//the safe controller specified in the ClientConfig.yaml, is considered stable
61
ros::ServiceClient safeController;
62
//the custom controller specified in the ClientConfig.yaml, is considered potentially unstable
63
ros::ServiceClient customController;
64

65
66
67
//values for safteyCheck
bool strictSafety;
float angleMargin;
68

69
ros::ServiceClient centralManager;
70
ros::Publisher controlCommandPublisher;
71

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

75
76
77
// publisher for flying state
ros::Publisher flyingStatePublisher;

78
79
rosbag::Bag bag;

80
81
82
83
// variables for the states:
int flying_state;
bool changed_state_flag;

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

87
88
//wheter to use safe of custom controller
bool usingSafeController;
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117


void loadSafeController() {
	ros::NodeHandle nodeHandle("~");

	std::string safeControllerName;
	if(!nodeHandle.getParam("safeController", safeControllerName)) {
		ROS_ERROR("Failed to get safe controller name");
		return;
	}

	ros::service::waitForService(safeControllerName);
	safeController = ros::service::createClient<Controller>(safeControllerName, true);
    ROS_INFO_STREAM("loaded safe controller: " << safeController.getService());
}

void loadCustomController() {
	ros::NodeHandle nodeHandle("~");

	std::string customControllerName;
	if(!nodeHandle.getParam("customController", customControllerName)) {
		ROS_ERROR("Failed to get custom controller name");
		return;
	}

	customController = ros::service::createClient<Controller>(customControllerName, true);
    ROS_INFO_STREAM("loaded custom controller " << customControllerName);
}

118

119
//checks if crazyflie is within allowed area and if custom controller returns no data
120
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
121
	//position check
122
	if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
123
		ROS_INFO_STREAM("x safety failed");
124
		return false;
muelmarc's avatar
muelmarc committed
125
	}
126
	if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
127
		ROS_INFO_STREAM("y safety failed");
128
		return false;
muelmarc's avatar
muelmarc committed
129
	}
130
	if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
131
		ROS_INFO_STREAM("z safety failed");
132
		return false;
muelmarc's avatar
muelmarc committed
133
	}
134

135
136
137
138
139
140
141
142
143
144
145
146
147
	//attitude check
	//if strictSafety is set to true in ClientConfig.yaml the SafeController takes also over if the roll and pitch angles get to large
	//the angleMargin is a value in the range (0,1). The closer to 1, the closer to 90 deg are the roll and pitch angles allowed to become before the safeController takes over
	if(strictSafety){
		if((data.roll > PI/2*angleMargin) or (data.roll < -PI/2*angleMargin)) {
			ROS_INFO_STREAM("roll too big.");
			return false;
		}
		if((data.pitch > PI/2*angleMargin) or (data.pitch < -PI/2*angleMargin)) {
			ROS_INFO_STREAM("pitch too big.");
			return false;
		}
	}
148

149
	return true;
150
}
phfriedl's avatar
phfriedl committed
151

bucyril's avatar
bucyril committed
152
153
154
155
void coordinatesToLocal(CrazyflieData& cf) {
	AreaBounds area = context.localArea;
	float originX = (area.xmin + area.xmax) / 2.0;
	float originY = (area.ymin + area.ymax) / 2.0;
156
157
158
    // 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;
bucyril's avatar
bucyril committed
159
160
161
162
163
164

	cf.x -= originX;
	cf.y -= originY;
	cf.z -= originZ;
}

165
166
167
168
169
170
171
172
173


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 + TAKE_OFF_OFFSET;           //previous one plus some offset
174
175
    // setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
    setpoint_msg.yaw = 0.0;
176
    safeControllerServiceSetpointPublisher.publish(setpoint_msg);
177
178
179
180
181
    ROS_INFO("Take OFF:");
    ROS_INFO("------Current coordinates:");
    ROS_INFO("X: %f, Y: %f, Z: %f", current_local_coordinates.x, current_local_coordinates.y, current_local_coordinates.z);
    ROS_INFO("------New coordinates:");
    ROS_INFO("X: %f, Y: %f, Z: %f", setpoint_msg.x, setpoint_msg.y, setpoint_msg.z);
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203

    // now, use safe controller to go to that setpoint
    usingSafeController = true;
    loadSafeController();
    // when do we finish? after some time with delay?
}

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 = LANDING_DISTANCE;           //previous one plus some offset
    setpoint_msg.yaw = current_local_coordinates.yaw;          //previous one
    safeControllerServiceSetpointPublisher.publish(setpoint_msg);

    // now, use safe controller to go to that setpoint
    usingSafeController = true;
    loadSafeController();
}

204
205
206
207
208
209
210
211
212
213
void goToOrigin()
{
    Setpoint setpoint_msg;
    setpoint_msg.x = 0;
    setpoint_msg.y = 0;
    setpoint_msg.z = 0.4;
    setpoint_msg.yaw = 0;
    safeControllerServiceSetpointPublisher.publish(setpoint_msg);
}

214
215
void changeFlyingStateTo(int new_state)
{
216
    ROS_INFO("Change state to: %d", new_state);
217
218
    flying_state = new_state;
    changed_state_flag = true;
219
220
221
    std_msgs::Int32 flying_state_msg;
    flying_state_msg.data = flying_state;
    flyingStatePublisher.publish(flying_state_msg);
222
223
224
225
226
227
}

int counter = 0;
bool finished_take_off = false;
bool finished_land = false;

228
//is called when new data from Vicon arrives
bucyril's avatar
bucyril committed
229
void viconCallback(const ViconData& viconData) {
230
	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
bucyril's avatar
bucyril committed
231
		CrazyflieData global = *it;
232

233
        if(global.crazyflieName == context.crazyflieName)
234
        {
235
236
237
238
239
240
241
242
243
244
245
            Controller controllerCall;
            CrazyflieData local = global;
            coordinatesToLocal(local);
            controllerCall.request.ownCrazyflie = local;

            switch(flying_state) //things here repeat every X ms, non-blocking stuff
            {
                case STATE_MOTORS_OFF: // here we will put the code of current disabled button
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
246
                        ROS_INFO("STATE_MOTORS_OFF");
247
248
249
250
251
252
253
254
255
                    }
                    break;
                case STATE_TAKE_OFF: //we need to move up from where we are now.
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
                        takeOffCF(local);
                        counter = 0;
                        finished_take_off = false;
256
                        ROS_INFO("STATE_TAKE_OFF");
257
258
                    }
                    counter++;
259
                    if(counter >= DURATION_TAKE_OFF)
260
261
262
263
264
265
266
267
268
269
270
271
272
273
                    {
                        counter = 0;
                        finished_take_off = true;
                    }
                    if(finished_take_off)
                    {
                        finished_take_off = false;
                        changeFlyingStateTo(STATE_FLYING);
                    }
                    break;
                case STATE_FLYING:
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
274
275
                        // need to change setpoint to the one from file
                        goToOrigin();
276
                        ROS_INFO("STATE_FLYING");
277
278
279
280
281
282
283
284
285
                    }
                    break;
                case STATE_LAND:
                    if(changed_state_flag) // stuff that will be run only once when changing state
                    {
                        changed_state_flag = false;
                        landCF(local);
                        counter = 0;
                        finished_land = false;
286
                        ROS_INFO("STATE_LAND");
287
288
                    }
                    counter++;
289
                    if(counter >= DURATION_LANDING)
290
291
292
293
294
295
296
297
298
299
300
301
302
                    {
                        counter = 0;
                        finished_land = true;
                    }
                    if(finished_land)
                    {
                        finished_land = false;
                        changeFlyingStateTo(STATE_MOTORS_OFF);
                    }
                    break;
                default:
                    break;
            }
303

304
305
306
307
308
309
310
            // control part that should always be running, calls to controller, computation of control output
            if(flying_state != STATE_MOTORS_OFF)
            {
                if(!global.occluded)    //if it is not occluded, then proceed to compute the controller output.
                {
                    if(!usingSafeController && flying_state == STATE_FLYING) // only enter in custom controller if we are not using safe controller AND the flying state is FLYING
                    {
311
                        bool success = customController.call(controllerCall);
312
313
                        if(!success)
                        {
314
315
316
317
                            ROS_ERROR("Failed to call custom controller, switching to safe controller");
                            ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists());
                            ROS_ERROR_STREAM("custom controller name: " << customController.getService());
                            usingSafeController = true;
318
319
320
                        }
                        else if(!safetyCheck(global, controllerCall.response.controlOutput))
                        {
321
322
323
324
                            usingSafeController = true;
                            ROS_INFO_STREAM("safety check failed, switching to safe controller");
                        }
                    }
325
326
                    else
                    {
327
328
329
330
331
332
333
334
335
336
337
338
339
340
                        bool success = safeController.call(controllerCall);
                        if(!success) {
                            ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
                        }
                    }

                    //ROS_INFO_STREAM("safe controller active: " << usingSafeController);

                    controlCommandPublisher.publish(controllerCall.response.controlOutput);

                    bag.write("ViconData", ros::Time::now(), local);
                    bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
                }
            }
341
342
343
344
345
346
347
348
            else
            {
                ControlCommand zeroOutput = ControlCommand(); //everything set to zero
                zeroOutput.onboardControllerType = 2; //set to motor_mode
                controlCommandPublisher.publish(zeroOutput);
                bag.write("ViconData", ros::Time::now(), local);
                bag.write("ControlOutput", ros::Time::now(), zeroOutput);
            }
349
        }
350
351
352
353
	}
}

void loadParameters(ros::NodeHandle& nodeHandle) {
354
355
356
	if(!nodeHandle.getParam("studentID", studentID)) {
		ROS_ERROR("Failed to get studentID");
	}
357
358
359
360
361
362
363
364
365
366
	if(!nodeHandle.getParam("strictSafety", strictSafety)) {
		ROS_ERROR("Failed to get strictSafety param");
		return;
	}
	if(!nodeHandle.getParam("angleMargin", angleMargin)) {
		ROS_ERROR("Failed to get angleMargin param");
		return;
	}


367
368
}

369
void loadCrazyflieContext() {
370
371
	CMQuery contextCall;
	contextCall.request.studentID = studentID;
Cyrill Burgener's avatar
Cyrill Burgener committed
372
	ROS_INFO_STREAM("StudentID:" << studentID);
373

374
375
376
377
	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall)) {
		context = contextCall.response.crazyflieContext;
378
		ROS_INFO_STREAM("CrazyflieContext:\n" << context);
379
	} else {
380
		ROS_ERROR("Failed to load context");
381
	}
382
383
384

	ros::NodeHandle nh("CrazyRadio");
	nh.setParam("crazyFlieAddress", context.crazyflieAddress);
385
}
386

387

388
389
390

void commandCallback(const std_msgs::Int32& commandMsg) {
	int cmd = commandMsg.data;
391

392
393
394
395
396
397
398
399
400
401
402
	switch(cmd) {
    	case CMD_USE_SAFE_CONTROLLER:
    		loadSafeController();
    		usingSafeController = true;
    		break;

    	case CMD_USE_CUSTOM_CONTROLLER:
    		loadCustomController();
    		usingSafeController = false;
    		break;

403
    	case CMD_CRAZYFLY_TAKE_OFF:
404
405
406
407
            if(flying_state == STATE_MOTORS_OFF)
            {
                changeFlyingStateTo(STATE_TAKE_OFF);
            }
408
409
    		break;

410
    	case CMD_CRAZYFLY_LAND:
411
412
413
414
            if(flying_state != STATE_MOTORS_OFF)
            {
                changeFlyingStateTo(STATE_LAND);
            }
415
    		break;
416
417
418
        case CMD_CRAZYFLY_MOTORS_OFF:
            changeFlyingStateTo(STATE_MOTORS_OFF);
            break;
419

420
421
422
    	default:
    		ROS_ERROR_STREAM("unexpected command number: " << cmd);
    		break;
423
	}
424
425
}

426
427
int main(int argc, char* argv[])
{
428
429
430
	ros::init(argc, argv, "PPSClient");
	ros::NodeHandle nodeHandle("~");
	loadParameters(nodeHandle);
431
432


433
	//ros::service::waitForService("/CentralManagerService/CentralManager");
434
	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
435
	loadCrazyflieContext();
436

437
438
	//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
439
	ROS_INFO_STREAM("successfully subscribed to ViconData");
440

441
	//ros::Publishers to advertise the control output
bucyril's avatar
bucyril committed
442
	controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
phfriedl's avatar
phfriedl committed
443

444
445
	//this topic lets the PPSClient listen to the terminal commands
    ros::Publisher commandPublisher = nodeHandle.advertise<std_msgs::Int32>("Command", 1);
bucyril's avatar
bucyril committed
446
    ros::Subscriber commandSubscriber = nodeHandle.subscribe("Command", 1, commandCallback);
phfriedl's avatar
phfriedl committed
447

448
449
450
    //this topic lets us use the terminal to communicate with crazyRadio node.
    ros::Publisher crazyRadioCommandPublisher = nodeHandle.advertise<std_msgs::Int32>("crazyRadioCommand", 1);

451
452
453
454
455
456
457
    // this topic will publish flying state whenever it changes.
    flyingStatePublisher = nodeHandle.advertise<std_msgs::Int32>("flyingState", 1);
    // publish first flying state data
    std_msgs::Int32 flying_state_msg;
    flying_state_msg.data = flying_state;
    flyingStatePublisher.publish(flying_state_msg);

458
459
460
461
    // SafeControllerServicePublisher:
    ros::NodeHandle namespaceNodeHandle = ros::NodeHandle();
    safeControllerServiceSetpointPublisher = namespaceNodeHandle.advertise<d_fall_pps::Setpoint>("SafeControllerService/Setpoint", 1);

462
	//start with safe controller
463
    flying_state = STATE_MOTORS_OFF;
464
465
	usingSafeController = true;
	loadSafeController();
466

muelmarc's avatar
muelmarc committed
467
468
469
	std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	ROS_INFO_STREAM(package_path);
470
	std::string record_file = package_path + "LoggingPPSClient.bag";
muelmarc's avatar
muelmarc committed
471
472
	bag.open(record_file, rosbag::bagmode::Write);

473
    ros::spin();
muelmarc's avatar
muelmarc committed
474
	bag.close();
475
    return 0;
476
}