transform_listener.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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" //frame graph service
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 { //subscribes to message and automatically stores incoming data
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   /* Methods from transformer unhiding them here */
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   /* \brief Transform a Stamped Twist Message into the target frame 
00107    * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */
00108   // http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review
00109   //  void transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& stamped_in, geometry_msgs::TwistStamped& stamped_out) const;
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   /* \brief Resolve frame_name into a frame_id using tf_prefix parameter */
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   // Must be above the listener
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


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Aug 11 2017 02:21:55