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
00032 #ifndef TF_TRANSFORMLISTENER_H
00033 #define TF_TRANSFORMLISTENER_H
00034
00035 #include "sensor_msgs/PointCloud.h"
00036 #include "std_msgs/Empty.h"
00037 #include "tf/tfMessage.h"
00038 #include "tf/tf.h"
00039 #include "ros/ros.h"
00040 #include "ros/callback_queue.h"
00041
00042 #include "tf/FrameGraph.h"
00043 #include "boost/thread.hpp"
00044
00045 #include <tf2_ros/transform_listener.h>
00046
00047
00048 namespace tf{
00049
00054 inline std::string getPrefixParam(ros::NodeHandle & nh) {
00055 std::string param;
00056 if (!nh.searchParam("tf_prefix", param))
00057 return "";
00058
00059 std::string return_val;
00060 nh.getParam(param, return_val);
00061 return return_val;
00062 }
00063
00066 std::string remap(const std::string& frame_id) __attribute__((deprecated));
00067
00069 class TransformListener : public Transformer {
00070
00071 public:
00074 TransformListener(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
00075
00081 TransformListener(const ros::NodeHandle& nh,
00082 ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
00083
00084 ~TransformListener();
00085
00086
00087 using Transformer::transformQuaternion;
00088 using Transformer::transformVector;
00089 using Transformer::transformPoint;
00090 using Transformer::transformPose;
00091
00092
00095 void transformQuaternion(const std::string& target_frame, const geometry_msgs::QuaternionStamped& stamped_in, geometry_msgs::QuaternionStamped& stamped_out) const;
00098 void transformVector(const std::string& target_frame, const geometry_msgs::Vector3Stamped& stamped_in, geometry_msgs::Vector3Stamped& stamped_out) const;
00101 void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const;
00104 void transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& stamped_in, geometry_msgs::PoseStamped& stamped_out) const;
00105
00106
00107
00108
00109
00110
00113 void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00114 const geometry_msgs::QuaternionStamped& qin,
00115 const std::string& fixed_frame, geometry_msgs::QuaternionStamped& qout) const;
00118 void transformVector(const std::string& target_frame, const ros::Time& target_time,
00119 const geometry_msgs::Vector3Stamped& vin,
00120 const std::string& fixed_frame, geometry_msgs::Vector3Stamped& vout) const;
00123 void transformPoint(const std::string& target_frame, const ros::Time& target_time,
00124 const geometry_msgs::PointStamped& pin,
00125 const std::string& fixed_frame, geometry_msgs::PointStamped& pout) const;
00128 void transformPose(const std::string& target_frame, const ros::Time& target_time,
00129 const geometry_msgs::PoseStamped& pin,
00130 const std::string& fixed_frame, geometry_msgs::PoseStamped& pout) const;
00131
00132
00135 void transformPointCloud(const std::string& target_frame, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout) const;
00136
00139 void transformPointCloud(const std::string& target_frame, const ros::Time& target_time,
00140 const sensor_msgs::PointCloud& pcin,
00141 const std::string& fixed_frame, sensor_msgs::PointCloud& pcout) const;
00142
00143
00144
00146
00147 bool getFrames(tf::FrameGraph::Request&, tf::FrameGraph::Response& res)
00148 {
00149 res.dot_graph = allFramesAsDot();
00150 return true;
00151 }
00152
00153
00154 std::string resolve(const std::string& frame_name)
00155 {
00156 ros::NodeHandle n("~");
00157 std::string prefix = tf::getPrefixParam(n);
00158 return tf::resolve(prefix, frame_name);
00159 };
00160
00161 protected:
00162 bool ok() const;
00163
00164 private:
00165
00166
00167 ros::NodeHandle node_;
00168
00170 tf2_ros::TransformListener tf2_listener_;
00171
00173 void transformPointCloud(const std::string & target_frame, const Transform& transform, const ros::Time& target_time, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout) const;
00174
00175 };
00176 }
00177
00178 #endif //TF_TRANSFORMLISTENER_H