PPSClient.cpp 9.16 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 "std_msgs/Int32.h"
31

muelmarc's avatar
muelmarc committed
32

33
#include "d_fall_pps/ControlCommand.h"
34

35
36
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
37
38
#define CMD_USE_CRAZYFLY_ENABLE 3
#define CMD_USE_CRAZYFLY_DISABLE 4
39

40
41
#define PI 3.141592653589

42
using namespace d_fall_pps;
43

44
45
//studentID, gives namespace and identifier in CentralManagerService
int studentID;
46

47
//the safe controller specified in the ClientConfig.yaml, is considered stable
48
ros::ServiceClient safeController;
49
//the custom controller specified in the ClientConfig.yaml, is considered potentially unstable
50
ros::ServiceClient customController;
51
52
53
//values for safteyCheck
bool strictSafety;
float angleMargin;
54

55
ros::ServiceClient centralManager;
56
ros::Publisher controlCommandPublisher;
57

58
59
rosbag::Bag bag;

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

63
64
//wheter to use safe of custom controller
bool usingSafeController;
65
66
//wheter crazyflie is enabled (ready to fly) or disabled (motors off)
bool crazyflieEnabled;
67

68
//checks if crazyflie is within allowed area and if custom controller returns no data
69
bool safetyCheck(CrazyflieData data, ControlCommand controlCommand) {
70
	//position check
71
	if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
72
		ROS_INFO_STREAM("x safety failed");
73
		return false;
muelmarc's avatar
muelmarc committed
74
	}
75
	if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
76
		ROS_INFO_STREAM("y safety failed");
77
		return false;
muelmarc's avatar
muelmarc committed
78
	}
79
	if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
80
		ROS_INFO_STREAM("z safety failed");
81
		return false;
muelmarc's avatar
muelmarc committed
82
	}
83

84
85
86
87
88
89
90
91
92
93
94
95
96
	//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;
		}
	}
97
98
	
	return true;
99
}
phfriedl's avatar
phfriedl committed
100

bucyril's avatar
bucyril committed
101
102
103
104
105
106
107
108
109
110
111
void coordinatesToLocal(CrazyflieData& cf) {
	AreaBounds area = context.localArea;
	float originX = (area.xmin + area.xmax) / 2.0;
	float originY = (area.ymin + area.ymax) / 2.0;
	float originZ = (area.zmin + area.zmax) / 2.0;

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

112
//is called when new data from Vicon arrives
bucyril's avatar
bucyril committed
113
void viconCallback(const ViconData& viconData) {
114
	for(std::vector<CrazyflieData>::const_iterator it = viconData.crazyflies.begin(); it != viconData.crazyflies.end(); ++it) {
bucyril's avatar
bucyril committed
115
		CrazyflieData global = *it;
116
		
bucyril's avatar
bucyril committed
117
		if(global.crazyflieName == context.crazyflieName) {
118
			Controller controllerCall;
119
			
bucyril's avatar
bucyril committed
120
121
122
			CrazyflieData local = global;
			coordinatesToLocal(local);
			controllerCall.request.ownCrazyflie = local;
123
124
125
126
			
			if(crazyflieEnabled){
				if(!usingSafeController) {
					bool success = customController.call(controllerCall);
127
					
128
129
					if(!success) {
						ROS_ERROR("Failed to call custom controller, switching to safe controller");
bucyril's avatar
bucyril committed
130
						ROS_ERROR_STREAM("custom controller status: valid: " << customController.isValid() << ", exists: " << customController.exists());
phfriedl's avatar
phfriedl committed
131
						ROS_ERROR_STREAM("custom controller name: " << customController.getService());
132
						usingSafeController = true;
muelmarc's avatar
muelmarc committed
133
134
					} else if(!safetyCheck(global, controllerCall.response.controlOutput)) {
						usingSafeController = true;
135
						ROS_INFO_STREAM("safety check failed, switching to safe controller");
136
					}
137
				}
138

139
				
140
141
142
				if(usingSafeController) {
					bool success = safeController.call(controllerCall);
					if(!success) {
bucyril's avatar
bucyril committed
143
						ROS_ERROR_STREAM("Failed to call safe controller, valid: " << safeController.isValid() << ", exists: " << safeController.exists());
144
					}
145
				}
146
147
				
				//ROS_INFO_STREAM("safe controller active: " << usingSafeController);
148

149
150
				controlCommandPublisher.publish(controllerCall.response.controlOutput);

bucyril's avatar
bucyril committed
151
				bag.write("ViconData", ros::Time::now(), local);
152
				bag.write("ControlOutput", ros::Time::now(), controllerCall.response.controlOutput);
muelmarc's avatar
muelmarc committed
153
			
154
155
156
157
			} else { //crazyflie disabled
				ControlCommand zeroOutput = ControlCommand(); //everything set to zero
				zeroOutput.onboardControllerType = 2; //set to motor_mode
				controlCommandPublisher.publish(zeroOutput);
bucyril's avatar
bucyril committed
158
				bag.write("ViconData", ros::Time::now(), local);
159
160
				bag.write("ControlOutput", ros::Time::now(), zeroOutput);
			}
161
		}
162
163
164
165
	}
}

void loadParameters(ros::NodeHandle& nodeHandle) {
166
167
168
	if(!nodeHandle.getParam("studentID", studentID)) {
		ROS_ERROR("Failed to get studentID");
	}
169
170
171
172
173
174
175
176
177
178
	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;
	}


179
180
}

181
void loadCrazyflieContext() {
182
183
	CMQuery contextCall;
	contextCall.request.studentID = studentID;
Cyrill Burgener's avatar
Cyrill Burgener committed
184
	ROS_INFO_STREAM("StudentID:" << studentID);
185
	
186
187
188
189
	centralManager.waitForExistence(ros::Duration(-1));

	if(centralManager.call(contextCall)) {
		context = contextCall.response.crazyflieContext;
190
		ROS_INFO_STREAM("CrazyflieContext:\n" << context);
191
	} else {
192
		ROS_ERROR("Failed to load context");
193
	}
194
195
196

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

199
200
201
202
203
204
205
206
207
208
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);
209
210
	safeController = ros::service::createClient<Controller>(safeControllerName, true);
    ROS_INFO_STREAM("loaded safe controller: " << safeController.getService());
211
212
213
}

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

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

bucyril's avatar
bucyril committed
222
	customController = ros::service::createClient<Controller>(customControllerName, true);
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
    ROS_INFO_STREAM("loaded custom controller " << customControllerName);
}

void commandCallback(const std_msgs::Int32& commandMsg) {
	int cmd = commandMsg.data;
	switch(cmd) {
    	case CMD_USE_SAFE_CONTROLLER:
    		loadSafeController();
    		usingSafeController = true;
    		break;

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

239
240
241
242
243
244
245
246
    	case CMD_USE_CRAZYFLY_ENABLE:
    		crazyflieEnabled = true;
    		break;

    	case CMD_USE_CRAZYFLY_DISABLE:
    		crazyflieEnabled = false;
    		break;

247
248
249
    	default:
    		ROS_ERROR_STREAM("unexpected command number: " << cmd);
    		break;
250
	}
251
252
253
254
255
256
}

int main(int argc, char* argv[]){
	ros::init(argc, argv, "PPSClient");
	ros::NodeHandle nodeHandle("~");
	loadParameters(nodeHandle);
257
	
258
259
	
	//ros::service::waitForService("/CentralManagerService/CentralManager");
260
	centralManager = nodeHandle.serviceClient<CMQuery>("/CentralManagerService/Query", false);
261
262
	loadCrazyflieContext();
	
263
264
	//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
265
	ROS_INFO_STREAM("successfully subscribed to ViconData");
muelmarc's avatar
muelmarc committed
266
	
267
	//ros::Publishers to advertise the control output
bucyril's avatar
bucyril committed
268
	controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
phfriedl's avatar
phfriedl committed
269

270
271
	//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
272
    ros::Subscriber commandSubscriber = nodeHandle.subscribe("Command", 1, commandCallback);
phfriedl's avatar
phfriedl committed
273

274
	//start with safe controller
275
	crazyflieEnabled = true;
276
277
	usingSafeController = true;
	loadSafeController();
278
	
muelmarc's avatar
muelmarc committed
279
280
281
	std::string package_path;
	package_path = ros::package::getPath("d_fall_pps") + "/";
	ROS_INFO_STREAM(package_path);
282
	std::string record_file = package_path + "LoggingPPSClient.bag";
muelmarc's avatar
muelmarc committed
283
284
	bag.open(record_file, rosbag::bagmode::Write);

285
    ros::spin();
muelmarc's avatar
muelmarc committed
286
	bag.close();
287
    return 0;
288
}