tutorial.cpp
Go to the documentation of this file.
00001 #include <ros/ros.h>
00002 #include <tf/transform_broadcaster.h>
00003 #include <tf/tf.h>
00004 
00005 #include <uncertain_tf/UncertainTransformListener.h>
00006 #include <uncertain_tf/UncertainTransformBroadcaster.h>
00007 
00008 #include <geometry_msgs/PoseArray.h>
00009 
00010 
00011 using namespace uncertain_tf;
00012 using namespace std;
00013 
00014 void spam_tfs()
00015 {
00016     tf::TransformBroadcaster tb;
00017 
00018     ros::Rate rt(5);
00019     while (ros::ok())
00020     {
00021         tf::Transform trans;
00022         trans.setOrigin(tf::Vector3(0.7,0,0));
00023         trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), -M_PI / 5));
00024         tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/map","/base_link"));
00025         trans.setOrigin(tf::Vector3(0,0.2,0));
00026         trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), M_PI / 4));
00027         tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/base_link","/eye"));
00028         trans.setOrigin(tf::Vector3(0,0.5,0));
00029         trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), M_PI / 3));
00030         tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/eye","object"));
00031         trans.setOrigin(tf::Vector3(0.2,0,0));
00032         trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), M_PI / 2));
00033         tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/base_link","/gripper"));
00034 
00035         rt.sleep();
00036     }
00037 
00038 }
00039 
00040 // add a frame 'turntable' that rotates in z and an object 'on' this table, rotating with it
00041 void spam_turntable_tfs()
00042 {
00043     tf::TransformBroadcaster tb;
00044 
00045     double angle = 0;
00046     ros::Time start_time = ros::Time::now();
00047 
00048     ros::Rate rt(25);
00049     while (ros::ok())
00050     {
00051         angle = (ros::Time::now() - start_time).toSec() * 10 / 180 * M_PI;
00052 
00053         tf::Transform trans;
00054         trans.setOrigin(tf::Vector3(-0.7,0,0));
00055         trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), angle));
00056         tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/map","/turntable"));
00057 
00058         trans.setOrigin(tf::Vector3(0.2,0.2,0.05));
00059         trans.setRotation(tf::Quaternion(tf::Vector3(0,0,1), 0));
00060         tb.sendTransform(tf::StampedTransform(trans,ros::Time::now(),"/turntable","/object_on_table"));
00061 
00062         rt.sleep();
00063     }
00064 
00065 }
00066 
00067 
00068 
00069 int main(int argc, char** argv)
00070 {
00071     ros::init(argc, argv, "my_tf_listener");
00072 
00073     std::cout << "USAGE bin/execname frame_name var_x var_y var_z var_rot_z var_rot_y var_rot_x" << std::endl;
00074 
00075     if (argc < 2)
00076         exit(0);
00077 
00078     ros::NodeHandle node;
00079 
00080     tf::TransformListener listener;
00081 
00082     UncertainTransformBroadcaster utfb;
00083 
00084     StampedCovariance stc;
00085     stc.resize(6,6);
00086     stc.setZero();
00087     //set values
00088     for (int i = 0; i < argc - 2; ++i)
00089     {
00090         stc(i,i) = atof(argv[i+2]);
00091     }
00092     stc.frame_id_ = argv[1];
00093     stc.stamp_ = ros::Time::now();
00094     utfb.sendCovariance(stc);
00095 
00096     UncertainTransformListener ulistener;
00097 
00098     ros::AsyncSpinner spinner(2);
00099     spinner.start();
00100 
00101     ros::Publisher poseArrPub = node.advertise<geometry_msgs::PoseArray>("sampled_poses", 100);
00102     ros::Publisher poseArrPubResampled = node.advertise<geometry_msgs::PoseArray>("resampled_poses", 100);
00103     ros::Publisher poseArrPubTimeGaussian = node.advertise<geometry_msgs::PoseArray>("sampled_time_gaussian", 100);
00104 
00105     boost::thread t1(spam_tfs);
00106     boost::thread t2(spam_turntable_tfs);
00107 
00108     ros::Duration(0.3).sleep();
00109 
00110     ros::Rate rate(30.0);
00111 
00112     ros::Time start_time = ros::Time::now();
00113 
00114     while (node.ok())
00115     {
00116 
00117         stc.stamp_ = ros::Time::now();
00118 
00119         StampedCovariance cov_to_send = stc;
00120         for (int i = 0; i < 6; ++i)
00121         {
00122             //cov_to_send(i,i) = fabs(sin((stc.stamp_ - start_time).toSec() * .5 )) * stc(i,i); // test time-varying covariance
00123             cov_to_send(i,i) = stc(i,i);
00124         }
00125 
00126         //std::cout << cov_to_send << std::endl;
00127 
00128         utfb.sendCovariance(cov_to_send);
00129 
00130         rate.sleep();
00131 
00132         ros::spinOnce();
00133 
00134         //sample
00135         if (1)
00136         {
00137             std::vector<StampedTransform> transform_samples;
00138             std::cout << endl << "***********************************************************************************" << endl << endl;
00139 
00140             ulistener.sampleTransform("map", "object", ros::Time(0), transform_samples, 500);
00141 
00142             geometry_msgs::PoseArray parr;
00143             parr.header.frame_id = transform_samples[0].frame_id_;
00144             parr.header.stamp = ros::Time(0);
00145             for (std::vector<StampedTransform>::iterator it = transform_samples.begin(); it != transform_samples.end(); ++it)
00146             {
00147                 geometry_msgs::Pose ps;
00148                 tf::poseTFToMsg(*it,ps);
00149                 parr.poses.push_back(ps);
00150             }
00151             poseArrPub.publish(parr);
00152         }
00153 
00154 
00155         // resample from covariance estimated from sample set
00156         if (1)
00157         {
00158             std::vector<StampedTransform> transform_samples;
00159 
00160             ulistener.sampleTransform("map", "object", ros::Time(0), transform_samples, 50);
00161 
00162             MatrixXd sample_covar;
00163             MatrixXd samples = ulistener.sampleSetTFtoMatrixXd(transform_samples);
00164             //ulistener.calculateSampleCovariance(samples.transpose(), samples.transpose(), sample_covar);
00165             ulistener.calculateSampleCovariance(samples, samples, sample_covar);
00166             //std::cout << "sample covariance " << endl << sample_covar << endl;
00167             MatrixXd mean = ulistener.calculateSampleMean(samples);
00168             //std::cout << "sample mean" << endl << mean << endl;
00169             std::vector<tf::Transform> resampled;
00170             ulistener.sampleFromMeanCov(ulistener.transformVectorXdToTF(mean), sample_covar, resampled, 50);
00171             geometry_msgs::PoseArray parr_resampled;
00172             parr_resampled.header.frame_id = transform_samples[0].frame_id_;
00173             parr_resampled.header.stamp = ros::Time(0);
00174             for (std::vector<tf::Transform>::iterator it = resampled.begin(); it != resampled.end(); ++it)
00175             {
00176                 geometry_msgs::Pose ps;
00177                 tf::poseTFToMsg(*it,ps);
00178                 parr_resampled.poses.push_back(ps);
00179             }
00180             poseArrPubResampled.publish(parr_resampled);
00181 
00182         }
00183 
00184         //sample from time a sec ago with .1 sec variance
00185         if (1)
00186         {
00187             std::vector<StampedTransform> transform_samples;
00188             ulistener.sampleTransformGaussianTime("map", "object", ros::Time::now() - ros::Duration(4),ros::Duration(2), transform_samples, 50);
00189 
00190             // note: the sampler catches all exeptions but does not generate a sample when catching one,
00191             // so the number of samples we get depends on how many fall inside a time period where the respective tfs are defined
00192             if (transform_samples.size() > 0)
00193             {
00194 
00195                 geometry_msgs::PoseArray parr;
00196                 parr.header.frame_id = transform_samples[0].frame_id_;
00197                 parr.header.stamp = ros::Time(0);
00198                 std::cout << "NUM SAMPLES TIME " << transform_samples.size() << std::endl;
00199                 for (std::vector<StampedTransform>::iterator it = transform_samples.begin(); it != transform_samples.end(); ++it)
00200                 {
00201                     geometry_msgs::Pose ps;
00202                     tf::poseTFToMsg(*it,ps);
00203                     parr.poses.push_back(ps);
00204                 }
00205 
00206                 poseArrPubTimeGaussian.publish(parr);
00207             }
00208         }
00209 
00210 
00211     }
00212     return 0;
00213 };
00214 
 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