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 "marker_base.h"
00031 #include "rviz/default_plugin/marker_display.h"
00032 #include "rviz/visualization_manager.h"
00033 #include "rviz/selection/selection_manager.h"
00034 #include "marker_selection_handler.h"
00035 #include "rviz/frame_manager.h"
00036
00037 #include <OGRE/OgreSceneNode.h>
00038 #include <OGRE/OgreSceneManager.h>
00039 #include <OGRE/OgreEntity.h>
00040 #include <OGRE/OgreSubEntity.h>
00041
00042 #include <tf/tf.h>
00043 #include <tf/transform_listener.h>
00044
00045 namespace rviz
00046 {
00047
00048 MarkerBase::MarkerBase(MarkerDisplay* owner, VisualizationManager* manager, Ogre::SceneNode* parent_node)
00049 : owner_(owner)
00050 , vis_manager_(manager)
00051 , scene_node_(parent_node->createChildSceneNode())
00052 , coll_(0)
00053 {}
00054
00055 MarkerBase::~MarkerBase()
00056 {
00057 vis_manager_->getSelectionManager()->removeObject(coll_);
00058 vis_manager_->getSceneManager()->destroySceneNode(scene_node_);
00059 }
00060
00061 void MarkerBase::setMessage(const Marker& message)
00062 {
00063
00064 MarkerConstPtr message_ptr( new Marker(message) );
00065 setMessage( message_ptr );
00066 }
00067
00068 void MarkerBase::setMessage(const MarkerConstPtr& message)
00069 {
00070 MarkerConstPtr old = message_;
00071 message_ = message;
00072
00073 expiration_ = ros::Time::now() + message->lifetime;
00074
00075 onNewMessage(old, message);
00076 }
00077
00078 void MarkerBase::updateFrameLocked()
00079 {
00080 ROS_ASSERT(message_ && message_->frame_locked);
00081 onNewMessage(message_, message_);
00082 }
00083
00084 bool MarkerBase::expired()
00085 {
00086 return ros::Time::now() >= expiration_;
00087 }
00088
00089 bool MarkerBase::transform(const MarkerConstPtr& message, Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scale)
00090 {
00091 ros::Time stamp = message->header.stamp;
00092 if (message->frame_locked)
00093 {
00094 stamp = ros::Time();
00095 }
00096
00097 if (!FrameManager::instance()->transform(message->header.frame_id, stamp, message->pose, pos, orient))
00098 {
00099 std::string error;
00100 FrameManager::instance()->transformHasProblems(message->header.frame_id, message->header.stamp, error);
00101 if ( owner_ )
00102 {
00103 owner_->setMarkerStatus(getID(), status_levels::Error, error);
00104 }
00105 return false;
00106 }
00107
00108 scale = Ogre::Vector3(message->scale.x, message->scale.y, message->scale.z);
00109
00110 return true;
00111 }
00112
00113 void MarkerBase::setControl( InteractiveMarkerControl* control )
00114 {
00115 SelectionHandlerPtr handler_ptr = vis_manager_->getSelectionManager()->getHandler( coll_ );
00116 MarkerSelectionHandler* handler = dynamic_cast<MarkerSelectionHandler*>( handler_ptr.get() );
00117 if ( handler )
00118 {
00119 handler->setControl( control );
00120 }
00121 }
00122
00123 void MarkerBase::setPosition( const Ogre::Vector3& position )
00124 {
00125 scene_node_->setPosition( position );
00126 }
00127
00128 void MarkerBase::setOrientation( const Ogre::Quaternion& orientation )
00129 {
00130 scene_node_->setOrientation( orientation );
00131 }
00132
00133 const Ogre::Vector3& MarkerBase::getPosition()
00134 {
00135 return scene_node_->getPosition();
00136 }
00137
00138 const Ogre::Quaternion& MarkerBase::getOrientation()
00139 {
00140 return scene_node_->getOrientation();
00141 }
00142
00143 void MarkerBase::extractMaterials( Ogre::Entity *entity, S_MaterialPtr &materials )
00144 {
00145 uint32_t num_sub_entities = entity->getNumSubEntities();
00146 for (uint32_t i = 0; i < num_sub_entities; ++i)
00147 {
00148 Ogre::SubEntity* sub = entity->getSubEntity(i);
00149 Ogre::MaterialPtr material = sub->getMaterial();
00150 materials.insert( material );
00151 }
00152 }
00153
00154
00155
00156 }