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
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
00116 tf::Quaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
00117 tf::Vector3 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(tf::Transform(bt_orientation,bt_position), time, frame);
00125 tf::Stamped<tf::Pose> pose_out;
00126
00127
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
00229 display->setStatus(status_levels::Error, status_name, status_text);
00230 }
00231
00232 }