Commit da168dca authored by phfriedl's avatar phfriedl
Browse files

PPSClient is able to receive data from ViconDataPublisher

parent c1cd8c44
......@@ -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();
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment