Go to the documentation of this file.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 #include "tf2_ros/transform_listener.h"
00033
00034
00035 using namespace tf2_ros;
00036
00037
00038 TransformListener::TransformListener(tf2::BufferCore& buffer, bool spin_thread):
00039 dedicated_listener_thread_(NULL), buffer_(buffer), using_dedicated_thread_(false)
00040 {
00041 if (spin_thread)
00042 initWithThread();
00043 else
00044 init();
00045 }
00046
00047 TransformListener::TransformListener(tf2::BufferCore& buffer, const ros::NodeHandle& nh, bool spin_thread)
00048 : dedicated_listener_thread_(NULL)
00049 , node_(nh)
00050 , buffer_(buffer)
00051 , using_dedicated_thread_(false)
00052 {
00053 if (spin_thread)
00054 initWithThread();
00055 else
00056 init();
00057 }
00058
00059
00060 TransformListener::~TransformListener()
00061 {
00062 using_dedicated_thread_ = false;
00063 if (dedicated_listener_thread_)
00064 {
00065 dedicated_listener_thread_->join();
00066 delete dedicated_listener_thread_;
00067 }
00068 }
00069
00070 void TransformListener::init()
00071 {
00072 message_subscriber_tf_ = node_.subscribe<tf2_msgs::TFMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1));
00073 message_subscriber_tf_static_ = node_.subscribe<tf2_msgs::TFMessage>("/tf_static", 100, boost::bind(&TransformListener::static_subscription_callback, this, _1));
00074 }
00075
00076 void TransformListener::initWithThread()
00077 {
00078 using_dedicated_thread_ = true;
00079 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_);
00080 message_subscriber_tf_ = node_.subscribe(ops_tf);
00081
00082 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_);
00083 message_subscriber_tf_static_ = node_.subscribe(ops_tf_static);
00084
00085 dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this));
00086
00087
00088 buffer_.setUsingDedicatedThread(true);
00089 }
00090
00091
00092
00093 void TransformListener::subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt)
00094 {
00095 subscription_callback_impl(msg_evt, false);
00096 }
00097 void TransformListener::static_subscription_callback(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt)
00098 {
00099 subscription_callback_impl(msg_evt, true);
00100 }
00101
00102 void TransformListener::subscription_callback_impl(const ros::MessageEvent<tf2_msgs::TFMessage const>& msg_evt, bool is_static)
00103 {
00104 ros::Time now = ros::Time::now();
00105 if(now < last_update_){
00106 ROS_WARN_STREAM("Detected jump back in time of " << (last_update_ - now).toSec() << "s. Clearing TF buffer.");
00107 buffer_.clear();
00108 }
00109 last_update_ = now;
00110
00111
00112
00113 const tf2_msgs::TFMessage& msg_in = *(msg_evt.getConstMessage());
00114 std::string authority = msg_evt.getPublisherName();
00115 for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
00116 {
00117 try
00118 {
00119 buffer_.setTransform(msg_in.transforms[i], authority, is_static);
00120 }
00121
00122 catch (tf2::TransformException& ex)
00123 {
00125 std::string temp = ex.what();
00126 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());
00127 }
00128 }
00129 };
00130
00131
00132
00133
00134