PPSClient.cpp 3.62 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

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

phfriedl's avatar
phfriedl committed
33
34
//global sevices
ros::ServiceClient rateClient;
35

phfriedl's avatar
phfriedl committed
36
37
38
//extract data from "data" and publish/add to service for controller
//not void: sould give back controlldata
void PPSClientToController(data){
39
	if(data.crazyflieName == cflie){
phfriedl's avatar
phfriedl committed
40
		/* unnecessairy: just send data!!!!!
41
42
43
44
45
46
47
48
49
		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
50
		*/
51
52
53
54


		//TODO:
		//Some way of choosing the correct controller: Safe or Custom
phfriedl's avatar
phfriedl committed
55
56
57
58
59
60
61
62
		//using the area data

		//TODO:
		//communicating with Controller


		//TODO:
		//return control commands
63
64
65
66
	}
	else {
		ROS_INFO("ViconData from other crazyflie received");
	}
phfriedl's avatar
phfriedl committed
67
}
68

69

phfriedl's avatar
phfriedl committed
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
//debugging
int callbackCalls = 0;

//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
	//debugging
	++callbackCalls;
	//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);

	//extract data from "data" and publish/add to service for controller
	PPSClientToController(data);

86
87
88
89
90
91
92
93
}

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

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

94
95
96
97
98
99
100
101
102
	//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");
	}
	
103
104
105
106
	ROS_INFO_STREAM("about to subscribe");
	ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
	ROS_INFO_STREAM("subscribed");

phfriedl's avatar
phfriedl committed
107
108
109
110
111
112
113
114
115
116
117
118




	//service: now only one available: to add several services depending on controller
	rateClient = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateCommand");






119
120
    ros::spin();
    return 0;
121
}