$search
00001 /* 00002 * Copyright (c) 2008, Willow Garage, Inc. 00003 * All rights reserved. 00004 * 00005 * Redistribution and use in source and binary forms, with or without 00006 * modification, are permitted provided that the following conditions are met: 00007 * 00008 * * Redistributions of source code must retain the above copyright 00009 * notice, this list of conditions and the following disclaimer. 00010 * * Redistributions in binary form must reproduce the above copyright 00011 * notice, this list of conditions and the following disclaimer in the 00012 * documentation and/or other materials provided with the distribution. 00013 * * Neither the name of the Willow Garage, Inc. nor the names of its 00014 * contributors may be used to endorse or promote products derived from 00015 * this software without specific prior written permission. 00016 * 00017 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00018 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00019 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00020 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00021 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00022 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00023 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00024 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00025 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00026 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00027 * POSSIBILITY OF SUCH DAMAGE. 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