UncertainTransformer.h
Go to the documentation of this file.
00001 
00002 
00003 #ifndef _UNCERTAIN_TRANSFORMER_
00004 #define _UNCERTAIN_TRANSFORMER_
00005 
00006 #include "tf/transform_listener.h"
00007 #include "uncertain_tf/CovarianceTimeCache.h"
00008 #include "uncertain_tf/CovarianceStamped.h"
00009 
00010 // ugly to inherit from listener but avoids multiple inheritance of Transformer in UncertainTransformListener
00011 
00012 namespace uncertain_tf{
00013 
00014 class UncertainTransformer : public tf::TransformListener
00015 {
00016 
00017 protected:
00018     bool setCovariance(const StampedCovariance &cov);
00019 
00020     CovarianceTimeCache* getCovariance(unsigned int frame_number);
00021 
00022     std::vector<CovarianceTimeCache*> covariances_;
00023 
00024     mutable boost::recursive_mutex cov_mutex_;
00025 
00026 };
00027 
00028 static inline void covarianceStampedTFToMsg(const StampedCovariance& covariance, uncertain_tf::CovarianceStamped& msg)
00029 {
00030     msg.header.stamp = covariance.stamp_;
00031     msg.header.frame_id = covariance.frame_id_;
00032     for (int row = 0; row < 6; ++row)
00033         for (int col = 0; col < 6; ++col)
00034             msg.covariance[row + col * 6] = covariance(row,col);
00035 };
00036 
00037 
00038 static inline void covarianceStampedMsgToTF(const uncertain_tf::CovarianceStamped& msg, StampedCovariance& covariance)
00039 {
00040     covariance.stamp_ = msg.header.stamp;
00041     covariance.frame_id_ = msg.header.frame_id;
00042     //make sure matrix is sized well
00043     covariance.resize(6,6);
00044     for (int row = 0; row < 6; ++row)
00045         for (int col = 0; col < 6; ++col)
00046             covariance(row,col) = msg.covariance[row + col * 6];
00047 };
00048 
00049 
00050 } // namespace uncertain_tf
00051 
00052 #endif
 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