transform_listener.h
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
32 #ifndef TF_TRANSFORMLISTENER_H
33 #define TF_TRANSFORMLISTENER_H
34 
35 #include "sensor_msgs/PointCloud.h"
36 #include "std_msgs/Empty.h"
37 #include "tf/tfMessage.h"
38 #include "tf/tf.h"
39 #include "ros/ros.h"
40 #include "ros/callback_queue.h"
41 
42 #include "tf/FrameGraph.h" //frame graph service
43 #include "boost/thread.hpp"
44 
46 
47 
48 namespace tf{
49 
54 inline std::string getPrefixParam(ros::NodeHandle & nh) {
55  std::string param;
56  if (!nh.searchParam("tf_prefix", param))
57  return "";
58 
59  std::string return_val;
60  nh.getParam(param, return_val);
61  return return_val;
62 }
63 
66 std::string remap(const std::string& frame_id) __attribute__((deprecated));
67 
69 class TransformListener : public Transformer { //subscribes to message and automatically stores incoming data
70 
71 public:
74  TransformListener(ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
75 
82  ros::Duration max_cache_time = ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread = true);
83 
85 
86  /* Methods from transformer unhiding them here */
91 
92 
95  void transformQuaternion(const std::string& target_frame, const geometry_msgs::QuaternionStamped& stamped_in, geometry_msgs::QuaternionStamped& stamped_out) const;
98  void transformVector(const std::string& target_frame, const geometry_msgs::Vector3Stamped& stamped_in, geometry_msgs::Vector3Stamped& stamped_out) const;
101  void transformPoint(const std::string& target_frame, const geometry_msgs::PointStamped& stamped_in, geometry_msgs::PointStamped& stamped_out) const;
104  void transformPose(const std::string& target_frame, const geometry_msgs::PoseStamped& stamped_in, geometry_msgs::PoseStamped& stamped_out) const;
105 
106  /* \brief Transform a Stamped Twist Message into the target frame
107  * This can throw all that lookupTransform can throw as well as tf::InvalidTransform */
108  // http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review
109  // void transformTwist(const std::string& target_frame, const geometry_msgs::TwistStamped& stamped_in, geometry_msgs::TwistStamped& stamped_out) const;
110 
113  void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
114  const geometry_msgs::QuaternionStamped& qin,
115  const std::string& fixed_frame, geometry_msgs::QuaternionStamped& qout) const;
118  void transformVector(const std::string& target_frame, const ros::Time& target_time,
119  const geometry_msgs::Vector3Stamped& vin,
120  const std::string& fixed_frame, geometry_msgs::Vector3Stamped& vout) const;
123  void transformPoint(const std::string& target_frame, const ros::Time& target_time,
124  const geometry_msgs::PointStamped& pin,
125  const std::string& fixed_frame, geometry_msgs::PointStamped& pout) const;
128  void transformPose(const std::string& target_frame, const ros::Time& target_time,
129  const geometry_msgs::PoseStamped& pin,
130  const std::string& fixed_frame, geometry_msgs::PoseStamped& pout) const;
131 
132 
135  void transformPointCloud(const std::string& target_frame, const sensor_msgs::PointCloud& pcin, sensor_msgs::PointCloud& pcout) const;
136 
139  void transformPointCloud(const std::string& target_frame, const ros::Time& target_time,
140  const sensor_msgs::PointCloud& pcin,
141  const std::string& fixed_frame, sensor_msgs::PointCloud& pcout) const;
142 
143 
144 
146 
147  bool getFrames(tf::FrameGraph::Request&, tf::FrameGraph::Response& res)
148  {
149  res.dot_graph = allFramesAsDot();
150  return true;
151  }
152 
153  /* \brief Resolve frame_name into a frame_id using tf_prefix parameter */
154  std::string resolve(const std::string& frame_name)
155  {
156  ros::NodeHandle n("~");
157  std::string prefix = tf::getPrefixParam(n);
158  return tf::resolve(prefix, frame_name);
159  };
160 
161 protected:
162  bool ok() const;
163 
164 private:
165 
166  // Must be above the listener
168 
171 
173  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;
174 
175 };
176 }
177 
178 #endif //TF_TRANSFORMLISTENER_H
bool getFrames(tf::FrameGraph::Request &, tf::FrameGraph::Response &res)
bool param(const std::string &param_name, T &param_val, const T &default_val)
std::string remap(const std::string &frame_id) __attribute__((deprecated))
resolve names
std::string getPrefixParam(ros::NodeHandle &nh)
Get the tf_prefix from the parameter server.
void transformVector(const std::string &target_frame, const Stamped< tf::Vector3 > &stamped_in, Stamped< tf::Vector3 > &stamped_out) const
Transform a Stamped Vector3 into the target frame This can throw anything a lookupTransform can throw...
Definition: tf.cpp:466
void transformQuaternion(const std::string &target_frame, const Stamped< tf::Quaternion > &stamped_in, Stamped< tf::Quaternion > &stamped_out) const
Transform a Stamped Quaternion into the target frame This can throw anything a lookupTransform can th...
Definition: tf.cpp:453
void transformPoint(const std::string &target_frame, const geometry_msgs::PointStamped &stamped_in, geometry_msgs::PointStamped &stamped_out) const
Transform a Stamped Point Message into the target frame This can throw all that lookupTransform can t...
static const double DEFAULT_CACHE_TIME
10.0 is the default amount of time to cache data in seconds, set in cpp file.
Definition: tf.h:95
Definition: exceptions.h:38
std::string resolve(const std::string &frame_name)
std::string resolve(const std::string &prefix, const std::string &frame_name)
resolve tf names
Definition: tf.cpp:159
tf2_ros::TransformListener tf2_listener_
replacing implementation with tf2_ros&#39;
void transformQuaternion(const std::string &target_frame, const geometry_msgs::QuaternionStamped &stamped_in, geometry_msgs::QuaternionStamped &stamped_out) const
Transform a Stamped Quaternion Message into the target frame This can throw all that lookupTransform ...
void transformPointCloud(const std::string &target_frame, const sensor_msgs::PointCloud &pcin, sensor_msgs::PointCloud &pcout) const
Transform a sensor_msgs::PointCloud natively This can throw all that lookupTransform can throw as wel...
TransformListener(ros::Duration max_cache_time=ros::Duration(DEFAULT_CACHE_TIME), bool spin_thread=true)
Constructor for transform listener.
bool searchParam(const std::string &key, std::string &result) const
void transformPose(const std::string &target_frame, const Stamped< tf::Pose > &stamped_in, Stamped< tf::Pose > &stamped_out) const
Transform a Stamped Pose into the target frame This can throw anything a lookupTransform can throw as...
Definition: tf.cpp:494
The Transform class supports rigid transforms with only translation and rotation and no scaling/shear...
Definition: Transform.h:31
This class inherits from Transformer and automatically subscribes to ROS transform messages...
void transformPose(const std::string &target_frame, const geometry_msgs::PoseStamped &stamped_in, geometry_msgs::PoseStamped &stamped_out) const
Transform a Stamped Pose Message into the target frame This can throw all that lookupTransform can th...
bool getParam(const std::string &key, std::string &s) const
void transformPoint(const std::string &target_frame, const Stamped< tf::Point > &stamped_in, Stamped< tf::Point > &stamped_out) const
Transform a Stamped Point into the target frame This can throw anything a lookupTransform can throw a...
Definition: tf.cpp:484
std::string allFramesAsDot(double current_time=0) const
A way to see what frames have been cached Useful for debugging.
Definition: tf.cpp:439
void transformVector(const std::string &target_frame, const geometry_msgs::Vector3Stamped &stamped_in, geometry_msgs::Vector3Stamped &stamped_out) const
Transform a Stamped Vector Message into the target frame This can throw all that lookupTransform can ...
A Class which provides coordinate transforms between any two frames in a system.
Definition: tf.h:90
tf::tfVector4 __attribute__


tf
Author(s): Tully Foote, Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Mon Jun 10 2019 12:25:26