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