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 WAREHOUSE_ROS_TF_COLLECTION_H
00040 #define WAREHOUSE_ROS_TF_COLLECTION_H
00041
00042 #include <warehouse_ros/message_collection.h>
00043 #include <tf/transform_listener.h>
00044 #include <tf/tfMessage.h>
00045
00046 namespace warehouse_ros
00047 {
00048
00051 class TransformSource
00052 {
00053 public:
00056 virtual tf::StampedTransform lookupTransform(const std::string& target_frame, const std::string& source_frame,
00057 double t) const = 0;
00058 };
00059
00067 class TransformCollection : public TransformSource
00068 {
00069 public:
00070 TransformCollection(MessageCollection<tf::tfMessage> &coll, const double search_back = 10.0,
00071 const double search_forward = 1.0) :
00072 TransformSource(), coll_(coll), search_back_(search_back), search_forward_(search_forward)
00073 {
00074 }
00075
00078 virtual tf::StampedTransform lookupTransform(const std::string& target_frame, const std::string& source_frame,
00079 double t) const;
00080
00082 void putTransform(tf::StampedTransform);
00083
00084 private:
00085 MessageCollection<tf::tfMessage> coll_;
00086 double search_back_;
00087 double search_forward_;
00088 };
00089
00092 class LiveTransformSource : public TransformSource
00093 {
00094 public:
00098 LiveTransformSource(double timeout = 0) :
00099 TransformSource(), tf_(new tf::TransformListener()), timeout_(timeout)
00100 {
00101 }
00102
00105 virtual tf::StampedTransform lookupTransform(const std::string& target, const std::string& source, double t) const
00106 {
00107 ros::Time tm(t);
00108 tf_->waitForTransform(target, source, tm, ros::Duration(timeout_));
00109 tf::StampedTransform trans;
00110 tf_->lookupTransform(target, source, tm, trans);
00111 return trans;
00112 }
00113
00114 private:
00115 ros::NodeHandle nh_;
00116 boost::shared_ptr<tf::TransformListener> tf_;
00117 ros::Duration timeout_;
00118 };
00119
00120 }
00121
00122 #endif // include guard