Commit 064ad51c authored by phfriedl's avatar phfriedl
Browse files

Service with controller implemented (using testvalues; no controlcommands calculated yet

parent f589ec4a
......@@ -25,6 +25,9 @@
#include "ros/ros.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/RateController.h"
using namespace d_fall_pps;
//the teamname and the assigned crazyflie, will be extracted from studentParams.yaml
std::string team; //is this needed here? maybe for room asignment received from CentralManager?
......@@ -35,7 +38,7 @@ ros::ServiceClient rateClient;
//extract data from "data" and publish/add to service for controller
//not void: sould give back controlldata
void PPSClientToController(data){
void ppsClientToController(ViconData data){
if(data.crazyflieName == cflie){
/* unnecessairy: just send data!!!!!
d_fall_pps::ViconData myDataToPublish;
......@@ -56,10 +59,27 @@ void PPSClientToController(data){
//TODO:
//communicating with Controller
RateController srvRate;
Setpoint goalLocation;
goalLocation.x = 9; //testvalue
goalLocation.y = 8; //testvalue
goalLocation.z = 7; //testvalue
srvRate.request.crazyflieLocation = data;
srvRate.request.setpoint = goalLocation;
//TODO:
//return control commands
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
}
}
else {
ROS_INFO("ViconData from other crazyflie received");
......@@ -68,12 +88,12 @@ void PPSClientToController(data){
//debugging
int callbackCalls = 0;
//int callbackCalls = 0;
//is called upon every new arrival of data in main
void viconCallback(const d_fall_pps::ViconData& data){
//debugging
++callbackCalls;
//++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);
......@@ -81,7 +101,7 @@ void viconCallback(const d_fall_pps::ViconData& data){
//ROS_INFO("My crazyflie is:"); ROS_INFO_STREAM(cflie);
//extract data from "data" and publish/add to service for controller
PPSClientToController(data);
ppsClientToController(data);
}
......@@ -90,6 +110,7 @@ void viconCallback(const d_fall_pps::ViconData& data){
//Send to Crayzradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
/*
void CControlMgr::callbackRunRateController(const ros::TimerEvent&)
{
if(m_pRateController!=NULL)
......@@ -144,7 +165,7 @@ void CControlMgr::SendToCrazyflie(ControllerOutput package)
}
*/
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
int main(int argc, char* argv[]){
......@@ -166,7 +187,7 @@ int main(int argc, char* argv[]){
ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback);
ROS_INFO_STREAM("subscribed");
/*
//publish package_for_crazyradio>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>
//m_pNodeHandle=nodeHandle;
//m_pCallbackQueueControlMgr=new ros::CallbackQueue();
......@@ -179,7 +200,7 @@ int main(int argc, char* argv[]){
ros::Pubslisher AngleCommandsPublisher = nodeHandle.advertise <d_fall_pps::MotorCommandsPackage>("MotorCommands", 10));
//<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
*/
......
......@@ -15,16 +15,29 @@
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "ros/ros.h"
#include "d_fall_pps/RateController.h"
#include "d_fall_pps/ViconData.h"
#include "d_fall_pps/Setpoint.h"
#include "d_fall_pps/MotorControl.h"
#include "d_fall_pps/RateCommand.h"
#include "d_fall_pps/RateController.h"
bool calculateControlOutput(d_fall_pps::RateController::Request &request, d_fall_pps::RateController::Response &response) {
ROS_INFO("calculate control output");
//add safeController
using namespace d_fall_pps;
bool calculateControlOutput(RateController::Request &request, RateController::Response &response) {
ROS_INFO("calculate control output");
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;
}
int main(int argc, char* argv[]) {
......
......@@ -32,6 +32,7 @@
#include "d_fall_pps/ViconData.h"
using namespace ViconDataStreamSDK::CPP;
using namespace d_fall_pps;
int main(int argc, char* argv[]) {
ros::init(argc, argv, "ViconDataPublisher");
......@@ -40,11 +41,11 @@ int main(int argc, char* argv[]) {
ros::Time::init();
ros::Publisher viconDataPublisher =
nodeHandle.advertise<d_fall_pps::ViconData>("ViconData", 1);
nodeHandle.advertise<ViconData>("ViconData", 1);
//publish something random if no viconData is available for testing
/*
d_fall_pps::ViconData viconData;
ViconData viconData;
double i = 1;
while(true)
{
......@@ -143,7 +144,7 @@ int main(int argc, char* argv[]) {
}
//build and publish message
d_fall_pps::ViconData viconData;
ViconData viconData;
viconData.crazyflieName = subjectName;
viconData.x = outputTranslation.Translation[0];
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment