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
00032 #ifndef TF_TF_H
00033 #define TF_TF_H
00034
00035 #include <iostream>
00036 #include <iomanip>
00037 #include <cmath>
00038 #include <vector>
00039 #include <sstream>
00040 #include <map>
00041
00042 #include <tf/exceptions.h>
00043 #include "tf/time_cache.h"
00044 #include <boost/thread/mutex.hpp>
00045 #include <boost/signals.hpp>
00046 #include "geometry_msgs/TwistStamped.h"
00047
00048 namespace tf
00049 {
00051 std::string resolve(const std::string& prefix, const std::string& frame_name);
00052
00054 __attribute__((deprecated)) static inline std::string remap(const std::string& prefix, const std::string& frame_name) { return tf::resolve(prefix, frame_name);} ;
00055
00056 enum ErrorValues { NO_ERROR = 0, LOOKUP_ERROR, CONNECTIVITY_ERROR, EXTRAPOLATION_ERROR};
00057
00061 typedef struct
00062 {
00063 std::vector<TransformStorage > inverseTransforms;
00064 std::vector<TransformStorage > forwardTransforms;
00065 } TransformLists;
00066
00085 class Transformer
00086 {
00087 public:
00088
00089 static const unsigned int MAX_GRAPH_DEPTH = 100UL;
00090 static const double DEFAULT_CACHE_TIME = 10.0;
00091 static const int64_t DEFAULT_MAX_EXTRAPOLATION_DISTANCE = 0ULL;
00092
00093
00099 Transformer(bool interpolating = true,
00100 ros::Duration cache_time_ = ros::Duration(DEFAULT_CACHE_TIME));
00101 virtual ~Transformer(void);
00102
00104 void clear();
00105
00111 bool setTransform(const StampedTransform& transform, const std::string & authority = "default_authority");
00112
00113
00114
00124 void lookupTransform(const std::string& target_frame, const std::string& source_frame,
00125 const ros::Time& time, StampedTransform& transform) const;
00137 void lookupTransform(const std::string& target_frame, const ros::Time& target_time,
00138 const std::string& source_frame, const ros::Time& source_time,
00139 const std::string& fixed_frame, StampedTransform& transform) const;
00140
00161 void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame, const std::string& reference_frame,
00162 const tf::Point & reference_point, const std::string& reference_point_frame,
00163 const ros::Time& time, const ros::Duration& averaging_interval,
00164 geometry_msgs::Twist& twist) const;
00165
00176 void lookupTwist(const std::string& tracking_frame, const std::string& observation_frame,
00177 const ros::Time& time, const ros::Duration& averaging_interval,
00178 geometry_msgs::Twist& twist) const;
00179
00188 bool waitForTransform(const std::string& target_frame, const std::string& source_frame,
00189 const ros::Time& time, const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
00190 std::string* error_msg = NULL) const;
00197 bool canTransform(const std::string& target_frame, const std::string& source_frame,
00198 const ros::Time& time,
00199 std::string* error_msg = NULL) const;
00200
00209 bool canTransform(const std::string& target_frame, const ros::Time& target_time,
00210 const std::string& source_frame, const ros::Time& source_time,
00211 const std::string& fixed_frame,
00212 std::string* error_msg = NULL) const;
00213
00224 bool waitForTransform(const std::string& target_frame, const ros::Time& target_time,
00225 const std::string& source_frame, const ros::Time& source_time,
00226 const std::string& fixed_frame,
00227 const ros::Duration& timeout, const ros::Duration& polling_sleep_duration = ros::Duration(0.01),
00228 std::string* error_msg = NULL) const;
00229
00232 int getLatestCommonTime(const std::string& source, const std::string& dest, ros::Time& time, std::string * error_string) const;
00233
00234
00237 void transformQuaternion(const std::string& target_frame, const Stamped<tf::Quaternion>& stamped_in, Stamped<tf::Quaternion>& stamped_out) const;
00240 void transformVector(const std::string& target_frame, const Stamped<tf::Vector3>& stamped_in, Stamped<tf::Vector3>& stamped_out) const;
00243 void transformPoint(const std::string& target_frame, const Stamped<tf::Point>& stamped_in, Stamped<tf::Point>& stamped_out) const;
00246 void transformPose(const std::string& target_frame, const Stamped<tf::Pose>& stamped_in, Stamped<tf::Pose>& stamped_out) const;
00247
00250 void transformQuaternion(const std::string& target_frame, const ros::Time& target_time,
00251 const Stamped<tf::Quaternion>& stamped_in,
00252 const std::string& fixed_frame,
00253 Stamped<tf::Quaternion>& stamped_out) const;
00256 void transformVector(const std::string& target_frame, const ros::Time& target_time,
00257 const Stamped<tf::Vector3>& stamped_in,
00258 const std::string& fixed_frame,
00259 Stamped<tf::Vector3>& stamped_out) const;
00262 void transformPoint(const std::string& target_frame, const ros::Time& target_time,
00263 const Stamped<tf::Point>& stamped_in,
00264 const std::string& fixed_frame,
00265 Stamped<tf::Point>& stamped_out) const;
00268 void transformPose(const std::string& target_frame, const ros::Time& target_time,
00269 const Stamped<tf::Pose>& stamped_in,
00270 const std::string& fixed_frame,
00271 Stamped<tf::Pose>& stamped_out) const;
00272
00277 std::string chainAsString(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame) const;
00278
00283 void chainAsVector(const std::string & target_frame, ros::Time target_time, const std::string & source_frame, ros::Time source_time, const std::string & fixed_frame, std::vector<std::string>& output) const;
00284
00288 std::string allFramesAsString() const;
00289
00293 std::string allFramesAsDot() const;
00294
00296 void getFrameStrings(std::vector<std::string>& ids) const;
00297
00302 bool getParent(const std::string& frame_id, ros::Time time, std::string& parent) const;
00303
00306 bool frameExists(const std::string& frame_id_str) const;
00307
00311 void setExtrapolationLimit(const ros::Duration& distance);
00312
00314 ros::Duration getCacheLength() { return cache_time;}
00315
00323 boost::signals::connection addTransformsChangedListener(boost::function<void(void)> callback);
00324 void removeTransformsChangedListener(boost::signals::connection c);
00325
00329 std::string getTFPrefix() const { return tf_prefix_;};
00330
00331
00332 void setUsingDedicatedThread(bool value) { using_dedicated_thread_ = value;};
00333
00334 bool isUsingDedicatedThread() { return using_dedicated_thread_;};
00335
00336 protected:
00337
00349
00350
00353 std::vector< TimeCache*> frames_;
00354
00356 boost::mutex frame_mutex_;
00357
00358 std::map<std::string, unsigned int> frameIDs_;
00359 std::map<unsigned int, std::string> frame_authority_;
00360 std::vector<std::string> frameIDs_reverse;
00361
00363 ros::Duration cache_time;
00364
00366 bool interpolating;
00367
00369 ros::Duration max_extrapolation_distance_;
00370
00371
00373 std::string tf_prefix_;
00374
00375 typedef boost::signal<void(void)> TransformsChangedSignal;
00377 TransformsChangedSignal transforms_changed_;
00378 boost::mutex transforms_changed_mutex_;
00379
00380
00381 bool using_dedicated_thread_;
00382
00383 public:
00384
00385 bool fall_back_to_wall_time_;
00386
00387 protected:
00388
00389 virtual bool ok() const;
00390
00392 ros::Time now() const {
00393 if (!fall_back_to_wall_time_)
00394 return ros::Time::now() ;
00395 else {
00396 ros::WallTime wt = ros::WallTime::now();
00397 return ros::Time(wt.sec, wt.nsec);
00398 };
00399 }
00400
00401
00402
00409 TimeCache* getFrame(unsigned int frame_number) const;
00410
00412 unsigned int lookupFrameNumber(const std::string& frameid_str) const
00413 {
00414 unsigned int retval = 0;
00415 boost::mutex::scoped_lock(frame_mutex_);
00416 std::map<std::string, unsigned int>::const_iterator map_it = frameIDs_.find(frameid_str);
00417 if (map_it == frameIDs_.end())
00418 {
00419 std::stringstream ss;
00420 ss << "Frame id " << frameid_str << " does not exist!";
00421 throw tf::LookupException(ss.str());
00422 }
00423 else
00424 retval = map_it->second;
00425 return retval;
00426 };
00427
00429 unsigned int lookupOrInsertFrameNumber(const std::string& frameid_str)
00430 {
00431 unsigned int retval = 0;
00432 boost::mutex::scoped_lock(frame_mutex_);
00433 std::map<std::string, unsigned int>::iterator map_it = frameIDs_.find(frameid_str);
00434 if (map_it == frameIDs_.end())
00435 {
00436 retval = frames_.size();
00437 frameIDs_[frameid_str] = retval;
00438 frames_.push_back( new TimeCache(interpolating, cache_time, max_extrapolation_distance_));
00439 frameIDs_reverse.push_back(frameid_str);
00440 }
00441 else
00442 retval = frameIDs_[frameid_str];
00443 return retval;
00444 };
00446 std::string lookupFrameString(unsigned int frame_id_num) const
00447 {
00448 if (frame_id_num >= frameIDs_reverse.size())
00449 {
00450 std::stringstream ss;
00451 ss << "Reverse lookup of frame id " << frame_id_num << " failed!";
00452 throw LookupException(ss.str());
00453 }
00454 else
00455 return frameIDs_reverse[frame_id_num];
00456
00457 };
00458
00459
00461 int lookupLists(unsigned int target_frame, ros::Time time, unsigned int source_frame, TransformLists & lists, std::string* error_string) const;
00462
00463 bool test_extrapolation_one_value(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const;
00464 bool test_extrapolation_past(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const;
00465 bool test_extrapolation_future(const ros::Time& target_time, const TransformStorage& tr, std::string* error_string) const;
00466 bool test_extrapolation(const ros::Time& target_time, const TransformLists& t_lists, std::string * error_string) const;
00467
00469 btTransform computeTransformFromList(const TransformLists & list) const;
00470
00471 };
00472
00473
00475 inline void assertQuaternionValid(const tf::Quaternion & q)
00476 {
00477 if(std::fabs(q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() - 1) > 0.01)
00478 {
00479 std::stringstream ss;
00480 ss << "Quaternion malformed, magnitude: " << q.x()*q.x() + q.y()*q.y() + q.z()*q.z() + q.w()*q.w() << " should be 1.0" <<std::endl;
00481 throw tf::InvalidArgument(ss.str());
00482 }
00483 };
00484
00486 inline void assertQuaternionValid(const geometry_msgs::Quaternion & q)
00487 {
00488 if(std::fabs(q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w - 1) > 0.01)
00489 {
00490 std::stringstream ss;
00491 ss << "Quaternion malformed, magnitude: " << q.x*q.x + q.y*q.y + q.z*q.z + q.w*q.w << " should be 1.0" <<std::endl;
00492 throw tf::InvalidArgument(ss.str());
00493 }
00494 };
00495 }
00496 #endif //TF_TF_H