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::setInteractiveObject( InteractiveObjectWPtr control )
00114 {
00115 SelectionHandlerPtr handler = vis_manager_->getSelectionManager()->getHandler( coll_ );
00116 if( handler )
00117 {
00118 handler->setInteractiveObject( control );
00119 }
00120 }
00121
00122 void MarkerBase::setPosition( const Ogre::Vector3& position )
00123 {
00124 scene_node_->setPosition( position );
00125 }
00126
00127 void MarkerBase::setOrientation( const Ogre::Quaternion& orientation )
00128 {
00129 scene_node_->setOrientation( orientation );
00130 }
00131
00132 const Ogre::Vector3& MarkerBase::getPosition()
00133 {
00134 return scene_node_->getPosition();
00135 }
00136
00137 const Ogre::Quaternion& MarkerBase::getOrientation()
00138 {
00139 return scene_node_->getOrientation();
00140 }
00141
00142 void MarkerBase::extractMaterials( Ogre::Entity *entity, S_MaterialPtr &materials )
00143 {
00144 uint32_t num_sub_entities = entity->getNumSubEntities();
00145 for (uint32_t i = 0; i < num_sub_entities; ++i)
00146 {
00147 Ogre::SubEntity* sub = entity->getSubEntity(i);
00148 Ogre::MaterialPtr material = sub->getMaterial();
00149 materials.insert( material );
00150 }
00151 }
00152
00153
00154
00155 }