visualize_samples.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <tf/tf.h>
00003 
00004 #include <uncertain_tf/UncertainTransformListener.h>
00005 
00006 #include <geometry_msgs/PoseArray.h>
00007 
00008 
00009 int main(int argc, char** argv)
00010 {
00011 
00012     std::cout << "USAGE: visualize_samples target_frame source_frame num_samples pose_array_topic period(milliseconds)" << std::endl;
00013 
00014     std::cout << "argc" << argc << std::endl;
00015 
00016     if (argc < 6)
00017         exit(0);
00018 
00019     ros::init(argc, argv, "visualize_samples", ros::init_options::AnonymousName);
00020 
00021     ros::NodeHandle node;
00022 
00023     ros::AsyncSpinner spinner(1);
00024     spinner.start();
00025 
00026     ros::Publisher poseArrPub = node.advertise<geometry_msgs::PoseArray>(argv[4], 100);
00027 
00028     uncertain_tf::UncertainTransformListener ulistener;
00029 
00030     double period = atof(argv[5]);
00031 
00032     ros::Rate rate(1000 / period);
00033 
00034     int num_samples = atoi(argv[3]);
00035 
00036     std::cout << "num samples" << num_samples << std::endl;
00037 
00038     while (node.ok())
00039     {
00040 
00041         std::vector<StampedTransform> transform_samples;
00042 
00043         ulistener.sampleTransform(argv[1],argv[2], ros::Time(0), transform_samples, num_samples);
00044 
00045         geometry_msgs::PoseArray parr;
00046 
00047         parr.header.frame_id = argv[1];
00048         parr.header.stamp = ros::Time::now();
00049         for (std::vector<StampedTransform>::iterator it = transform_samples.begin(); it != transform_samples.end(); ++it)
00050         {
00051             geometry_msgs::Pose ps;
00052             tf::poseTFToMsg(*it,ps);
00053             parr.poses.push_back(ps);
00054         }
00055         poseArrPub.publish(parr);
00056 
00057         rate.sleep();
00058     }
00059 
00060 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


uncertain_tf
Author(s): Thomas Ruehr
autogenerated on Thu May 23 2013 17:49:36