PPSClient.cpp 6.45 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
//    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/>.

phfriedl's avatar
phfriedl committed
17
18
19

//TODO:
//CentralManager: extract data about room from vicon data
20
//CentralManager: assign localArea for each group and those coordinates to PPSClients
phfriedl's avatar
phfriedl committed
21
//ViconDataPublisher: extract data about room from vicon data in and send also to PPSClient
22
23
//PPSClient: Compare data received from CentralManager and ViconDataPublisher and determine in which localArea you are
//PPSClient: Choose correct controller accoring to current localArea
phfriedl's avatar
phfriedl committed
24
25


26
#include "ros/ros.h"
muelmarc's avatar
muelmarc committed
27
#include <stdlib.h>
28
29

//include autogenerated headers from srv files
30
#include "d_fall_pps/RateController.h"
31
32
33
34
35
36
#include "d_fall_pps/AngleController.h"
#include "d_fall_pps/MotorController.h"
#include "d_fall_pps/CentralManager.h"

//include autogenerated headers from msg files
#include "d_fall_pps/ViconData.h"
37
38
39
#include "d_fall_pps/AngleCommand.h"
#include "d_fall_pps/RateCommand.h"
#include "d_fall_pps/MotorCommand.h"
muelmarc's avatar
muelmarc committed
40
41
#include "d_fall_pps/CrazyflieContext.h"

42
43

using namespace d_fall_pps;
44

45
//the teamname and the assigned crazyflie, will be extracted from studentParams.yaml
phfriedl's avatar
phfriedl committed
46
std::string team; //is this needed here? maybe for room asignment received from CentralManager?
47
48
std::string cflie;

phfriedl's avatar
phfriedl committed
49
//global sevices
50
ros::ServiceClient safeController;
51
ros::ServiceClient centralClient;
52

bucyril's avatar
bucyril committed
53
54
55
ros::Publisher angleCommandPublisher;
ros::Publisher rateCommandPublisher;
ros::Publisher motorCommandPublisher;
56

57
AreaBounds localArea;
58
59

void ppsClientToController(ViconData data, bool autocontrolOn){
60
	if(data.crazyflieName == cflie){
61
		//call safecontroller if autocontrol is true
62

63
		if(autocontrolOn){
64
65
			return;
			//call safecontroller here
66
67
		}
		else {
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
			//student controller is called here
				//for the moment use safecontroller for TESTING
			
			RateController srvRate;
			Setpoint goalLocation;

			goalLocation.x = -5; //testvalue
			goalLocation.y = 250; //testvalue
			goalLocation.z = 300; //testvalue

			srvRate.request.crazyflieLocation = data;
			srvRate.request.setpoint = goalLocation;

			//TODO:
			//return control commands
			if(safeController.call(srvRate)){
				ROS_INFO("Received control input");
				ROS_INFO_STREAM(srvRate.response.controlOutput);
				
87
				rateCommandPublisher.publish(srvRate.response.controlOutput);
88
89
90
  				//onboardControllerType = ??????????????????????
				
				
91
			} else {
92
93
94
				ROS_ERROR("Failed to call SafeControllerService");
				//return 1; //return some useful stuff
			}
95
		}
phfriedl's avatar
phfriedl committed
96

97
		
98
99
100
101
	}
	else {
		ROS_INFO("ViconData from other crazyflie received");
	}
phfriedl's avatar
phfriedl committed
102
}
103

104
105
106
107
108
109
110
//acceptance test for crazyflie position and attitude 
bool safetyCheck(ViconData data){
	CrazyflieContext CrazyflieContext;
	//ROS_INFO("in safetyCheck");
	//ROS_INFO_STREAM("ViconData: " << data.x << ", " << data.y << ", " << data.z);
	
	//position check
111
	if((data.x < localArea.xmin) or (data.x > localArea.xmax)){
muelmarc's avatar
muelmarc committed
112
113
		return true;
	}
114
	if((data.y < localArea.ymin) or (data.y > localArea.ymax)){
muelmarc's avatar
muelmarc committed
115
116
		return true;
	}
117
	if((data.z < localArea.zmin) or (data.z > localArea.zmax)){
muelmarc's avatar
muelmarc committed
118
119
		return true;
	}
120
121
122
123
124
125
	
	//attitude check
	
	//all checks passed
	return false;
}
phfriedl's avatar
phfriedl committed
126
127

//is called upon every new arrival of data in main
bucyril's avatar
bucyril committed
128
void viconCallback(const ViconData& data){
129
	ROS_INFO("in viconCallback PPSClient");
130
	//ROS_INFO_STREAM(data);
phfriedl's avatar
phfriedl committed
131
132
133
	//ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
	//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);

134
135
136
	if(data.crazyflieName == cflie){	
		//forward data to safety check
		bool autocontrolOn = safetyCheck(data);
137
		//ROS_INFO_STREAM("autocontrolOn: " << autocontrolOn);
138
139
140
141
142
		ppsClientToController(data, autocontrolOn);
	}
	else {
		ROS_INFO("ViconData from other crazyflie received");
	}
muelmarc's avatar
muelmarc committed
143

phfriedl's avatar
phfriedl committed
144

145
146
147
148
149
150
151
152
}

int main(int argc, char* argv[]){
	ROS_INFO_STREAM("PPSClient started");

	ros::init(argc, argv, "PPSClient");
	ros::NodeHandle nodeHandle("~");

153
154
155
156
157
158
159
160
161
	//get the params defined in studentParams.yaml
	if(!nodeHandle.getParam("TeamName",team)){
		ROS_ERROR("Failed to get TeamName");
	}

	if(!nodeHandle.getParam("CrazyFlieName",cflie)){
		ROS_ERROR("Failed to get CrazyFlieName");
	}
	
162
	ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
163
	ROS_INFO_STREAM("successfully subscribed to ViconData");
muelmarc's avatar
muelmarc committed
164
	
165
	//ros::Publishers to advertise on the three command type topics
bucyril's avatar
bucyril committed
166
167
168
	angleCommandPublisher = nodeHandle.advertise <AngleCommand>("AngleCommand", 1);
	rateCommandPublisher = nodeHandle.advertise<RateCommand>("RateCommand", 1);
	motorCommandPublisher = nodeHandle.advertise <MotorCommand>("MotorCommand", 1);
phfriedl's avatar
phfriedl committed
169
170


171
172
	//service 
		//to be expanded with additional services depending on controller (currently only one available)
173
	ros::service::waitForService("/SafeControllerService/RateController");
174
	safeController = nodeHandle.serviceClient<RateController>("/SafeControllerService/RateController", true);
175
	
176
	//safeController = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController", true);
177
178
179
180
	//http://wiki.ros.org/roscpp/Overview/Services 
	//2.1 Persistenct Connection: ROS also allows for persistent connections to services. With a persistent connection, a client stays connected to a service. 
	// Otherwise, a client normally does a lookup and reconnects to a service each time.

phfriedl's avatar
phfriedl committed
181

182
	//service 
bucyril's avatar
bucyril committed
183
	centralClient = nodeHandle.serviceClient<CentralManager>("/CentralManagerService/CentralManager");
184
185
186
187
188
189
190
	
	
	//TBD: some sort of init procedure to get data from CentralManager upfront
	//this is only for testing>>>>>>>>>>>>>>>>>>>>>>>>>>>>
	CentralManager ManagerSettings;
	if(centralClient.call(ManagerSettings)){

191
		localArea = ManagerSettings.response.context.localArea;
192
		ROS_INFO("CentralManager responded");
193
		ROS_INFO("localAreaBoundaries Set");
194
		
phfriedl's avatar
phfriedl committed
195

196
197
198
199
200
201
202
	}
	else{
		ROS_ERROR("Failed to call CentralManagerService. Callback is aborted");
		//return some useful stuff
		return 0;
	}
	//<<<<<<<<<<<<<<<<<<<<<<<this is only for testing
phfriedl's avatar
phfriedl committed
203

204
205
    ros::spin();
    return 0;
206
}