From 2c6aeca0e463ec82827ddccd1c7631dcf237a179 Mon Sep 17 00:00:00 2001 From: phfriedl <phfriedl@student.ethz.ch> Date: Thu, 6 Apr 2017 11:40:07 +0200 Subject: [PATCH] small changes to PPSClient --- .../src/d_fall_pps/launch/studentParams.yaml | 2 +- .../d_fall_pps/src/CentralManagerService.cpp | 14 ++++++------- pps_ws/src/d_fall_pps/src/PPSClient.cpp | 21 ++++--------------- 3 files changed, 11 insertions(+), 26 deletions(-) diff --git a/pps_ws/src/d_fall_pps/launch/studentParams.yaml b/pps_ws/src/d_fall_pps/launch/studentParams.yaml index a8384c2c..9b635ca0 100644 --- a/pps_ws/src/d_fall_pps/launch/studentParams.yaml +++ b/pps_ws/src/d_fall_pps/launch/studentParams.yaml @@ -1,6 +1,6 @@ TeamName: 'Two' CrazyFlieName: "cfTwo" -CrazyFlieAddress: "radio://0/99/2M" +CrazyFlieAddress: "radio://0/72/2M" #controllertypes to add with adjustable #motor, angle and rate diff --git a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp b/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp index 8a767b68..2cb409de 100644 --- a/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp +++ b/pps_ws/src/d_fall_pps/src/CentralManagerService.cpp @@ -21,19 +21,17 @@ using namespace d_fall_pps; -/*TODO: -request from paul -Specificy controller in a file +//receive request from students (containing (maybe among other things) their name) +//send them back the CrazyflieContext (containing (maybe among other things) the area to fly in) +bool returnCrazyflieContext(CentralManager::Request &request, CentralManager::Response &response) { + ROS_INFO("central manager"); + return true; +} -*/ -bool returnCrazyflieContext(CentralManager::Request &request, CentralManager::Response &response) { - ROS_INFO("cnetal manager"); - return true; -} int main(int argc, char* argv[]) { ros::init(argc, argv, "CentralManagerService"); diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp index 7f24b704..4c7224a3 100644 --- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp +++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp @@ -51,19 +51,6 @@ ros::Publisher motorCommandPublisher; //not void: sould give back controlldata void ppsClientToController(ViconData data){ if(data.crazyflieName == cflie){ - /* unnecessairy: just send data!!!!! - 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; - */ - - //TODO: //Some way of choosing the correct controller: Safe or Custom //using the area data @@ -106,7 +93,7 @@ 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 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); @@ -192,9 +179,9 @@ int main(int argc, char* argv[]){ //ros::Publishers to advertise on the three command type topics - 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); + angleCommandPublisher = nodeHandle.advertise <d_fall_pps::AngleCommand>("AngleCommand", 1); + rateCommandPublisher = nodeHandle.advertise<d_fall_pps::RateCommand>("RateCommand", 1); + motorCommandPublisher = nodeHandle.advertise <d_fall_pps::MotorCommand>("MotorCommand", 1); //service: now only one available: to add several services depending on controller -- GitLab