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
00045
00046
00047 class BufferInterface
00048 {
00049 public:
00050
00061 virtual geometry_msgs::TransformStamped
00062 lookupTransform(const std::string& target_frame, const std::string& source_frame,
00063 const ros::Time& time, const ros::Duration timeout) const = 0;
00064
00077 virtual geometry_msgs::TransformStamped
00078 lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00079 const std::string& source_frame, const ros::Time& source_time,
00080 const std::string& fixed_frame, const ros::Duration timeout) const = 0;
00081
00082
00091 virtual bool
00092 canTransform(const std::string& target_frame, const std::string& source_frame,
00093 const ros::Time& time, const ros::Duration timeout, std::string* errstr = NULL) const = 0;
00094
00105 virtual bool
00106 canTransform(const std::string& target_frame, const ros::Time& target_time,
00107 const std::string& source_frame, const ros::Time& source_time,
00108 const std::string& fixed_frame, const ros::Duration timeout, std::string* errstr = NULL) const = 0;
00109
00110
00111 template <class T>
00112 T& transform(const T& in, T& out,
00113 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
00114 {
00115
00116 tf2::doTransform(in, out, lookupTransform(target_frame, tf2::getFrameId(in), tf2::getTimestamp(in), timeout));
00117 return out;
00118 }
00119
00120
00121
00122 template <class T>
00123 T transform(const T& in,
00124 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
00125 {
00126 T out;
00127 return transform(in, out, target_frame, timeout);
00128 }
00129
00130
00131 template <class A, class B>
00132 B& transform(const A& in, B& out,
00133 const std::string& target_frame, ros::Duration timeout=ros::Duration(0.0)) const
00134 {
00135 A copy = transform(in, target_frame, timeout);
00136 tf2::convert(copy, out);
00137 return out;
00138 }
00139
00140
00141 template <class T>
00142 T& transform(const T& in, T& out,
00143 const std::string& target_frame, const ros::Time& target_time,
00144 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
00145 {
00146
00147 tf2::doTransform(in, out, lookupTransform(target_frame, target_time,
00148 tf2::getFrameId(in), tf2::getTimestamp(in),
00149 fixed_frame, timeout));
00150 return out;
00151 }
00152
00153
00154
00155 template <class T>
00156 T transform(const T& in,
00157 const std::string& target_frame, const ros::Time& target_time,
00158 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
00159 {
00160 T out;
00161 return transform(in, out, target_frame, target_time, fixed_frame, timeout);
00162 }
00163
00164
00165 template <class A, class B>
00166 B& transform(const A& in, B& out,
00167 const std::string& target_frame, const ros::Time& target_time,
00168 const std::string& fixed_frame, ros::Duration timeout=ros::Duration(0.0)) const
00169 {
00170
00171 A copy = transform(in, target_frame, target_time, fixed_frame, timeout);
00172 tf2::convert(copy, out);
00173 return out;
00174 }
00175
00176
00177 };
00178
00179
00180 }
00181
00182 #endif // TF2_ROS_BUFFER_INTERFACE_H