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 #include <std_msgs/Float32.h>
00038
00039 namespace rviz
00040 {
00041
00042 FrameManager::FrameManager()
00043 {
00044 tf_.reset(new tf::TransformListener(ros::NodeHandle(), ros::Duration(10*60), true));
00045 setSyncMode( SyncOff );
00046 setPause(false);
00047 }
00048
00049 FrameManager::~FrameManager()
00050 {
00051 }
00052
00053 void FrameManager::update()
00054 {
00055 boost::mutex::scoped_lock lock(cache_mutex_);
00056 if ( !pause_ )
00057 {
00058 cache_.clear();
00059 }
00060
00061 if ( !pause_ )
00062 {
00063 switch ( sync_mode_ )
00064 {
00065 case SyncOff:
00066 sync_time_ = ros::Time::now();
00067 break;
00068 case SyncExact:
00069 break;
00070 case SyncApprox:
00071
00072 current_delta_ = 0.7*current_delta_ + 0.3*sync_delta_;
00073 try
00074 {
00075 sync_time_ = ros::Time::now()-ros::Duration(current_delta_);
00076 }
00077 catch (...)
00078 {
00079 sync_time_ = ros::Time::now();
00080 }
00081 break;
00082 }
00083 }
00084 }
00085
00086 void FrameManager::setFixedFrame(const std::string& frame)
00087 {
00088 bool emit = false;
00089 {
00090 boost::mutex::scoped_lock lock(cache_mutex_);
00091 if( fixed_frame_ != frame )
00092 {
00093 fixed_frame_ = frame;
00094 cache_.clear();
00095 emit = true;
00096 }
00097 }
00098 if( emit )
00099 {
00100
00101 Q_EMIT fixedFrameChanged();
00102 }
00103 }
00104
00105 void FrameManager::setPause( bool pause )
00106 {
00107 pause_ = pause;
00108 }
00109
00110 void FrameManager::setSyncMode( SyncMode mode )
00111 {
00112 sync_mode_ = mode;
00113 sync_time_ = ros::Time(0);
00114 current_delta_ = 0;
00115 sync_delta_ = 0;
00116 }
00117
00118 void FrameManager::syncTime( ros::Time time )
00119 {
00120 switch ( sync_mode_ )
00121 {
00122 case SyncOff:
00123 break;
00124 case SyncExact:
00125 sync_time_ = time;
00126 break;
00127 case SyncApprox:
00128 if ( time == ros::Time(0) )
00129 {
00130 sync_delta_ = 0;
00131 return;
00132 }
00133
00134 if ( ros::Time::now() >= time )
00135 {
00136 sync_delta_ = (ros::Time::now() - time).toSec();
00137 }
00138 else
00139 {
00140 setSyncMode( SyncApprox );
00141 }
00142 break;
00143 }
00144 }
00145
00146 bool FrameManager::adjustTime( const std::string &frame, ros::Time& time )
00147 {
00148
00149 if ( time != ros::Time() )
00150 {
00151 return true;
00152 }
00153
00154 switch ( sync_mode_ )
00155 {
00156 case SyncOff:
00157 break;
00158 case SyncExact:
00159 time = sync_time_;
00160 break;
00161 case SyncApprox:
00162 {
00163
00164 ros::Time latest_time;
00165 std::string error_string;
00166 int error_code;
00167 error_code = tf_->getLatestCommonTime( fixed_frame_, frame, latest_time, &error_string );
00168
00169 if ( error_code != 0 )
00170 {
00171 ROS_ERROR("Error getting latest time from frame '%s' to frame '%s': %s (Error code: %d)", frame.c_str(), fixed_frame_.c_str(), error_string.c_str(), error_code);
00172 return false;
00173 }
00174
00175 if ( latest_time > sync_time_ )
00176 {
00177 time = sync_time_;
00178 }
00179 }
00180 break;
00181 }
00182 return true;
00183 }
00184
00185
00186
00187 bool FrameManager::getTransform(const std::string& frame, ros::Time time, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00188 {
00189 if ( !adjustTime(frame, time) )
00190 {
00191 return false;
00192 }
00193
00194 boost::mutex::scoped_lock lock(cache_mutex_);
00195
00196 position = Ogre::Vector3(9999999, 9999999, 9999999);
00197 orientation = Ogre::Quaternion::IDENTITY;
00198
00199 if (fixed_frame_.empty())
00200 {
00201 return false;
00202 }
00203
00204 M_Cache::iterator it = cache_.find(CacheKey(frame, time));
00205 if (it != cache_.end())
00206 {
00207 position = it->second.position;
00208 orientation = it->second.orientation;
00209 return true;
00210 }
00211
00212 geometry_msgs::Pose pose;
00213 pose.orientation.w = 1.0f;
00214
00215 if (!transform(frame, time, pose, position, orientation))
00216 {
00217 return false;
00218 }
00219
00220 cache_.insert(std::make_pair(CacheKey(frame, time), CacheEntry(position, orientation)));
00221
00222 return true;
00223 }
00224
00225 bool FrameManager::transform(const std::string& frame, ros::Time time, const geometry_msgs::Pose& pose_msg, Ogre::Vector3& position, Ogre::Quaternion& orientation)
00226 {
00227 if ( !adjustTime(frame, time) )
00228 {
00229 return false;
00230 }
00231
00232 position = Ogre::Vector3::ZERO;
00233 orientation = Ogre::Quaternion::IDENTITY;
00234
00235
00236 tf::Quaternion bt_orientation(pose_msg.orientation.x, pose_msg.orientation.y, pose_msg.orientation.z, pose_msg.orientation.w);
00237 tf::Vector3 bt_position(pose_msg.position.x, pose_msg.position.y, pose_msg.position.z);
00238
00239 if (bt_orientation.x() == 0.0 && bt_orientation.y() == 0.0 && bt_orientation.z() == 0.0 && bt_orientation.w() == 0.0)
00240 {
00241 bt_orientation.setW(1.0);
00242 }
00243
00244 tf::Stamped<tf::Pose> pose_in(tf::Transform(bt_orientation,bt_position), time, frame);
00245 tf::Stamped<tf::Pose> pose_out;
00246
00247
00248 try
00249 {
00250 tf_->transformPose( fixed_frame_, pose_in, pose_out );
00251 }
00252 catch(std::runtime_error& e)
00253 {
00254 ROS_DEBUG("Error transforming from frame '%s' to frame '%s': %s", frame.c_str(), fixed_frame_.c_str(), e.what());
00255 return false;
00256 }
00257
00258 bt_position = pose_out.getOrigin();
00259 position = Ogre::Vector3(bt_position.x(), bt_position.y(), bt_position.z());
00260
00261 bt_orientation = pose_out.getRotation();
00262 orientation = Ogre::Quaternion( bt_orientation.w(), bt_orientation.x(), bt_orientation.y(), bt_orientation.z() );
00263
00264 return true;
00265 }
00266
00267 bool FrameManager::frameHasProblems(const std::string& frame, ros::Time time, std::string& error)
00268 {
00269 if (!tf_->frameExists(frame))
00270 {
00271 error = "Frame [" + frame + "] does not exist";
00272 if (frame == fixed_frame_)
00273 {
00274 error = "Fixed " + error;
00275 }
00276 return true;
00277 }
00278
00279 return false;
00280 }
00281
00282 bool FrameManager::transformHasProblems(const std::string& frame, ros::Time time, std::string& error)
00283 {
00284 if ( !adjustTime(frame, time) )
00285 {
00286 return false;
00287 }
00288
00289 std::string tf_error;
00290 bool transform_succeeded = tf_->canTransform(fixed_frame_, frame, time, &tf_error);
00291 if (transform_succeeded)
00292 {
00293 return false;
00294 }
00295
00296 bool ok = true;
00297 ok = ok && !frameHasProblems(fixed_frame_, time, error);
00298 ok = ok && !frameHasProblems(frame, time, error);
00299
00300 if (ok)
00301 {
00302 std::stringstream ss;
00303 ss << "No transform to fixed frame [" << fixed_frame_ << "]. TF error: [" << tf_error << "]";
00304 error = ss.str();
00305 ok = false;
00306 }
00307
00308 {
00309 std::stringstream ss;
00310 ss << "For frame [" << frame << "]: " << error;
00311 error = ss.str();
00312 }
00313
00314 return !ok;
00315 }
00316
00317 std::string getTransformStatusName(const std::string& caller_id)
00318 {
00319 std::stringstream ss;
00320 ss << "Transform [sender=" << caller_id << "]";
00321 return ss.str();
00322 }
00323
00324 std::string FrameManager::discoverFailureReason(const std::string& frame_id, const ros::Time& stamp, const std::string& caller_id, tf::FilterFailureReason reason)
00325 {
00326 if (reason == tf::filter_failure_reasons::OutTheBack)
00327 {
00328 std::stringstream ss;
00329 ss << "Message removed because it is too old (frame=[" << frame_id << "], stamp=[" << stamp << "])";
00330 return ss.str();
00331 }
00332 else
00333 {
00334 std::string error;
00335 if (transformHasProblems(frame_id, stamp, error))
00336 {
00337 return error;
00338 }
00339 }
00340
00341 return "Unknown reason for transform failure";
00342 }
00343
00344 void FrameManager::messageArrived( const std::string& frame_id, const ros::Time& stamp,
00345 const std::string& caller_id, Display* display )
00346 {
00347 display->setStatusStd( StatusProperty::Ok, getTransformStatusName( caller_id ), "Transform OK" );
00348 }
00349
00350 void FrameManager::messageFailed( const std::string& frame_id, const ros::Time& stamp,
00351 const std::string& caller_id, tf::FilterFailureReason reason, Display* display )
00352 {
00353 std::string status_name = getTransformStatusName( caller_id );
00354 std::string status_text = discoverFailureReason( frame_id, stamp, caller_id, reason );
00355
00356 display->setStatusStd(StatusProperty::Error, status_name, status_text );
00357 }
00358
00359 }