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


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust, William Woodall
autogenerated on Fri Aug 2 2024 08:43:10