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
00032 #ifndef TF2_ROS_BUFFER_INTERFACE_H
00033 #define TF2_ROS_BUFFER_INTERFACE_H
00034
00035 #include <tf2/buffer_core.h>
00036 #include <tf2/transform_datatypes.h>
00037 #include <tf2/exceptions.h>
00038 #include <geometry_msgs/TransformStamped.h>
00039 #include <sstream>
00040 #include <tf2/convert.h>
00041
00042 namespace tf2_ros
00043 {
00044
00048 class BufferInterface
00049 {
00050 public:
00051
00062 virtual geometry_msgs::TransformStamped
00063 lookupTransform(const std::string& target_frame, const std::string& source_frame,
00064 const ros::Time& time, const ros::Duration timeout) const = 0;
00065
00078 virtual geometry_msgs::TransformStamped
00079 lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00080 const std::string& source_frame, const ros::Time& source_time,
00081 const std::string& fixed_frame, const ros::Duration timeout) const = 0;
00082
00083
00092 virtual bool
00093 canTransform(const std::string& target_frame, const std::string& source_frame,
00094 const ros::Time& time, const ros::Duration timeout, std::string* errstr = NULL) const = 0;
00095
00106 virtual bool
00107 canTransform(const std::string& target_frame, const ros::Time& target_time,
00108 const std::string& source_frame, const ros::Time& source_time,
00109 const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const = 0;
00110
00122 template <class T>
00123 T& transform(const T& in, T& out,
00124 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
00125 {
00126
00127 tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
00128 return out;
00129 }
00130
00142 template <class T>
00143 T transform(const T& in,
00144 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
00145 {
00146 T out;
00147 return transform(in, out, target_frame, timeout);
00148 }
00149
00167 template <class A, class B>
00168 B& transform(const A& in, B& out,
00169 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
00170 {
00171 A copy = transform(in, target_frame, timeout);
00172 tf2::convert(copy, out);
00173 return out;
00174 }
00175
00191 template <class T>
00192 T& transform(const T& in, T& out,
00193 const std::string& target_frame, const ros::Time& target_time,
00194 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
00195 {
00196
00197 tf2::doTransform(in, out, lookupTransform(target_frame, target_time,
00198 tf2::getFrameId(in), tf2::getTimestamp(in),
00199 fixed_frame, timeout));
00200 return out;
00201 }
00202
00203
00219 template <class T>
00220 T transform(const T& in,
00221 const std::string& target_frame, const ros::Time& target_time,
00222 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
00223 {
00224 T out;
00225 return transform(in, out, target_frame, target_time, fixed_frame, timeout);
00226 }
00227
00228
00250 template <class A, class B>
00251 B& transform(const A& in, B& out,
00252 const std::string& target_frame, const ros::Time& target_time,
00253 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
00254 {
00255
00256 A copy = transform(in, target_frame, target_time, fixed_frame, timeout);
00257 tf2::convert(copy, out);
00258 return out;
00259 }
00260
00261
00262 };
00263
00264
00265 }
00266
00267 #endif // TF2_ROS_BUFFER_INTERFACE_H