PPSClient.cpp 7.46 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
20
#include <rosbag/bag.h>
#include <std_msgs/String.h>
21

22
#include "d_fall_pps/Controller.h"
23
24
25
#include "d_fall_pps/CentralManager.h"

#include "d_fall_pps/ViconData.h"
26
#include "d_fall_pps/ControlCommand.h"
muelmarc's avatar
muelmarc committed
27
#include "d_fall_pps/CrazyflieContext.h"
28
#include "std_msgs/Int32.h"
muelmarc's avatar
muelmarc committed
29

30
#include "d_fall_pps/ControlCommand.h"
31

32
33
#define CMD_USE_SAFE_CONTROLLER 1
#define CMD_USE_CUSTOM_CONTROLLER 2
34
35
#define CMD_USE_CRAZYFLY_ENABLE 3
#define CMD_USE_CRAZYFLY_DISABLE 4
36

37
using namespace d_fall_pps;
38

39
40
41
42
//name of the student team
std::string teamName;
//name of the crazyflie, as specified in Vicon
std::string crazyflieName;
43

44
//the safe controller specified in the ClientConfig.yaml, is considered trusted
45
ros::ServiceClient safeController;
46
47
//the custom controller specified in the ClientConfig.yaml, is considered untrusted
ros::ServiceClient customController;
48

49
ros::ServiceClient centralManager;
50
ros::Publisher controlCommandPublisher;
51

52
53
rosbag::Bag bag;

54
55
56
57
//describes the area of the crazyflie and other parameters
CrazyflieContext context;
//wheter to use safe of custom controller
bool usingSafeController;
58
59
//wheter crazyflie is enabled (ready to fly) or disabled (motors off)
bool crazyflieEnabled;
60

61
62
int safetyDelay;

63
64
//checks if crazyflie is within allowed area and if custom controller returns no data
bool safetyCheck(ViconData data, ControlCommand controlCommand) {
65
66
	
	//position check
67
	if((data.x < context.localArea.xmin) or (data.x > context.localArea.xmax)) {
68
69
		safetyDelay--;
		return false;
muelmarc's avatar
muelmarc committed
70
	}
71
	if((data.y < context.localArea.ymin) or (data.y > context.localArea.ymax)) {
72
73
		safetyDelay--;
		return false;
muelmarc's avatar
muelmarc committed
74
	}
75
	if((data.z < context.localArea.zmin) or (data.z > context.localArea.zmax)) {
76
77
		safetyDelay--;
		return false;
muelmarc's avatar
muelmarc committed
78
	}
79

80
81
	
	return true;
82
}
phfriedl's avatar
phfriedl committed
83

84
85
//is called when new data from Vicon arrives
void viconCallback(const ViconData& data) {
86
	if(data.crazyflieName == crazyflieName) {
87
88
		Controller controllerCall;
		controllerCall.request.crazyflieLocation = data;
89
90
		

91
92
93
94
95
96
97
		if(crazyflieEnabled){
			if(!usingSafeController) {
				bool success = customController.call(controllerCall);
				if(!success) {
					ROS_ERROR("Failed to call custom controller, switching to safe controller");
					usingSafeController = true;
				}
98

99

100
			usingSafeController = true; //debug
101
			}
102

103
		
104
105
106
107
108
			if(usingSafeController) {
				bool success = safeController.call(controllerCall);
				if(!success) {
					ROS_ERROR("Failed to call safe controller");
				}
109
			}
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
		
		if(!safetyCheck(data, controllerCall.response.controlOutput)){
			ROS_INFO_STREAM("AutocontrolOn >>>>>> SWITCHED OFF");
			if(safetyDelay == 0){
				ROS_INFO_STREAM("ROS Shutdown");
				//bag.close();
				ros::shutdown();
			}
			ControlCommand switchOffControls;
			switchOffControls.roll = 0;
			switchOffControls.pitch = 0;
			switchOffControls.yaw = 0;
			switchOffControls.motorCmd1 = 0;
			switchOffControls.motorCmd2 = 0;
			switchOffControls.motorCmd3 = 0;
			switchOffControls.motorCmd4 = 0;
			switchOffControls.onboardControllerType = 0;
			
			controllerCall.response.controlOutput = switchOffControls;
		}
		else{
			safetyDelay=20;
		}
136
137

		controlCommandPublisher.publish(controllerCall.response.controlOutput);
138
139
140
141
142
143
144
145
146
		
		std_msgs::String str;
		str.data = std::string("foo");
		
		std_msgs::Int32 i;
		i.data = 42;

		bag.write("testfoo: ", ros::Time::now(), str);
		bag.write("test42: ", ros::Time::now(), i);
147
		*/
148

149
		controlCommandPublisher.publish(controllerCall.response.controlOutput);
150
151
152
153
154
		} else{ //crazyflie disabled
			ControlCommand zeroOutput = ControlCommand(); //everything set to zero
			zeroOutput.onboardControllerType = 2; //set to motor_mode
			controlCommandPublisher.publish(zeroOutput);
		}
155
156
157
158
159
160
	}
}

void loadParameters(ros::NodeHandle& nodeHandle) {
	if(!nodeHandle.getParam("teamName", teamName)) {
		ROS_ERROR("Failed to get teamName");
161
	}
162
163
164

	if(!nodeHandle.getParam("crazyFlieName", crazyflieName)) {
		ROS_ERROR("Failed to get crazyFlieName");
165
	}
166
167
}

168
169
170
171
172
173
174
175
176
void loadCrazyflieContext() {
	CentralManager centralManagerCall;
	if(centralManager.call(centralManagerCall)) {
		context = centralManagerCall.response.context;
		ROS_INFO("CrazyflieContext obtained");
	} else {
		ROS_ERROR("Failed to call CentralManagerService");
	}
}
177

178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
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 = nodeHandle.serviceClient<Controller>(safeControllerName, true);
    ROS_INFO_STREAM("loaded safe controller " << safeControllerName);
}

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

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

201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
	customController = nodeHandle.serviceClient<Controller>(customControllerName, true);
    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;

218
219
220
221
222
223
224
225
    	case CMD_USE_CRAZYFLY_ENABLE:
    		crazyflieEnabled = true;
    		break;

    	case CMD_USE_CRAZYFLY_DISABLE:
    		crazyflieEnabled = false;
    		break;

226
227
228
    	default:
    		ROS_ERROR_STREAM("unexpected command number: " << cmd);
    		break;
229
	}
230
231
232
233
234
235
}

int main(int argc, char* argv[]){
	ros::init(argc, argv, "PPSClient");
	ros::NodeHandle nodeHandle("~");
	loadParameters(nodeHandle);
236
	
237
238
239
240
241
	
	//ros::service::waitForService("/CentralManagerService/CentralManager");
	centralManager = nodeHandle.serviceClient<CentralManager>("/CentralManagerService/CentralManager");
	loadCrazyflieContext();
	
242
243
	//keeps 100 messages because otherwise ViconDataPublisher would override the data immediately
	ros::Subscriber viconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 100, viconCallback);
244
	ROS_INFO_STREAM("successfully subscribed to ViconData");
muelmarc's avatar
muelmarc committed
245
	
246
	//ros::Publishers to advertise the control output
bucyril's avatar
bucyril committed
247
	controlCommandPublisher = nodeHandle.advertise <ControlCommand>("ControlCommand", 1);
phfriedl's avatar
phfriedl committed
248

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

253
	//start with safe controller
254
	crazyflieEnabled = true;
255
256
	usingSafeController = true;
	loadSafeController();
phfriedl's avatar
phfriedl committed
257

258
259
	
	bag.open("testbag.bag", rosbag::bagmode::Write);
phfriedl's avatar
phfriedl committed
260

261
262
    ros::spin();
    return 0;
263
}