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 #include "tf2/time_cache.h"
00033 #include "tf2/exceptions.h"
00034
00035 #include <tf2/LinearMath/Vector3.h>
00036 #include <tf2/LinearMath/Quaternion.h>
00037 #include <tf2/LinearMath/Transform.h>
00038 #include <geometry_msgs/TransformStamped.h>
00039 #include <assert.h>
00040
00041 namespace tf2 {
00042
00043 TransformStorage::TransformStorage()
00044 {
00045 }
00046
00047 TransformStorage::TransformStorage(const geometry_msgs::TransformStamped& data, CompactFrameID frame_id,
00048 CompactFrameID child_frame_id)
00049 : stamp_(data.header.stamp)
00050 , frame_id_(frame_id)
00051 , child_frame_id_(child_frame_id)
00052 {
00053 const geometry_msgs::Quaternion& o = data.transform.rotation;
00054 rotation_ = tf2::Quaternion(o.x, o.y, o.z, o.w);
00055 const geometry_msgs::Vector3& v = data.transform.translation;
00056 translation_ = tf2::Vector3(v.x, v.y, v.z);
00057 }
00058
00059 TimeCache::TimeCache(ros::Duration max_storage_time)
00060 : max_storage_time_(max_storage_time)
00061 {}
00062
00063 namespace cache {
00064
00065 void createExtrapolationException1(ros::Time t0, ros::Time t1, std::string* error_str)
00066 {
00067 if (error_str)
00068 {
00069 std::stringstream ss;
00070 ss << "Lookup would require extrapolation at time " << t0 << ", but only time " << t1 << " is in the buffer";
00071 *error_str = ss.str();
00072 }
00073 }
00074
00075 void createExtrapolationException2(ros::Time t0, ros::Time t1, std::string* error_str)
00076 {
00077 if (error_str)
00078 {
00079 std::stringstream ss;
00080 ss << "Lookup would require extrapolation into the future. Requested time " << t0 << " but the latest data is at time " << t1;
00081 *error_str = ss.str();
00082 }
00083 }
00084
00085 void createExtrapolationException3(ros::Time t0, ros::Time t1, std::string* error_str)
00086 {
00087 if (error_str)
00088 {
00089 std::stringstream ss;
00090 ss << "Lookup would require extrapolation into the past. Requested time " << t0 << " but the earliest data is at time " << t1;
00091 *error_str = ss.str();
00092 }
00093 }
00094 }
00095
00096 uint8_t TimeCache::findClosest(TransformStorage*& one, TransformStorage*& two, ros::Time target_time, std::string* error_str)
00097 {
00098
00099 if (storage_.empty())
00100 {
00101 return 0;
00102 }
00103
00104
00105 if (target_time.isZero())
00106 {
00107 one = &storage_.front();
00108 return 1;
00109 }
00110
00111
00112 if (++storage_.begin() == storage_.end())
00113 {
00114 TransformStorage& ts = *storage_.begin();
00115 if (ts.stamp_ == target_time)
00116 {
00117 one = &ts;
00118 return 1;
00119 }
00120 else
00121 {
00122 cache::createExtrapolationException1(target_time, ts.stamp_, error_str);
00123 return 0;
00124 }
00125 }
00126
00127 ros::Time latest_time = (*storage_.begin()).stamp_;
00128 ros::Time earliest_time = (*(storage_.rbegin())).stamp_;
00129
00130 if (target_time == latest_time)
00131 {
00132 one = &(*storage_.begin());
00133 return 1;
00134 }
00135 else if (target_time == earliest_time)
00136 {
00137 one = &(*storage_.rbegin());
00138 return 1;
00139 }
00140
00141 else if (target_time > latest_time)
00142 {
00143 cache::createExtrapolationException2(target_time, latest_time, error_str);
00144 return 0;
00145 }
00146 else if (target_time < earliest_time)
00147 {
00148 cache::createExtrapolationException3(target_time, earliest_time, error_str);
00149 return 0;
00150 }
00151
00152
00153
00154 L_TransformStorage::iterator storage_it = storage_.begin();
00155 while(storage_it != storage_.end())
00156 {
00157 if (storage_it->stamp_ <= target_time)
00158 break;
00159 storage_it++;
00160 }
00161
00162
00163 one = &*(storage_it);
00164 two = &*(--storage_it);
00165 return 2;
00166
00167
00168 }
00169
00170 void TimeCache::interpolate(const TransformStorage& one, const TransformStorage& two, ros::Time time, TransformStorage& output)
00171 {
00172
00173 if( two.stamp_ == one.stamp_ )
00174 {
00175 output = two;
00176 return;
00177 }
00178
00179 tf2Scalar ratio = (time.toSec() - one.stamp_.toSec()) / (two.stamp_.toSec() - one.stamp_.toSec());
00180
00181
00182 output.translation_.setInterpolate3(one.translation_, two.translation_, ratio);
00183
00184
00185 output.rotation_ = slerp( one.rotation_, two.rotation_, ratio);
00186
00187 output.stamp_ = one.stamp_;
00188 output.frame_id_ = one.frame_id_;
00189 output.child_frame_id_ = one.child_frame_id_;
00190 }
00191
00192 bool TimeCache::getData(ros::Time time, TransformStorage & data_out, std::string* error_str)
00193 {
00194 TransformStorage* p_temp_1;
00195 TransformStorage* p_temp_2;
00196
00197 int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
00198 if (num_nodes == 0)
00199 {
00200 return false;
00201 }
00202 else if (num_nodes == 1)
00203 {
00204 data_out = *p_temp_1;
00205 }
00206 else if (num_nodes == 2)
00207 {
00208 if( p_temp_1->frame_id_ == p_temp_2->frame_id_)
00209 {
00210 interpolate(*p_temp_1, *p_temp_2, time, data_out);
00211 }
00212 else
00213 {
00214 data_out = *p_temp_1;
00215 }
00216 }
00217 else
00218 {
00219 assert(0);
00220 }
00221
00222 return true;
00223 }
00224
00225 CompactFrameID TimeCache::getParent(ros::Time time, std::string* error_str)
00226 {
00227 TransformStorage* p_temp_1;
00228 TransformStorage* p_temp_2;
00229
00230 int num_nodes = findClosest(p_temp_1, p_temp_2, time, error_str);
00231 if (num_nodes == 0)
00232 {
00233 return 0;
00234 }
00235
00236 return p_temp_1->frame_id_;
00237 }
00238
00239 bool TimeCache::insertData(const TransformStorage& new_data)
00240 {
00241 L_TransformStorage::iterator storage_it = storage_.begin();
00242
00243 if(storage_it != storage_.end())
00244 {
00245 if (storage_it->stamp_ > new_data.stamp_ + max_storage_time_)
00246 {
00247 return false;
00248 }
00249 }
00250
00251
00252 while(storage_it != storage_.end())
00253 {
00254 if (storage_it->stamp_ <= new_data.stamp_)
00255 break;
00256 storage_it++;
00257 }
00258 storage_.insert(storage_it, new_data);
00259
00260 pruneList();
00261 return true;
00262 }
00263
00264 void TimeCache::clearList()
00265 {
00266 storage_.clear();
00267 }
00268
00269 unsigned int TimeCache::getListLength()
00270 {
00271 return storage_.size();
00272 }
00273
00274 P_TimeAndFrameID TimeCache::getLatestTimeAndParent()
00275 {
00276 if (storage_.empty())
00277 {
00278 return std::make_pair(ros::Time(), 0);
00279 }
00280
00281 const TransformStorage& ts = storage_.front();
00282 return std::make_pair(ts.stamp_, ts.frame_id_);
00283 }
00284
00285 ros::Time TimeCache::getLatestTimestamp()
00286 {
00287 if (storage_.empty()) return ros::Time();
00288 return storage_.front().stamp_;
00289 }
00290
00291 ros::Time TimeCache::getOldestTimestamp()
00292 {
00293 if (storage_.empty()) return ros::Time();
00294 return storage_.back().stamp_;
00295 }
00296
00297 void TimeCache::pruneList()
00298 {
00299 ros::Time latest_time = storage_.begin()->stamp_;
00300
00301 while(!storage_.empty() && storage_.back().stamp_ + max_storage_time_ < latest_time)
00302 {
00303 storage_.pop_back();
00304 }
00305
00306 }
00307 }