Go to the documentation of this file.00001
00026 #include "stdafx.h"
00027 #include <string>
00028 #include <stdio.h>
00029 #include "ros.h"
00030 #include <geometry_msgs/PoseWithCovarianceStamped.h>
00031 #include <windows.h>
00032
00033 using std::string;
00034
00035 void estimated_pose_callback (const geometry_msgs::PoseWithCovarianceStamped & pose)
00036 {
00037 printf ("Received pose %f, %f, %f\n", pose.pose.pose.position.x,
00038 pose.pose.pose.position.y, pose.pose.pose.position.z);
00039 }
00040
00041 int _tmain (int argc, _TCHAR * argv[])
00042 {
00043 ros::NodeHandle nh;
00044 char *ros_master = "1.2.3.4";
00045
00046 printf ("Connecting to server at %s\n", ros_master);
00047 nh.initNode (ros_master);
00048
00049 ros::Subscriber < geometry_msgs::PoseWithCovarianceStamped >
00050 poseSub ("estimated_pose", &estimated_pose_callback);
00051 nh.subscribe (poseSub);
00052
00053 printf ("Waiting to receive messages\n");
00054 while (1)
00055 {
00056 nh.spinOnce ();
00057 Sleep (100);
00058 }
00059
00060 printf ("All done!\n");
00061 return 0;
00062 }