transform_collection.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the Willow Garage, Inc. nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  *
29  */
30 
40 #include <boost/foreach.hpp>
41 
42 namespace warehouse_ros
43 {
44 tf::StampedTransform TransformCollection::lookupTransform(const std::string& target, const std::string& src,
45  const double t) const
46 {
47  // Query all transforms between t-search_back_ and t+search_forward_
49  q->appendRangeInclusive("stamp", t - search_back_, t + search_forward_);
50 
51  // Iterate over the messages and add them to a Transformer
54  for (ResultIterator<tf::tfMessage> it = res.first; it != res.second; ++it)
55  {
56  BOOST_FOREACH (const geometry_msgs::TransformStamped& trans, (*it)->transforms)
57  {
58  const geometry_msgs::Vector3& v = trans.transform.translation;
59  const geometry_msgs::Quaternion& q = trans.transform.rotation;
60  const std_msgs::Header& h = trans.header;
61  const tf::Transform tr(tf::Quaternion(q.x, q.y, q.z, q.w), tf::Vector3(v.x, v.y, v.z));
62  const tf::StampedTransform t(tr, h.stamp, h.frame_id, trans.child_frame_id);
63  const bool ok = buffer.setTransform(t);
64  (void)ok;
65  ROS_ASSERT_MSG(ok, "Tf setTransform returned false for transform from %s to %s at %.4f",
66  trans.child_frame_id.c_str(), h.frame_id.c_str(), h.stamp.toSec());
67  }
68  }
69  tf::StampedTransform result;
70  buffer.lookupTransform(target, src, ros::Time(t), result); // Can throw
71  return result;
72 }
73 
75 {
76 }
77 
78 } // namespace warehouse_ros
QueryResults< M >::range_t query(Query::ConstPtr query, bool metadata_only=false, const std::string &sort_by="", bool ascending=true) const
bool setTransform(const StampedTransform &transform, const std::string &authority="default_authority")
std::pair< ResultIterator< M >, ResultIterator< M > > range_t
Definition: query_results.h:94
void putTransform(tf::StampedTransform)
Put the transform into the collection.
#define ROS_ASSERT_MSG(cond,...)
void lookupTransform(const std::string &target_frame, const std::string &source_frame, const ros::Time &time, StampedTransform &transform) const
tf::StampedTransform lookupTransform(const std::string &target_frame, const std::string &source_frame, double t) const override
MessageCollection< tf::tfMessage > coll_


warehouse_ros
Author(s): Bhaskara Marthi , Connor Brew
autogenerated on Mon Apr 26 2021 02:23:26