UncertainTransformer.cpp
Go to the documentation of this file.
00001 
00002 
00003 #include "uncertain_tf/UncertainTransformer.h"
00004 
00005 
00006 using namespace Eigen;
00007 using namespace tf;
00008 using namespace uncertain_tf;
00009 using namespace std;
00010 
00011 namespace uncertain_tf {
00012 
00013 
00014 CovarianceTimeCache* UncertainTransformer::getCovariance(unsigned int frame_id)
00015 {
00016     if (frame_id == 0) 
00017         return NULL;
00018     else
00019     {
00020         while (frame_id >= covariances_.size())
00021             covariances_.push_back(NULL); // thats a hack
00022 
00023         // insert 0 covariance at time 0 to have covariances defined for all frames we query
00024         if (covariances_[frame_id] == NULL)
00025         {
00026             //std::cout << "creating new CovarianceTimeCache for id " << frame_id << endl;
00027             covariances_[frame_id] = new CovarianceTimeCache(ros::Duration(10)); 
00028             MatrixXd cov(6,6);
00029             cov.setZero();
00030             covariances_[frame_id]->insertData(CovarianceStorage(cov, ros::Time(0))); //, frame_id));
00031         }
00032         return covariances_[frame_id];
00033     }
00034 };
00035 
00036 bool UncertainTransformer::setCovariance(const StampedCovariance &cov)
00037 {
00038     std::string frame_id = tf::resolve(getTFPrefix(), cov.frame_id_);
00039     //std::cout << "setCovariance " << frame_id << endl;
00040     // todo: set the parent id, we could run into cases where the parent changes and we still have the covariance for another parent
00041 
00042     StampedCovariance mapped_covariance((MatrixXd)cov, cov.stamp_, cov.frame_id_);
00043     mapped_covariance.frame_id_ = tf::resolve(getTFPrefix(), cov.frame_id_);
00044 
00045     {
00046         boost::recursive_mutex::scoped_lock lock(cov_mutex_);
00047         CompactFrameID frame_number = lookupOrInsertFrameNumber(mapped_covariance.frame_id_); 
00048 
00049         CovarianceTimeCache* covariance = getCovariance(frame_number);
00050         /*if (covariance == NULL)
00051         {
00052             std::cout << "creating new CovarianceTimeCache for id " << frame_number << endl;
00053             covariances_[frame_number] = new CovarianceTimeCache(cache_time);
00054             covariance = covariances_[frame_number];
00055         }*/
00056 
00057         if (!covariance->insertData(CovarianceStorage(mapped_covariance, cov.stamp_))) // ,lookupOrInsertFrameNumber(mapped_covariance.frame_id_))))
00058         {
00059             ROS_INFO("ERROR in cov-insert");
00060             return false;
00061         }
00062 
00063     }
00064 
00065     return true;
00066 }
00067 
00068 
00069 } // namespace uncertain_tf
 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