PPSClient.cpp 7.3 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
20
21
22
23
24
25

//TODO:
//CentralManager: extract data about room from vicon data
//CentralManager: assign area for each group and those coordinates to PPSClients
//ViconDataPublisher: extract data about room from vicon data in and send also to PPSClient
//PPSClient: Compare data received from CentralManager and ViconDataPublisher and determine in which area you are
//PPSClient: Choose correct controller accoring to current area


26
#include "ros/ros.h"
27
#include "d_fall_pps/ViconData.h"
28
#include "d_fall_pps/RateController.h"
29
30
31
#include "d_fall_pps/AngleCommand.h"
#include "d_fall_pps/RateCommand.h"
#include "d_fall_pps/MotorCommand.h"
muelmarc's avatar
muelmarc committed
32
33
#include "d_fall_pps/CrazyflieContext.h"

34
35

using namespace d_fall_pps;
36

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

phfriedl's avatar
phfriedl committed
41
42
//global sevices
ros::ServiceClient rateClient;
43

bucyril's avatar
bucyril committed
44
45
46
ros::Publisher angleCommandPublisher;
ros::Publisher rateCommandPublisher;
ros::Publisher motorCommandPublisher;
47
48
49
50
51

//uncommenting the next line causes FATAL Error at runtime: "You must call ros::init() before creating the first NodeHandle"
//ros::NodeHandle nodeHandle;


muelmarc's avatar
muelmarc committed
52
53
54
55
56
57
58
59
//acceptance test for crazyflie position and attitude 
bool safetyCheck(ViconData data){
	CrazyflieContext CrazyflieContext;
	
	return true;
}


phfriedl's avatar
phfriedl committed
60
61
//extract data from "data" and publish/add to service for controller
//not void: sould give back controlldata
muelmarc's avatar
muelmarc committed
62
void ppsClientToController(ViconData data, bool autocontrol){
63
	if(data.crazyflieName == cflie){
phfriedl's avatar
phfriedl committed
64
		/* unnecessairy: just send data!!!!!
65
66
67
68
69
70
71
72
73
		d_fall_pps::ViconData myDataToPublish;
		myDataToPublish.crazyflieName = data.crazyflieName;
		myDataToPublish.x = data.x;
		myDataToPublish.y = data.y;
		myDataToPublish.z = data.z;
		myDataToPublish.roll = data.roll;
		myDataToPublish.pitch = data.pitch;
		myDataToPublish.yaw = data.yaw;
		myDataToPublish.acquiringTime = data.acquiringTime;
phfriedl's avatar
phfriedl committed
74
		*/
75
76
77
78


		//TODO:
		//Some way of choosing the correct controller: Safe or Custom
phfriedl's avatar
phfriedl committed
79
80
81
82
		//using the area data

		//TODO:
		//communicating with Controller
83
84
85
86
87
88
		RateController srvRate;
		Setpoint goalLocation;

		goalLocation.x = 9; //testvalue
		goalLocation.y = 8; //testvalue
		goalLocation.z = 7; //testvalue
phfriedl's avatar
phfriedl committed
89

90
91
		srvRate.request.crazyflieLocation = data;
		srvRate.request.setpoint = goalLocation;
phfriedl's avatar
phfriedl committed
92
93
94

		//TODO:
		//return control commands
95
96
97
98
99
100
101
102
103
		if(rateClient.call(srvRate)){
			ROS_INFO("Service gave response");
			ROS_INFO("Received control input");
			ROS_INFO_STREAM(srvRate.response.controlOutput);
		}
		else{
			ROS_ERROR("Failed to call SafeControllerService");
			//return 1; //return some useful stuff
		}
104
105
106
107
	}
	else {
		ROS_INFO("ViconData from other crazyflie received");
	}
phfriedl's avatar
phfriedl committed
108
}
109

110

phfriedl's avatar
phfriedl committed
111
//debugging
112
//int callbackCalls = 0;
phfriedl's avatar
phfriedl committed
113
114
115
116

//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
	//debugging
117
	//++callbackCalls;
phfriedl's avatar
phfriedl committed
118
119
120
121
122
123
	//ROS_INFO("Callback called #%d",callbackCalls);
	//ROS_INFO("Recived Pitch in this callback: %f", data.pitch);
	//ROS_INFO("received data:"); ROS_INFO_STREAM(data);
	//ROS_INFO("My teamname is:"); ROS_INFO_STREAM(team);
	//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);

muelmarc's avatar
muelmarc committed
124
125
126
127
	//forward data to safety check
	bool autocontrolOn = safetyCheck(data);

	ppsClientToController(data, autocontrolOn);
phfriedl's avatar
phfriedl committed
128

129
130
}

muelmarc's avatar
muelmarc committed
131
132


133
134
135
//callback method to publish d_fall_pps::AngleCommand
void callbackAngleCommand(const ros::TimerEvent&)
{
bucyril's avatar
bucyril committed
136
137
138
139
140
	d_fall_pps::AngleCommand angleCommandPkg;
	angleCommandPkg.rollAngle = 1;
	angleCommandPkg.pitchAngle = 1;
	angleCommandPkg.yawAngle = 1;
	angleCommandPkg.thrust = 50;
141
	
bucyril's avatar
bucyril committed
142
143
	angleCommandPublisher.publish(angleCommandPkg);
	ROS_INFO_STREAM("AngleCommandTimer pubslishes: " << angleCommandPkg.rollAngle << ", " << angleCommandPkg.pitchAngle << ", " << angleCommandPkg.yawAngle);
144
145
146
147
148
}

//callback method to publish d_fall_pps::RateCommand
void callbackRateCommand(const ros::TimerEvent&)
{
bucyril's avatar
bucyril committed
149
150
151
152
153
	d_fall_pps::RateCommand rateCommandPkg;
	rateCommandPkg.rollRate = 2;
	rateCommandPkg.pitchRate = 2;
	rateCommandPkg.yawRate = 2;
	rateCommandPkg.thrust = 50;
154
	
bucyril's avatar
bucyril committed
155
156
	rateCommandPublisher.publish(rateCommandPkg);
	ROS_INFO_STREAM("RateCommandTimer pubslishes: " << rateCommandPkg.rollRate << ", " << rateCommandPkg.pitchRate << ", " << rateCommandPkg.yawRate);
157
158
159
160
161
}

//callback method to publish d_fall_pps::MotorCommand
void callbackMotorCommand(const ros::TimerEvent&)
{
bucyril's avatar
bucyril committed
162
163
164
165
166
	d_fall_pps::MotorCommand motorCommandPkg;
	motorCommandPkg.cmd1 = 3;
	motorCommandPkg.cmd2 = 3;
	motorCommandPkg.cmd3 = 3;
	motorCommandPkg.cmd4 = 3;
167
	
bucyril's avatar
bucyril committed
168
169
	motorCommandPublisher.publish(motorCommandPkg);
	ROS_INFO_STREAM("MotorCommandTimer pubslishes: " << motorCommandPkg.cmd1 << ", " << motorCommandPkg.cmd2 << ", " << motorCommandPkg.cmd3 << ", " << motorCommandPkg.cmd4);
170
171
172
173
174
}




175
176
177
178
179
180
int main(int argc, char* argv[]){
	ROS_INFO_STREAM("PPSClient started");

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

181
182
183
184
185
186
187
188
189
	//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");
	}
	
190
	ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
191
	ROS_INFO_STREAM("successfully subscribed to ViconData");
muelmarc's avatar
muelmarc committed
192
	
193
194
	
	//ros::Timers to call method that publishes controller outputs for crayzradio node
195
	/*
196
197
198
	Timers let you schedule a callback to happen at a specific rate through the same callback queue mechanism used by subscription, service, etc. callbacks. 
	Timers are not a realtime thread/kernel replacement, rather they are useful for things that do not have hard realtime requirements. 
	Reference: http://wiki.ros.org/roscpp/Overview/Timers
199
	*/
200
    ROS_INFO("creating publishers for package_for_crazyradio");
bucyril's avatar
bucyril committed
201
202
203
	ros::Timer angleCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackAngleCommand);
	ros::Timer rateCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackRateCommand);
	ros::Timer motorCommandTimer = nodeHandle.createTimer(ros::Duration(0.1), callbackMotorCommand);
204
205
206
	
	
	//ros::Publishers to advertise on the three command type topics
bucyril's avatar
bucyril committed
207
208
209
	angleCommandPublisher = nodeHandle.advertise <d_fall_pps::AngleCommand>("AngleCommand", 1000);
	rateCommandPublisher = nodeHandle.advertise<d_fall_pps::RateCommand>("RateCommand", 1000);
	motorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("MotorCommand", 1000);
phfriedl's avatar
phfriedl committed
210
211
212


	//service: now only one available: to add several services depending on controller
phfriedl's avatar
phfriedl committed
213
	rateClient = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateController");
phfriedl's avatar
phfriedl committed
214
215
216



217
218
    ros::spin();
    return 0;
219
}