tf_link_updater.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2009, 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 #include "tf_link_updater.h"
31 #include "frame_manager.h"
32 
33 #include <tf/tf.h>
34 
35 #include <OgreVector3.h>
36 #include <OgreQuaternion.h>
37 
38 namespace rviz
39 {
40 
41 TFLinkUpdater::TFLinkUpdater(FrameManager* frame_manager, const StatusCallback& status_cb, const std::string& tf_prefix)
42 : frame_manager_(frame_manager)
43 , status_callback_(status_cb)
44 , tf_prefix_(tf_prefix)
45 {
46 }
47 
48 bool TFLinkUpdater::getLinkTransforms(const std::string& _link_name, Ogre::Vector3& visual_position, Ogre::Quaternion& visual_orientation,
49  Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation) const
50 {
51  std::string link_name = _link_name;
52  if (!tf_prefix_.empty())
53  {
54  link_name = tf::resolve(tf_prefix_, link_name);
55  }
56 
57  Ogre::Vector3 position;
58  Ogre::Quaternion orientation;
59  if (!frame_manager_->getTransform(link_name, ros::Time(), position, orientation))
60  {
61  std::stringstream ss;
62  ss << "No transform from [" << link_name << "] to [" << frame_manager_->getFixedFrame() << "]";
63  setLinkStatus(StatusProperty::Error, link_name, ss.str());
64  return false;
65  }
66 
67  setLinkStatus(StatusProperty::Ok, link_name, "Transform OK");
68 
69  // Collision/visual transforms are the same in this case
70  visual_position = position;
71  visual_orientation = orientation;
72  collision_position = position;
73  collision_orientation = orientation;
74 
75  return true;
76 }
77 
78 void TFLinkUpdater::setLinkStatus(StatusLevel level, const std::string& link_name, const std::string& text) const
79 {
80  if (status_callback_)
81  {
82  status_callback_(level, link_name, text);
83  }
84 }
85 
86 }
boost::function< void(StatusLevel, const std::string &, const std::string &)> StatusCallback
std::string resolve(const std::string &prefix, const std::string &frame_name)
std::string tf_prefix_
Helper class for transforming data into Ogre&#39;s world frame (the fixed frame).
Definition: frame_manager.h:63
virtual bool getLinkTransforms(const std::string &link_name, Ogre::Vector3 &visual_position, Ogre::Quaternion &visual_orientation, Ogre::Vector3 &collision_position, Ogre::Quaternion &collision_orientation) const
const std::string & getFixedFrame()
Return the current fixed frame name.
virtual void setLinkStatus(StatusLevel level, const std::string &link_name, const std::string &text) const
StatusCallback status_callback_
bool getTransform(const Header &header, Ogre::Vector3 &position, Ogre::Quaternion &orientation)
Return the pose for a header, relative to the fixed frame, in Ogre classes.
FrameManager * frame_manager_
TFLinkUpdater(FrameManager *frame_manager, const StatusCallback &status_cb=StatusCallback(), const std::string &tf_prefix=std::string())


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Wed Aug 28 2019 04:01:51