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 tf2_msgs::TFMessageConstPtr& msg)
00094 {
00095 subscription_callback_impl(msg, false);
00096 }
00097 void TransformListener::static_subscription_callback(const tf2_msgs::TFMessageConstPtr& msg)
00098 {
00099 subscription_callback_impl(msg, true);
00100 }
00101
00102 void TransformListener::subscription_callback_impl(const tf2_msgs::TFMessageConstPtr& msg, bool is_static)
00103 {
00104 ros::Time now = ros::Time::now();
00105 if(now < last_update_){
00106 ROS_WARN("Detected jump back in time. Clearing TF buffer.");
00107 buffer_.clear();
00108 }
00109 last_update_ = now;
00110
00111 const tf2_msgs::TFMessage& msg_in = *msg;
00112 for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
00113 {
00114 std::map<std::string, std::string>* msg_header_map = msg_in.__connection_header.get();
00115 std::string authority;
00116 std::map<std::string, std::string>::iterator it = msg_header_map->find("callerid");
00117 if (it == msg_header_map->end())
00118 {
00119 ROS_WARN("Message recieved without callerid");
00120 authority = "no callerid";
00121 }
00122 else
00123 {
00124 authority = it->second;
00125 }
00126
00127 try
00128 {
00129 buffer_.setTransform(msg_in.transforms[i], authority, is_static);
00130 }
00131
00132 catch (tf2::TransformException& ex)
00133 {
00135 std::string temp = ex.what();
00136 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());
00137 }
00138 }
00139 };
00140
00141
00142
00143
00144