$search
00001 /* 00002 * Copyright (c) 2009, 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 00030 #include "frame_manager.h" 00031 #include "display.h" 00032 #include "properties/property.h" 00033 00034 #include <tf/transform_listener.h> 00035 #include <ros/ros.h> 00036 00037 namespace rviz 00038 { 00039 00040 FrameManagerPtr FrameManager::instance() 00041 { 00042 static FrameManagerWPtr instw; 00043 00044 FrameManagerPtr inst = instw.lock(); 00045 if (!inst) 00046 { 00047 inst.reset(new FrameManager); 00048 instw = inst; 00049 } 00050 00051 return inst; 00052 } 00053 00054 FrameManager::FrameManager() 00055 { 00056 tf_ = new tf::TransformListener(ros::NodeHandle(), ros::Duration(10*60), false); 00057 } 00058 00059 FrameManager::~FrameManager() 00060 { 00061 delete tf_; 00062 } 00063 00064 void FrameManager::update() 00065 { 00066 boost::mutex::scoped_lock lock(cache_mutex_); 00067 cache_.clear(); 00068 } 00069 00070 void FrameManager::setFixedFrame(const std::string& frame) 00071 { 00072 boost::mutex::scoped_lock lock(cache_mutex_); 00073 fixed_frame_ = frame; 00074 cache_.clear(); 00075 } 00076 00077 bool FrameManager::getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation) 00078 { 00079 boost::mutex::scoped_lock lock(cache_mutex_); 00080 00081 position = Ogre::Vector3(9999999, 9999999, 9999999); 00082 orientation = Ogre::Quaternion::IDENTITY; 00083 00084 if (fixed_frame_.empty()) 00085 { 00086 return false; 00087 } 00088 00089 M_Cache::iterator it = cache_.find(CacheKey(frame, time)); 00090 if (it != cache_.end()) 00091 { 00092 position = it->second.position; 00093 orientation = it->second.orientation; 00094 return true; 00095 } 00096 00097 geometry_msgs::Pose pose; 00098 pose.orientation.w = 1.0f; 00099 00100 if (!transform(frame, time, pose, position, orientation)) 00101 { 00102 return false; 00103 } 00104 00105 cache_.insert(std::make_pair(CacheKey(frame, time), CacheEntry(position, orientation))); 00106 00107 return true; 00108 } 00109 00110 bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, Ogre::Vector3& position, Ogre::Quaternion& orientation) 00111 { 00112 position = Ogre::Vector3::ZERO; 00113 orientation = Ogre::Quaternion::IDENTITY; 00114 00115 // put all pose data into a tf stamped pose 00116 btQuaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w); 00117 btVector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z); 00118 00119 if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0) 00120 { 00121 bt_orientation.setW(1.0); 00122 } 00123 00124 tf::Stamped<tf::Pose> pose_in(btTransform(bt_orientation,bt_position), time, frame); 00125 tf::Stamped<tf::Pose> pose_out; 00126 00127 // convert pose into new frame 00128 try 00129 { 00130 tf_->transformPose( fixed_frame_, pose_in, pose_out ); 00131 } 00132 catch(tf::TransformException& e) 00133 { 00134 ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what()); 00135 return false; 00136 } 00137 00138 bt_position = pose_out.getOrigin(); 00139 position = Ogre::Vector3(bt_position.x(), bt_position.y(), bt_position.z()); 00140 00141 bt_orientation = pose_out.getRotation(); 00142 orientation = Ogre::Quaternion( bt_orientation.w(), bt_orientation.x(), bt_orientation.y(), bt_orientation.z() ); 00143 00144 return true; 00145 } 00146 00147 bool FrameManager::frameHasProblems(const std::string& frame, ros::Time time, std::string& error) 00148 { 00149 if (!tf_->frameExists(frame)) 00150 { 00151 error = "Frame [" + frame + "] does not exist"; 00152 if (frame == fixed_frame_) 00153 { 00154 error = "Fixed " + error; 00155 } 00156 return true; 00157 } 00158 00159 return false; 00160 } 00161 00162 bool FrameManager::transformHasProblems(const std::string& frame, ros::Time time, std::string& error) 00163 { 00164 std::string tf_error; 00165 bool transform_succeeded = tf_->canTransform(fixed_frame_, frame, time, &tf_error); 00166 if (transform_succeeded) 00167 { 00168 return false; 00169 } 00170 00171 bool ok = true; 00172 ok = ok && !frameHasProblems(fixed_frame_, time, error); 00173 ok = ok && !frameHasProblems(frame, time, error); 00174 00175 if (ok) 00176 { 00177 std::stringstream ss; 00178 ss << "No transform to fixed frame [" << fixed_frame_ << "]. TF error: [" << tf_error << "]"; 00179 error = ss.str(); 00180 ok = false; 00181 } 00182 00183 { 00184 std::stringstream ss; 00185 ss << "For frame [" << frame << "]: " << error; 00186 error = ss.str(); 00187 } 00188 00189 return !ok; 00190 } 00191 00192 std::string getTransformStatusName(const std::string& caller_id) 00193 { 00194 std::stringstream ss; 00195 ss << "Transform [sender=" << caller_id << "]"; 00196 return ss.str(); 00197 } 00198 00199 std::string FrameManager::discoverFailureReason(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason) 00200 { 00201 if (reason == tf::filter_failure_reasons::OutTheBack) 00202 { 00203 std::stringstream ss; 00204 ss << "Message removed because it is too old (frame=[" << frame_id << "], stamp=[" << stamp << "])"; 00205 return ss.str(); 00206 } 00207 else 00208 { 00209 std::string error; 00210 if (transformHasProblems(frame_id, stamp, error)) 00211 { 00212 return error; 00213 } 00214 } 00215 00216 return "Unknown reason for transform failure"; 00217 } 00218 00219 void FrameManager::messageArrived(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, Display* display) 00220 { 00221 display->setStatus(status_levels::Ok, getTransformStatusName(caller_id), "Transform OK"); 00222 } 00223 00224 void FrameManager::messageFailed(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason, Display* display) 00225 { 00226 std::string status_name = getTransformStatusName(caller_id); 00227 std::string status_text = discoverFailureReason(frame_id, stamp, caller_id, reason); 00228 display->setStatus(status_levels::Error, status_name, status_text); 00229 } 00230 00231 }