transform_collection.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 // We'll look at all transforms between t-history_length and t+INC
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   // Form the query
00070   const mongo::Query q = transformQuery(t);
00071 
00072   // Iterate over the messages and add them to a Transformer
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); // Can throw
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); // Can throw
00106   return trans;
00107 }
00108 
00109 } // namespace


warehouse_ros
Author(s): Bhaskara Marthi
autogenerated on Fri Apr 22 2016 04:08:50