Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00039 #ifndef MONGO_ROS_TF_COLLECTION_H
00040 #define MONGO_ROS_TF_COLLECTION_H
00041
00042 #include <mongo_ros/message_collection.h>
00043 #include <tf/transform_listener.h>
00044 #include <tf/tfMessage.h>
00045
00046 namespace mongo_ros
00047 {
00048
00051 class TransformSource
00052 {
00053 public:
00054
00057 virtual tf::StampedTransform lookupTransform (const std::string& target_frame,
00058 const std::string& source_frame,
00059 double t) const = 0;
00060 };
00061
00069 class TransformCollection : public TransformSource
00070 {
00071 public:
00072 TransformCollection (const std::string& db,
00073 const std::string& coll="tf",
00074 const std::string& host="localhost",
00075 const unsigned port=27017,
00076 const double history_length=10.0) :
00077 TransformSource(),
00078 coll_(db, coll, host, port), history_length_(history_length)
00079 {}
00080
00083 virtual
00084 tf::StampedTransform lookupTransform (const std::string& target_frame,
00085 const std::string& source_frame,
00086 double t) const;
00087
00088
00089 private:
00090
00092 mongo::Query transformQuery (double t) const;
00093
00094 MessageCollection<tf::tfMessage> coll_;
00095 double history_length_;
00096
00097 };
00098
00099
00102 class LiveTransformSource : public TransformSource
00103 {
00104 public:
00105
00109 LiveTransformSource (double timeout = 0) :
00110 TransformSource(), tf_(new tf::TransformListener()), timeout_(timeout)
00111 {}
00112
00115 virtual
00116 tf::StampedTransform lookupTransform (const std::string& target,
00117 const std::string& source,
00118 double t) const;
00119
00120 private:
00121
00122 ros::NodeHandle nh_;
00123 boost::shared_ptr<tf::TransformListener> tf_;
00124 ros::Duration timeout_;
00125 };
00126
00127 }
00128
00129 #endif // include guard