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 
38 TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread, ros::TransportHints transport_hints):
39  dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false), transport_hints_(transport_hints)
40 {
41  if (spin_thread)
43  else
44  init();
45 }
46 
48  ros::TransportHints transport_hints)
49 : dedicated_listener_thread_(NULL)
50 , node_(nh)
51 , buffer_(buffer)
52 , using_dedicated_thread_(false)
53 , transport_hints_(transport_hints)
54 {
55  if (spin_thread)
57  else
58  init();
59 }
60 
62 {
65  {
68  }
69 }
70 
72 {
73  message_subscriber_tf_ = node_.subscribe<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, boost::placeholders::_1), ros::VoidConstPtr(), transport_hints_);
74  message_subscriber_tf_static_ = node_.subscribe<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, boost::placeholders::_1));
75 }
76 
78 {
80  ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, boost::placeholders::_1), ros::VoidPtr(), &tf_message_callback_queue_);
83 
84  ros::SubscribeOptions ops_tf_static = ros::SubscribeOptions::create<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, boost::placeholders::_1), ros::VoidPtr(), &tf_message_callback_queue_);
86 
87  dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this));
88 
89  //Tell the buffer we have a dedicated thread to enable timeouts
91 }
92 
93 
94 
96 {
97  subscription_callback_impl(msg_evt, false);
98 }
100 {
101  subscription_callback_impl(msg_evt, true);
102 }
103 
105 {
106  ros::Time now = ros::Time::now();
107  if(now < last_update_){
108  ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing TF buffer.");
109  buffer_.clear();
110  }
111  last_update_ = now;
112 
113 
114 
115  const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage());
116  std::string authority = msg_evt.getPublisherName(); // lookup the authority
117  for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
118  {
119  try
120  {
121  buffer_.setTransform(msg_in.transforms[i], authority, is_static);
122  }
123 
124  catch (tf2::TransformException& ex)
125  {
127  std::string temp = ex.what();
128  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());
129  }
130  }
131 };
132 
133 
134 
135 
136 
tf2_ros::TransformListener::buffer_
tf2::BufferCore & buffer_
Definition: transform_listener.h:76
tf2_ros::TransformListener::initWithThread
void initWithThread()
Definition: transform_listener.cpp:77
tf2_ros::TransformListener::message_subscriber_tf_static_
ros::Subscriber message_subscriber_tf_static_
Definition: transform_listener.h:75
tf2::BufferCore::setUsingDedicatedThread
void setUsingDedicatedThread(bool value)
boost::shared_ptr< void >
tf2_ros::TransformListener::static_subscription_callback
void static_subscription_callback(const ros::MessageEvent< tf2_msgs::TFMessage const > &msg_evt)
Definition: transform_listener.cpp:99
tf2_ros::TransformListener::subscription_callback_impl
void subscription_callback_impl(const ros::MessageEvent< tf2_msgs::TFMessage const > &msg_evt, bool is_static)
Definition: transform_listener.cpp:104
ros::SubscribeOptions::transport_hints
TransportHints transport_hints
tf2::BufferCore::setTransform
bool setTransform(const geometry_msgs::TransformStamped &transform, const std::string &authority, bool is_static=false)
ros::TransportHints
ros::MessageEvent::getPublisherName
const std::string & getPublisherName() const
tf2_ros::TransformListener::dedicated_listener_thread_
boost::thread * dedicated_listener_thread_
Definition: transform_listener.h:72
ros::MessageEvent::getConstMessage
const boost::shared_ptr< ConstMessage > & getConstMessage() const
tf2_ros::TransformListener::init
void init()
Initialize this transform listener, subscribing, advertising services, etc.
Definition: transform_listener.cpp:71
tf2_ros::TransformListener::using_dedicated_thread_
bool using_dedicated_thread_
Definition: transform_listener.h:77
ROS_WARN_STREAM
#define ROS_WARN_STREAM(args)
tf2_ros::TransformListener::subscription_callback
void subscription_callback(const ros::MessageEvent< tf2_msgs::TFMessage const > &msg_evt)
Callback function for ros message subscription.
Definition: transform_listener.cpp:95
tf2_ros
Definition: buffer.h:42
ros::VoidConstPtr
boost::shared_ptr< void const > VoidConstPtr
tf2::BufferCore::clear
void clear()
tf2_ros::TransformListener::last_update_
ros::Time last_update_
Definition: transform_listener.h:79
transform_listener.h
tf2_ros::TransformListener::node_
ros::NodeHandle node_
Definition: transform_listener.h:73
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, const boost::function< void(C)> &callback, const VoidConstPtr &tracked_object=VoidConstPtr(), const TransportHints &transport_hints=TransportHints())
tf2_ros::TransformListener::~TransformListener
~TransformListener()
Definition: transform_listener.cpp:61
ros::SubscribeOptions
ros::Time
tf2::BufferCore
tf2_ros::TransformListener::TransformListener
TransformListener(tf2::BufferCore &buffer, bool spin_thread=true, ros::TransportHints transport_hints=ros::TransportHints())
Constructor for transform listener.
Definition: transform_listener.cpp:38
ROS_ERROR
#define ROS_ERROR(...)
tf2_ros::TransformListener::dedicatedListenerThread
void dedicatedListenerThread()
Definition: transform_listener.h:81
tf2_ros::TransformListener::transport_hints_
ros::TransportHints transport_hints_
Definition: transform_listener.h:78
tf2::TransformException
tf2_ros::TransformListener::message_subscriber_tf_
ros::Subscriber message_subscriber_tf_
Definition: transform_listener.h:74
ros::NodeHandle
ros::MessageEvent
ros::Time::now
static Time now()
tf2_ros::TransformListener::tf_message_callback_queue_
ros::CallbackQueue tf_message_callback_queue_
Definition: transform_listener.h:71


tf2_ros
Author(s): Eitan Marder-Eppstein, Wim Meeussen
autogenerated on Sun Feb 4 2024 03:18:16