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 namespace tf{
00046
00051 inline std::string getPrefixParam(ros::NodeHandle & nh) {
00052 std::string param;
00053 if (!nh.searchParam("tf_prefix", param))
00054 return "";
00055
00056 std::string return_val;
00057 nh.getParam(param, return_val);
00058 return return_val;
00059 }
00060
00063 std::string remap(const std::string& frame_id) __attribute__((deprecated));
00064
00066 class TransformListener : public Transformer {
00067
00068 public:
00071 TransformListener(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
00072
00078 TransformListener(const ros::NodeHandle& nh,
00079 ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
00080
00081 ~TransformListener();
00082
00083
00084 using Transformer::transformQuaternion;
00085 using Transformer::transformVector;
00086 using Transformer::transformPoint;
00087 using Transformer::transformPose;
00088
00089
00092 void transformQuaternion(const std::string& target_frame, const geometry_msgs::QuaternionStamped& stamped_in, geometry_msgs::QuaternionStamped& stamped_out) const;
00095 void transformVector(const std::string& target_frame, const geometry_msgs::Vector3Stamped& stamped_in, geometry_msgs::Vector3Stamped& stamped_out) const;
00098 void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const;
00101 void transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& stamped_in, geometry_msgs::PoseStamped& stamped_out) const;
00102
00105
00106
00107
00110 void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00111 const geometry_msgs::QuaternionStamped& qin,
00112 const std::string& fixed_frame, geometry_msgs::QuaternionStamped& qout) const;
00115 void transformVector(const std::string& target_frame, const ros::Time& target_time,
00116 const geometry_msgs::Vector3Stamped& vin,
00117 const std::string& fixed_frame, geometry_msgs::Vector3Stamped& vout) const;
00120 void transformPoint(const std::string& target_frame, const ros::Time& target_time,
00121 const geometry_msgs::PointStamped& pin,
00122 const std::string& fixed_frame, geometry_msgs::PointStamped& pout) const;
00125 void transformPose(const std::string& target_frame, const ros::Time& target_time,
00126 const geometry_msgs::PoseStamped& pin,
00127 const std::string& fixed_frame, geometry_msgs::PoseStamped& pout) const;
00128
00129
00132 void transformPointCloud(const std::string& target_frame, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout) const;
00133
00136 void transformPointCloud(const std::string& target_frame, const ros::Time& target_time,
00137 const sensor_msgs::PointCloud& pcin,
00138 const std::string& fixed_frame, sensor_msgs::PointCloud& pcout) const;
00139
00140
00141
00143
00144 bool getFrames(tf::FrameGraph::Request& req, tf::FrameGraph::Response& res)
00145 {
00146 res.dot_graph = allFramesAsDot();
00147 return true;
00148 }
00149
00150
00151 std::string resolve(const std::string& frame_name)
00152 {
00153 return tf::resolve(tf_prefix_, frame_name);
00154 };
00155
00156 private:
00158 ros::Time last_update_ros_time_;
00159
00161 void init();
00162 void initWithThread();
00163
00165 void subscription_callback(const tf::tfMessageConstPtr& msg);
00166
00168 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;
00169
00171 std_msgs::Empty empty_;
00172 ros::ServiceServer tf_frames_srv_;
00173
00174
00175 ros::CallbackQueue tf_message_callback_queue_;
00176 boost::thread* dedicated_listener_thread_;
00177 ros::NodeHandle node_;
00178 ros::Subscriber message_subscriber_tf_, reset_time_subscriber_;
00179
00180 void dedicatedListenerThread()
00181 {
00182 while (using_dedicated_thread_)
00183 {
00184 tf_message_callback_queue_.callAvailable(ros::WallDuration(0.01));
00185 }
00186 };
00187
00188 protected:
00189 bool ok() const;
00190
00191 };
00192 }
00193
00194 #endif //TF_TRANSFORMLISTENER_H