transform_listener.cpp
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 
33 
34 
35 using namespace tf2_ros;
36 
37 
39  dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false)
40 {
41  if (spin_thread)
43  else
44  init();
45 }
46 
49 , node_(nh)
50 , buffer_(buffer)
52 {
53  if (spin_thread)
55  else
56  init();
57 }
58 
59 
61 {
64  {
67  }
68 }
69 
71 {
72  message_subscriber_tf_ = node_.subscribe<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1));
73  message_subscriber_tf_static_ = node_.subscribe<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, _1));
74 }
75 
77 {
79  ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1), ros::VoidPtr(), &tf_message_callback_queue_);
81 
82  ros::SubscribeOptions ops_tf_static = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, _1), ros::VoidPtr(), &tf_message_callback_queue_);
84 
85  dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this));
86 
87  //Tell the buffer we have a dedicated thread to enable timeouts
89 }
90 
91 
92 
94 {
95  subscription_callback_impl(msg_evt, false);
96 }
98 {
99  subscription_callback_impl(msg_evt, true);
100 }
101 
103 {
104  ros::Time now = ros::Time::now();
105  if(now < last_update_){
106  ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing TF buffer.");
107  buffer_.clear();
108  }
109  last_update_ = now;
110 
111 
112 
113  const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage());
114  std::string authority = msg_evt.getPublisherName(); // lookup the authority
115  for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
116  {
117  try
118  {
119  buffer_.setTransform(msg_in.transforms[i], authority, is_static);
120  }
121 
122  catch (tf2::TransformException& ex)
123  {
125  std::string temp = ex.what();
126  ROS_ERROR("Failure to set recieved transform from %s to %s with error: %s\n", msg_in.transforms[i].child_frame_id.c_str(), msg_in.transforms[i].header.frame_id.c_str(), temp.c_str());
127  }
128  }
129 };
130 
131 
132 
133 
134 
TransformListener(tf2::BufferCore &buffer, bool spin_thread=true)
Constructor for transform listener.
boost::thread * dedicated_listener_thread_
ros::Subscriber message_subscriber_tf_static_
ros::CallbackQueue tf_message_callback_queue_
void setUsingDedicatedThread(bool value)
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
void subscription_callback_impl(const ros::MessageEvent< tf2_msgs::TFMessage const > &msg_evt, bool is_static)
const std::string & getPublisherName() const
const boost::shared_ptr< ConstMessage > & getConstMessage() const
ros::Subscriber message_subscriber_tf_
#define ROS_WARN_STREAM(args)
void init()
Initialize this transform listener, subscribing, advertising services, etc.
void static_subscription_callback(const ros::MessageEvent< tf2_msgs::TFMessage const > &msg_evt)
Definition: buffer.h:42
static Time now()
void subscription_callback(const ros::MessageEvent< tf2_msgs::TFMessage const > &msg_evt)
Callback function for ros message subscriptoin.
boost::shared_ptr< void > VoidPtr
#define ROS_ERROR(...)


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Fri Oct 16 2020 19:08:54