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 #include "tf_link_updater.h"
00031 #include "frame_manager.h"
00032
00033 #include <tf/tf.h>
00034
00035 #include <OgreVector3.h>
00036 #include <OgreQuaternion.h>
00037
00038 namespace rviz
00039 {
00040
00041 TFLinkUpdater::TFLinkUpdater(FrameManager* frame_manager, const StatusCallback& status_cb, const std::string& tf_prefix)
00042 : frame_manager_(frame_manager)
00043 , status_callback_(status_cb)
00044 , tf_prefix_(tf_prefix)
00045 {
00046 }
00047
00048 bool TFLinkUpdater::getLinkTransforms(const std::string& _link_name, Ogre::Vector3& visual_position, Ogre::Quaternion& visual_orientation,
00049 Ogre::Vector3& collision_position, Ogre::Quaternion& collision_orientation) const
00050 {
00051 std::string link_name = _link_name;
00052 if (!tf_prefix_.empty())
00053 {
00054 link_name = tf::resolve(tf_prefix_, link_name);
00055 }
00056
00057 Ogre::Vector3 position;
00058 Ogre::Quaternion orientation;
00059 if (!frame_manager_->getTransform(link_name, ros::Time(), position, orientation))
00060 {
00061 std::stringstream ss;
00062 ss << "No transform from [" << link_name << "] to [" << frame_manager_->getFixedFrame() << "]";
00063 setLinkStatus(StatusProperty::Error, link_name, ss.str());
00064 return false;
00065 }
00066
00067 setLinkStatus(StatusProperty::Ok, link_name, "Transform OK");
00068
00069
00070 visual_position = position;
00071 visual_orientation = orientation;
00072 collision_position = position;
00073 collision_orientation = orientation;
00074
00075 return true;
00076 }
00077
00078 void TFLinkUpdater::setLinkStatus(StatusLevel level, const std::string& link_name, const std::string& text) const
00079 {
00080 if (status_callback_)
00081 {
00082 status_callback_(level, link_name, text);
00083 }
00084 }
00085
00086 }