PPSClient.cpp 7.2 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
30
#include "d_fall_pps/RateController.h"

using namespace d_fall_pps;
31

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

phfriedl's avatar
phfriedl committed
36
37
//global sevices
ros::ServiceClient rateClient;
38

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


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

		//TODO:
		//communicating with Controller
62
63
64
65
66
67
		RateController srvRate;
		Setpoint goalLocation;

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

69
70
		srvRate.request.crazyflieLocation = data;
		srvRate.request.setpoint = goalLocation;
phfriedl's avatar
phfriedl committed
71
72
73

		//TODO:
		//return control commands
74
75
76
77
78
79
80
81
82
		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
		}
83
84
85
86
	}
	else {
		ROS_INFO("ViconData from other crazyflie received");
	}
phfriedl's avatar
phfriedl committed
87
}
88

89

phfriedl's avatar
phfriedl committed
90
//debugging
91
//int callbackCalls = 0;
phfriedl's avatar
phfriedl committed
92
93
94
95

//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
	//debugging
96
	//++callbackCalls;
phfriedl's avatar
phfriedl committed
97
98
99
100
101
102
103
	//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
104
	ppsClientToController(data);
phfriedl's avatar
phfriedl committed
105

106
107
}

muelmarc's avatar
muelmarc committed
108
109
110
111
112




//Send to Crayzradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
113
/*
muelmarc's avatar
muelmarc committed
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
void CControlMgr::callbackRunRateController(const ros::TimerEvent&)
{
    if(m_pRateController!=NULL)
        if(m_pRateController->fIsInit())
        {
            DistributePowerAndSendToCrazyflie(m_pRateController->computeOutput());
        }
}

void CControlMgr::DistributePowerAndSendToCrazyflie(ControllerOutput rateControllerOutput)
{
    assert(rateControllerOutput.onboardControllerType=eOnboardMotorCmdController);
    assert(m_isRateOffboard==true);


    rateControllerOutput.motorCmd1=m_CrazyControllerOutput.motorCmd1-rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
    rateControllerOutput.motorCmd2=m_CrazyControllerOutput.motorCmd2-rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;
    rateControllerOutput.motorCmd3=m_CrazyControllerOutput.motorCmd3+rateControllerOutput.roll/2.0+rateControllerOutput.pitch/2.0-rateControllerOutput.yaw;
    rateControllerOutput.motorCmd4=m_CrazyControllerOutput.motorCmd4+rateControllerOutput.roll/2.0-rateControllerOutput.pitch/2.0+rateControllerOutput.yaw;

    SendToCrazyflie(rateControllerOutput);
}

void CControlMgr::SendToCrazyflie(ControllerOutput package)
{
    if(m_isStopped)
    {
        m_packageToSend.motorCmd1=0;
        m_packageToSend.motorCmd2=0;
        m_packageToSend.motorCmd3=0;
        m_packageToSend.motorCmd4=0;
        m_packageToSend.onboardControllerType=eOnboardMotorCmdController;
        m_pPublisherControllerOutput->publish(m_packageToSend);
        return;
    }
    else
    {
        m_packageToSend.roll=package.roll*RAD2DEG;
        m_packageToSend.pitch=package.pitch*RAD2DEG;
        m_packageToSend.yaw=package.yaw*RAD2DEG;

        m_packageToSend.thrust=SaturateToUINT16(package.thrust);
        m_packageToSend.motorCmd1=SaturateToUINT16(package.motorCmd1);
        m_packageToSend.motorCmd2=SaturateToUINT16(package.motorCmd2);
        m_packageToSend.motorCmd3=SaturateToUINT16(package.motorCmd3);
        m_packageToSend.motorCmd4=SaturateToUINT16(package.motorCmd4);

        m_packageToSend.onboardControllerType=package.onboardControllerType;

        m_pPublisherControllerOutput->publish(m_packageToSend);
    }
}


168
*/
muelmarc's avatar
muelmarc committed
169
170
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<

171
172
173
174
175
176
int main(int argc, char* argv[]){
	ROS_INFO_STREAM("PPSClient started");

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

177
178
179
180
181
182
183
184
185
	//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");
	}
	
186
187
188
	ROS_INFO_STREAM("about to subscribe");
	ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
	ROS_INFO_STREAM("subscribed");
muelmarc's avatar
muelmarc committed
189
	
190
	/*
muelmarc's avatar
muelmarc committed
191
192
193
194
195
196
197
198
199
200
201
202
	//publish package_for_crazyradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
	    //m_pNodeHandle=nodeHandle;
    	//m_pCallbackQueueControlMgr=new ros::CallbackQueue();
    	//m_pNodeHandle->setCallbackQueue(m_pCallbackQueueControlMgr);


    ROS_INFO_STREAM("creating publishers for package_for_crazyradio");
	ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::AngleCommandsPackage>("AngleCommands", 10));
	ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::RateCommandsPackage>("RateCommands", 10));
	ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::MotorCommandsPackage>("MotorCommands", 10));

	//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
203
	*/
phfriedl's avatar
phfriedl committed
204
205
206
207
208
209
210
211
212
213
214
215




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






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