diff --git a/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch new file mode 100644 index 0000000000000000000000000000000000000000..078b17e45a979d5a8b488b18304e8eb5718b06cc --- /dev/null +++ b/pps_ws/src/d_fall_pps/launch/ppsLaunch.launch @@ -0,0 +1,26 @@ +<launch> + <!-- Might be needed when our CrazyRadio works. There is no CrazyRadio.launch file yet! + <include file="$(find d_fall_pps)/launch/CrazyRadio.launch" /> + --> + + <node pkg="d_fall_pps" name="ViconDataPublisher" output="screen" type="ViconDataPublisher"> + </node> + + <node pkg="d_fall_pps" name="PPSClient" output="screen" type="PPSClient"> + <rosparam command="load" file="$(find d_fall_pps)/launch/studentParams.yaml" /> + </node> + + + + <!-- We will need something similar for our Control node(s) + <node pkg="d_fall_pps" name="SafeControllerService" output="screen" type="FlightControl"> + </node> + --> + + <!-- When we have a GUI, this has to be adapted and included + <node pkg="d_fall_pps" name="GUI" output="screen" type="GUI"> + <rosparam command="load" file="$(find d_fall_pps)/launch/ControlParams.yaml" /> + </node> + --> + +</launch> \ No newline at end of file diff --git a/pps_ws/src/d_fall_pps/launch/studentParams.yaml b/pps_ws/src/d_fall_pps/launch/studentParams.yaml new file mode 100644 index 0000000000000000000000000000000000000000..8a9c80453c6842622d631c90751aafff50718088 --- /dev/null +++ b/pps_ws/src/d_fall_pps/launch/studentParams.yaml @@ -0,0 +1,5 @@ +TeamName: 'Two' +CrazyFlieName: "cfTwo" + +#controllertypes to add with adjustable +#motor, angle and rate \ No newline at end of file diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp index bca760e2817eb35bf6e3ad57249cd925cd50f012..f2cc115cea6d56ec0dcba19fc144bc3bbef33ef3 100644 --- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp +++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp @@ -17,10 +17,47 @@ #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 cflie; + + + +//debugging +int callbackCalls = 0; + //is called upon every new arrival of data in main void viconCallback(const d_fall_pps::ViconData& data){ - ROS_INFO("Callback called"); - ROS_INFO_STREAM(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 + if(data.crazyflieName == cflie){ + 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; + //ROS_INFO("data to share with right controller:"); + //ROS_INFO_STREAM(myDataToPublish); + + + //TODO: + //Some way of choosing the correct controller: Safe or Custom + } + else { + ROS_INFO("ViconData from other crazyflie received"); + } + } @@ -30,8 +67,16 @@ int main(int argc, char* argv[]){ ros::init(argc, argv, "PPSClient"); ros::NodeHandle nodeHandle("~"); + //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"); + } + ROS_INFO_STREAM("about to subscribe"); - //maybe set second argument to 1 (as we only want the most recent data) ros::Subscriber ViconSubscriber = nodeHandle.subscribe("/ViconDataPublisher/ViconData", 1, viconCallback); ROS_INFO_STREAM("subscribed");