$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 "tf/transform_listener.h" 00033 00034 #include <boost/numeric/ublas/matrix.hpp> 00035 #include <boost/numeric/ublas/io.hpp> 00036 00037 00038 using namespace tf; 00039 std::string tf::remap(const std::string& frame_id) 00040 { 00041 ros::NodeHandle n("~"); 00042 return tf::resolve(getPrefixParam(n), frame_id); 00043 }; 00044 00045 00046 TransformListener::TransformListener(ros::Duration max_cache_time, bool spin_thread): 00047 Transformer(true, max_cache_time), dedicated_listener_thread_(NULL) 00048 { 00049 if (spin_thread) 00050 initWithThread(); 00051 else 00052 init(); 00053 } 00054 00055 TransformListener::TransformListener(const ros::NodeHandle& nh, ros::Duration max_cache_time, bool spin_thread): 00056 Transformer(true, max_cache_time), dedicated_listener_thread_(NULL), node_(nh) 00057 { 00058 if (spin_thread) 00059 initWithThread(); 00060 else 00061 init(); 00062 } 00063 00064 TransformListener::~TransformListener() 00065 { 00066 using_dedicated_thread_ = false; 00067 if (dedicated_listener_thread_) 00068 { 00069 dedicated_listener_thread_->join(); 00070 delete dedicated_listener_thread_; 00071 } 00072 } 00073 00074 //Override Transformer::ok() for ticket:4882 00075 bool TransformListener::ok() const { return ros::ok(); } 00076 00077 void TransformListener::init() 00078 { 00079 message_subscriber_tf_ = node_.subscribe<tf::tfMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1)); 00080 00081 00082 if (! ros::service::exists("~tf_frames", false)) // Avoid doublely advertizing if multiple instances of this library 00083 { 00084 ros::NodeHandle nh("~"); 00085 tf_frames_srv_ = nh.advertiseService("tf_frames", &TransformListener::getFrames, this); 00086 } 00087 00088 ros::NodeHandle local_nh("~"); 00089 00090 tf_prefix_ = getPrefixParam(local_nh); 00091 last_update_ros_time_ = ros::Time::now(); 00092 } 00093 00094 void TransformListener::initWithThread() 00095 { 00096 using_dedicated_thread_ = true; 00097 ros::SubscribeOptions ops_tf = ros::SubscribeOptions::create<tf::tfMessage>("/tf", 100, boost::bind(&TransformListener::subscription_callback, this, _1), ros::VoidPtr(), &tf_message_callback_queue_); 00098 00099 message_subscriber_tf_ = node_.subscribe(ops_tf); 00100 00101 dedicated_listener_thread_ = new boost::thread(boost::bind(&TransformListener::dedicatedListenerThread, this)); 00102 00103 if (! ros::service::exists("~tf_frames", false)) // Avoid doublely advertizing if multiple instances of this library 00104 { 00105 ros::NodeHandle nh("~"); 00106 tf_frames_srv_ = nh.advertiseService("tf_frames", &TransformListener::getFrames, this); 00107 } 00108 00109 ros::NodeHandle local_nh("~"); 00110 tf_prefix_ = getPrefixParam(local_nh); 00111 last_update_ros_time_ = ros::Time::now(); 00112 } 00113 00114 void TransformListener::transformQuaternion(const std::string& target_frame, 00115 const geometry_msgs::QuaternionStamped& msg_in, 00116 geometry_msgs::QuaternionStamped& msg_out) const 00117 { 00118 tf::assertQuaternionValid(msg_in.quaternion); 00119 00120 Stamped<Quaternion> pin, pout; 00121 quaternionStampedMsgToTF(msg_in, pin); 00122 transformQuaternion(target_frame, pin, pout); 00123 quaternionStampedTFToMsg(pout, msg_out); 00124 } 00125 00126 void TransformListener::transformVector(const std::string& target_frame, 00127 const geometry_msgs::Vector3Stamped& msg_in, 00128 geometry_msgs::Vector3Stamped& msg_out) const 00129 { 00130 Stamped<Vector3> pin, pout; 00131 vector3StampedMsgToTF(msg_in, pin); 00132 transformVector(target_frame, pin, pout); 00133 vector3StampedTFToMsg(pout, msg_out); 00134 } 00135 00136 void TransformListener::transformPoint(const std::string& target_frame, 00137 const geometry_msgs::PointStamped& msg_in, 00138 geometry_msgs::PointStamped& msg_out) const 00139 { 00140 Stamped<Point> pin, pout; 00141 pointStampedMsgToTF(msg_in, pin); 00142 transformPoint(target_frame, pin, pout); 00143 pointStampedTFToMsg(pout, msg_out); 00144 } 00145 00146 void TransformListener::transformPose(const std::string& target_frame, 00147 const geometry_msgs::PoseStamped& msg_in, 00148 geometry_msgs::PoseStamped& msg_out) const 00149 { 00150 tf::assertQuaternionValid(msg_in.pose.orientation); 00151 00152 Stamped<Pose> pin, pout; 00153 poseStampedMsgToTF(msg_in, pin); 00154 transformPose(target_frame, pin, pout); 00155 poseStampedTFToMsg(pout, msg_out); 00156 } 00157 /* http://www.ros.org/wiki/tf/Reviews/2010-03-12_API_Review 00158 void TransformListener::transformTwist(const std::string& target_frame, 00159 const geometry_msgs::TwistStamped& msg_in, 00160 geometry_msgs::TwistStamped& msg_out) const 00161 { 00162 tf::Vector3 twist_rot(msg_in.twist.angular.x, 00163 msg_in.twist.angular.y, 00164 msg_in.twist.angular.z); 00165 tf::Vector3 twist_vel(msg_in.twist.linear.x, 00166 msg_in.twist.linear.y, 00167 msg_in.twist.linear.z); 00168 00169 tf::StampedTransform transform; 00170 lookupTransform(target_frame,msg_in.header.frame_id, msg_in.header.stamp, transform); 00171 00172 00173 btVector3 out_rot = transform.getBasis() * twist_rot; 00174 btVector3 out_vel = transform.getBasis()* twist_vel + transform.getOrigin().cross(out_rot); 00175 00176 geometry_msgs::TwistStamped interframe_twist; 00177 lookupVelocity(target_frame, msg_in.header.frame_id, msg_in.header.stamp, ros::Duration(0.1), interframe_twist); //\todo get rid of hard coded number 00178 00179 msg_out.header.stamp = msg_in.header.stamp; 00180 msg_out.header.frame_id = target_frame; 00181 msg_out.twist.linear.x = out_vel.x() + interframe_twist.twist.linear.x; 00182 msg_out.twist.linear.y = out_vel.y() + interframe_twist.twist.linear.y; 00183 msg_out.twist.linear.z = out_vel.z() + interframe_twist.twist.linear.z; 00184 msg_out.twist.angular.x = out_rot.x() + interframe_twist.twist.angular.x; 00185 msg_out.twist.angular.y = out_rot.y() + interframe_twist.twist.angular.y; 00186 msg_out.twist.angular.z = out_rot.z() + interframe_twist.twist.angular.z; 00187 00188 }*/ 00189 00190 void TransformListener::transformQuaternion(const std::string& target_frame, const ros::Time& target_time, 00191 const geometry_msgs::QuaternionStamped& msg_in, 00192 const std::string& fixed_frame, geometry_msgs::QuaternionStamped& msg_out) const 00193 { 00194 tf::assertQuaternionValid(msg_in.quaternion); 00195 Stamped<Quaternion> pin, pout; 00196 quaternionStampedMsgToTF(msg_in, pin); 00197 transformQuaternion(target_frame, target_time, pin, fixed_frame, pout); 00198 quaternionStampedTFToMsg(pout, msg_out); 00199 } 00200 00201 void TransformListener::transformVector(const std::string& target_frame, const ros::Time& target_time, 00202 const geometry_msgs::Vector3Stamped& msg_in, 00203 const std::string& fixed_frame, geometry_msgs::Vector3Stamped& msg_out) const 00204 { 00205 Stamped<Vector3> pin, pout; 00206 vector3StampedMsgToTF(msg_in, pin); 00207 transformVector(target_frame, target_time, pin, fixed_frame, pout); 00208 vector3StampedTFToMsg(pout, msg_out); 00209 } 00210 00211 void TransformListener::transformPoint(const std::string& target_frame, const ros::Time& target_time, 00212 const geometry_msgs::PointStamped& msg_in, 00213 const std::string& fixed_frame, geometry_msgs::PointStamped& msg_out) const 00214 { 00215 Stamped<Point> pin, pout; 00216 pointStampedMsgToTF(msg_in, pin); 00217 transformPoint(target_frame, target_time, pin, fixed_frame, pout); 00218 pointStampedTFToMsg(pout, msg_out); 00219 } 00220 00221 void TransformListener::transformPose(const std::string& target_frame, const ros::Time& target_time, 00222 const geometry_msgs::PoseStamped& msg_in, 00223 const std::string& fixed_frame, geometry_msgs::PoseStamped& msg_out) const 00224 { 00225 tf::assertQuaternionValid(msg_in.pose.orientation); 00226 00227 Stamped<Pose> pin, pout; 00228 poseStampedMsgToTF(msg_in, pin); 00229 transformPose(target_frame, target_time, pin, fixed_frame, pout); 00230 poseStampedTFToMsg(pout, msg_out); 00231 } 00232 00233 void TransformListener::transformPointCloud(const std::string & target_frame, const sensor_msgs::PointCloud & cloudIn, sensor_msgs::PointCloud & cloudOut) const 00234 { 00235 StampedTransform transform; 00236 lookupTransform(target_frame, cloudIn.header.frame_id, cloudIn.header.stamp, transform); 00237 00238 transformPointCloud(target_frame, transform, cloudIn.header.stamp, cloudIn, cloudOut); 00239 } 00240 void TransformListener::transformPointCloud(const std::string& target_frame, const ros::Time& target_time, 00241 const sensor_msgs::PointCloud& cloudIn, 00242 const std::string& fixed_frame, sensor_msgs::PointCloud& cloudOut) const 00243 { 00244 StampedTransform transform; 00245 lookupTransform(target_frame, target_time, 00246 cloudIn.header.frame_id, cloudIn.header.stamp, 00247 fixed_frame, 00248 transform); 00249 00250 transformPointCloud(target_frame, transform, target_time, cloudIn, cloudOut); 00251 00252 00253 } 00254 00255 inline void transformPointMatVec(const tf::Vector3 &origin, const btMatrix3x3 &basis, const geometry_msgs::Point32 &in, geometry_msgs::Point32 &out) 00256 { 00257 // Use temporary variables in case &in == &out 00258 double x = basis[0].x() * in.x + basis[0].y() * in.y + basis[0].z() * in.z + origin.x(); 00259 double y = basis[1].x() * in.x + basis[1].y() * in.y + basis[1].z() * in.z + origin.y(); 00260 double z = basis[2].x() * in.x + basis[2].y() * in.y + basis[2].z() * in.z + origin.z(); 00261 00262 out.x = x; out.y = y; out.z = z; 00263 } 00264 00265 00266 void TransformListener::transformPointCloud(const std::string & target_frame, const tf::Transform& net_transform, 00267 const ros::Time& target_time, const sensor_msgs::PointCloud & cloudIn, 00268 sensor_msgs::PointCloud & cloudOut) const 00269 { 00270 tf::Vector3 origin = net_transform.getOrigin(); 00271 btMatrix3x3 basis = net_transform.getBasis(); 00272 00273 unsigned int length = cloudIn.points.size(); 00274 00275 // Copy relevant data from cloudIn, if needed 00276 if (&cloudIn != &cloudOut) 00277 { 00278 cloudOut.header = cloudIn.header; 00279 cloudOut.points.resize(length); 00280 cloudOut.channels.resize(cloudIn.channels.size()); 00281 for (unsigned int i = 0 ; i < cloudIn.channels.size() ; ++i) 00282 cloudOut.channels[i] = cloudIn.channels[i]; 00283 } 00284 00285 // Transform points 00286 cloudOut.header.stamp = target_time; 00287 cloudOut.header.frame_id = target_frame; 00288 for (unsigned int i = 0; i < length ; i++) { 00289 transformPointMatVec(origin, basis, cloudIn.points[i], cloudOut.points[i]); 00290 } 00291 } 00292 00293 00294 void TransformListener::subscription_callback(const tf::tfMessageConstPtr& msg) 00295 { 00296 ros::Duration ros_diff = ros::Time::now() - last_update_ros_time_; 00297 float ros_dt = ros_diff.toSec(); 00298 00299 if (ros_dt < 0.0) 00300 { 00301 ROS_WARN("Saw a negative time change of %f seconds, clearing the tf buffer.", ros_dt); 00302 clear(); 00303 } 00304 00305 last_update_ros_time_ = ros::Time::now(); 00306 00307 const tf::tfMessage& msg_in = *msg; 00308 for (unsigned int i = 0; i < msg_in.transforms.size(); i++) 00309 { 00310 StampedTransform trans; 00311 transformStampedMsgToTF(msg_in.transforms[i], trans); 00312 try 00313 { 00314 std::map<std::string, std::string>* msg_header_map = msg_in.__connection_header.get(); 00315 std::string authority; 00316 std::map<std::string, std::string>::iterator it = msg_header_map->find("callerid"); 00317 if (it == msg_header_map->end()) 00318 { 00319 ROS_WARN("Message recieved without callerid"); 00320 authority = "no callerid"; 00321 } 00322 else 00323 { 00324 authority = it->second; 00325 } 00326 setTransform(trans, authority); 00327 } 00328 00329 catch (TransformException& ex) 00330 { 00332 std::string temp = ex.what(); 00333 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()); 00334 } 00335 } 00336 }; 00337 00338 00339 00340 00341