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