Skip to content
Snippets Groups Projects
Commit da168dca authored by phfriedl's avatar phfriedl
Browse files

PPSClient is able to receive data from ViconDataPublisher

parent c1cd8c44
No related branches found
No related tags found
No related merge requests found
......@@ -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;
}
......@@ -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();
......
0% Loading or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment