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