diff --git a/pps_ws/src/d_fall_pps/src/PPSClient.cpp b/pps_ws/src/d_fall_pps/src/PPSClient.cpp index 38b3134b6555a7abfcfffffe1651ed6bdffa43c2..bca760e2817eb35bf6e3ad57249cd925cd50f012 100644 --- a/pps_ws/src/d_fall_pps/src/PPSClient.cpp +++ b/pps_ws/src/d_fall_pps/src/PPSClient.cpp @@ -15,7 +15,26 @@ // along with this program. If not, see <http://www.gnu.org/licenses/>. #include "ros/ros.h" +#include "d_fall_pps/ViconData.h" -int main(int argc, char* argv[]) { - ROS_INFO_STREAM("fdsafsfdsalj"); +//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); + +} + +int main(int argc, char* argv[]){ + ROS_INFO_STREAM("PPSClient started"); + + ros::init(argc, argv, "PPSClient"); + ros::NodeHandle nodeHandle("~"); + + 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"); + + ros::spin(); + return 0; } diff --git a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp index 6b9fcc54ea0979b032ed123d57c8d6e2f7bc0258..a3a59b0bfd3b606106663903fd50a93de732ffbf 100644 --- a/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp +++ b/pps_ws/src/d_fall_pps/src/ViconDataPublisher.cpp @@ -76,7 +76,19 @@ int main(int argc, char* argv[]) { //maybe we need a loop rate?---------e.g. 0.5 MHz ----------------------------- //ros::Rate loop_rate(500000) + + int iterations = 0; while (ros::ok()) { + //if you want to see at least some output in the terminal + //to see that you are still publishing + if(iterations % 1000 == 0){ + ROS_INFO("iteration #%d",iterations); + } + iterations++; + + + + // Get a frame while (client.GetFrame().Result != Result::Success) { // Sleep a little so that we don't lumber the CPU with a busy poll @@ -103,17 +115,11 @@ int main(int argc, char* argv[]) { double quat_y = outputRotation.Rotation[1]; double quat_z = outputRotation.Rotation[2]; double quat_w = outputRotation.Rotation[3]; - ROS_INFO("x-value and w-value:"); - ROS_INFO_STREAM(quat_x); - ROS_INFO_STREAM(quat_w); //TODO check whether this transformation is correct double roll = atan2(2 * (quat_w * quat_x + quat_y * quat_z), 1 - 2 * (quat_x * quat_x + quat_y * quat_y)); double pitch = asin(2 * (quat_w * quat_y - quat_z * quat_x)); double yaw = atan2(2 * (quat_w * quat_z + quat_x * quat_y), 1 - 2 * (quat_y * quat_y + quat_z * quat_z)); - ROS_INFO("roll:"); - ROS_INFO_STREAM(roll); - ROS_INFO("----------------------"); //calculate time until frame data was received Output_GetLatencyTotal outputLatencyTotal = client.GetLatencyTotal();