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