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