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;
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
00089
00090 void TransformListener::subscription_callback(const tf2_msgs::TFMessageConstPtr& msg)
00091 {
00092 subscription_callback_impl(msg, false);
00093 }
00094 void TransformListener::static_subscription_callback(const tf2_msgs::TFMessageConstPtr& msg)
00095 {
00096 subscription_callback_impl(msg, true);
00097 }
00098
00099 void TransformListener::subscription_callback_impl(const tf2_msgs::TFMessageConstPtr& msg, bool is_static)
00100 {
00101
00102 const tf2_msgs::TFMessage& msg_in = *msg;
00103 for (unsigned int i = 0; i < msg_in.transforms.size(); i++)
00104 {
00105 std::map<std::string, std::string>* msg_header_map = msg_in.__connection_header.get();
00106 std::string authority;
00107 std::map<std::string, std::string>::iterator it = msg_header_map->find("callerid");
00108 if (it == msg_header_map->end())
00109 {
00110 ROS_WARN("Message recieved without callerid");
00111 authority = "no callerid";
00112 }
00113 else
00114 {
00115 authority = it->second;
00116 }
00117
00118 try
00119 {
00120 buffer_.setTransform(msg_in.transforms[i], authority, is_static);
00121 }
00122
00123 catch (TransformException& ex)
00124 {
00126 std::string temp = ex.what();
00127 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());
00128 }
00129 }
00130 };
00131
00132
00133
00134
00135