Go to the documentation of this file.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
00039 #include <mongo_ros/transform_collection.h>
00040 #include <boost/foreach.hpp>
00041 #include <boost/format.hpp>
00042
00043 namespace mongo_ros
00044 {
00045
00046 namespace gm=geometry_msgs;
00047 using std::string;
00048 using std::vector;
00049 using boost::format;
00050
00051
00052
00053 const double INC=1.0;
00054
00055 mongo::Query TransformCollection::transformQuery (const double t) const
00056 {
00057 format query("{stamp : { $gte : %.9f, $lte : %.9f } }");
00058 const double start = t-history_length_;
00059 const double end = t+INC;
00060 string s = (query % start % end).str();
00061 mongo::Query q = mongo::fromjson(s);
00062 return q;
00063 }
00064
00065 tf::StampedTransform TransformCollection::lookupTransform (const string& target,
00066 const string& src,
00067 const double t) const
00068 {
00069
00070 const mongo::Query q = transformQuery(t);
00071
00072
00073 tf::Transformer buffer(true, ros::Duration(history_length_+INC*1.1));
00074 typedef MessageWithMetadata<tf::tfMessage>::ConstPtr MsgPtr;
00075 BOOST_FOREACH (MsgPtr m, coll_.queryResults(q))
00076 {
00077 BOOST_FOREACH (const gm::TransformStamped& trans, m->transforms)
00078 {
00079 const gm::Vector3& v = trans.transform.translation;
00080 const gm::Quaternion& q = trans.transform.rotation;
00081 const std_msgs::Header& h = trans.header;
00082 const tf::Transform tr(tf::Quaternion(q.x, q.y, q.z, q.w),
00083 tf::Vector3(v.x, v.y, v.z));
00084 const tf::StampedTransform t(tr, h.stamp, h.frame_id,
00085 trans.child_frame_id);
00086 const bool ok = buffer.setTransform(t);
00087 ROS_ASSERT_MSG(ok, "Tf setTransform returned false for transform from %s "
00088 "to %s at %.4f", trans.child_frame_id.c_str(),
00089 h.frame_id.c_str(), h.stamp.toSec());
00090 }
00091 }
00092 tf::StampedTransform result;
00093 buffer.lookupTransform(target, src, ros::Time(t), result);
00094 return result;
00095 }
00096
00097
00098 tf::StampedTransform LiveTransformSource::lookupTransform (const string& target,
00099 const string& src,
00100 const double tm) const
00101 {
00102 ros::Time t(tm);
00103 tf_->waitForTransform(target, src, t, ros::Duration(timeout_));
00104 tf::StampedTransform trans;
00105 tf_->lookupTransform(target, src, t, trans);
00106 return trans;
00107 }
00108
00109 }