SafeControllerService.cpp 1.87 KB
Newer Older
bucyril's avatar
bucyril committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
//    Alternate controller that is expected to work.
//    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"
18
19
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
20
21
#include "d_fall_pps/RateCommand.h"
#include "d_fall_pps/RateController.h"
22

23
using namespace d_fall_pps;
phfriedl's avatar
phfriedl committed
24

25
26
bool calculateControlOutput(RateController::Request &request, RateController::Response &response) {
    ROS_INFO("calculate control output");
phfriedl's avatar
phfriedl committed
27
    
28
29
30
31
32
33
34
35
36
37
38
39
40
    ViconData vicon = request.crazyflieLocation;
    Setpoint goal = request.setpoint;
    ROS_INFO("request received with following ViconData");
    ROS_INFO_STREAM(vicon);
    ROS_INFO("the goal setpoint is:");
    ROS_INFO_STREAM(request.setpoint);

    //add/calculate safeController
    response.controlOutput.rollRate = 1; //testvalue
	response.controlOutput.pitchRate = 2; //testvalue
	response.controlOutput.yawRate = 3; //testvalue

	return true;
41
}
bucyril's avatar
bucyril committed
42
43

int main(int argc, char* argv[]) {
44
45
46
47
    ros::init(argc, argv, "SafeControllerService");

    ros::NodeHandle nodeHandle("~");

48
    ros::ServiceServer service = nodeHandle.advertiseService("RateController", calculateControlOutput);
49
50
51
52
    ROS_INFO("SafeControllerService ready");
    ros::spin();

    return 0;
bucyril's avatar
bucyril committed
53
}