diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp index f2cc115cea6d56ec0dcba19fc144bc3bbef33ef3..0bbdc9e641788fa3120778f588b97f6103aceb64 100644 --- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp +++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp @@ -14,30 +14,30 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see <http://www.gnu.org/licenses/>. + +//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 + + #include "ros/ros.h" #include "d_fall_pps/ViconData.h" //the teamname and the assigned crazyflie, will be extracted from studentParams.yaml -std::string team; //is this needed? +std::string team; //is this needed here? maybe for room asignment received from CentralManager? std::string cflie; +//global sevices +ros::ServiceClient rateClient; - -//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 +//extract data from "data" and publish/add to service for controller +//not void: sould give back controlldata +void PPSClientToController(data){ if(data.crazyflieName == cflie){ + /* unnecessairy: just send data!!!!! d_fall_pps::ViconData myDataToPublish; myDataToPublish.crazyflieName = data.crazyflieName; myDataToPublish.x = data.x; @@ -47,18 +47,42 @@ void viconCallback(const d_fall_pps::ViconData& data){ myDataToPublish.pitch = data.pitch; myDataToPublish.yaw = data.yaw; myDataToPublish.acquiringTime = data.acquiringTime; - //ROS_INFO("data to share with right controller:"); - //ROS_INFO_STREAM(myDataToPublish); + */ //TODO: //Some way of choosing the correct controller: Safe or Custom + //using the area data + + //TODO: + //communicating with Controller + + + //TODO: + //return control commands } else { ROS_INFO("ViconData from other crazyflie received"); } +} +//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); + } int main(int argc, char* argv[]){ @@ -80,6 +104,18 @@ int main(int argc, char* argv[]){ ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback); ROS_INFO_STREAM("subscribed"); + + + + + //service: now only one available: to add several services depending on controller + rateClient = nodeHandle.serviceClient<d_fall_pps::RateController>("/SafeControllerService/RateCommand"); + + + + + + ros::spin(); return 0; } diff --git a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp index 20b2bac73b53640c240046848089160b95b41bd5..06ffb7a9d781bab9be0589232d5f7a1584e58d99 100644 --- a/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp +++ b/pps_ws/src/d_fall_pps/src/SafeControllerService.cpp @@ -22,6 +22,9 @@ bool calculateControlOutput(d_fall_pps::RateController::Request &request, d_fall_pps::RateController::Response &response) { ROS_INFO("calculate control output"); + //add safeController + + } int main(int argc, char* argv[]) { diff --git a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp index a3a59b0bfd3b606106663903fd50a93de732ffbf..32ade4735a2ba03657798e958e9d438415222256 100644 --- a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp +++ b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp @@ -14,6 +14,18 @@ // You should have received a copy of the GNU General Public License // along with this program. If not, see <http://www.gnu.org/licenses/>. + + + +//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 + + + #include <string.h> #include "DataStreamClient.h" #include "ros/ros.h"