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 "triangle_list_marker.h"
00031
00032 #include "marker_selection_handler.h"
00033 #include "rviz/default_plugin/marker_display.h"
00034 #include "rviz/selection/selection_manager.h"
00035 #include "rviz/uniform_string_stream.h"
00036
00037 #include "rviz/visualization_manager.h"
00038 #include "rviz/mesh_loader.h"
00039 #include "marker_display.h"
00040
00041 #include <OGRE/OgreSceneNode.h>
00042 #include <OGRE/OgreSceneManager.h>
00043 #include <OGRE/OgreManualObject.h>
00044 #include <OGRE/OgreMaterialManager.h>
00045 #include <OGRE/OgreTextureManager.h>
00046 #include <OGRE/OgreTechnique.h>
00047
00048 namespace rviz
00049 {
00050
00051 TriangleListMarker::TriangleListMarker(MarkerDisplay* owner, VisualizationManager* manager, Ogre::SceneNode* parent_node)
00052 : MarkerBase(owner, manager, parent_node)
00053 , manual_object_(0)
00054 {
00055 }
00056
00057 TriangleListMarker::~TriangleListMarker()
00058 {
00059 vis_manager_->getSceneManager()->destroyManualObject(manual_object_);
00060
00061 for (size_t i = 0; i < material_->getNumTechniques(); ++i)
00062 {
00063 Ogre::Technique* t = material_->getTechnique(i);
00064
00065
00066 if (t->getSchemeName() == "Pick")
00067 {
00068 Ogre::TextureManager::getSingleton().remove(t->getPass(0)->getTextureUnitState(0)->getTextureName());
00069 }
00070 }
00071
00072 material_->unload();
00073 Ogre::MaterialManager::getSingleton().remove(material_->getName());
00074 }
00075
00076 void TriangleListMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00077 {
00078 ROS_ASSERT(new_message->type == visualization_msgs::Marker::TRIANGLE_LIST);
00079
00080 size_t num_points = new_message->points.size();
00081 if( (num_points % 3) != 0 || num_points == 0 )
00082 {
00083 std::stringstream ss;
00084 if( num_points == 0 )
00085 {
00086 ss << "TriMesh marker [" << getStringID() << "] has no points.";
00087 }
00088 else
00089 {
00090 ss << "TriMesh marker [" << getStringID() << "] has a point count which is not divisible by 3 [" << num_points <<"]";
00091 }
00092 if ( owner_ )
00093 {
00094 owner_->setMarkerStatus(getID(), status_levels::Error, ss.str());
00095 }
00096 ROS_DEBUG("%s", ss.str().c_str());
00097
00098 scene_node_->setVisible( false );
00099 return;
00100 }
00101 else
00102 {
00103 scene_node_->setVisible( true );
00104 }
00105
00106 if (!manual_object_)
00107 {
00108 static uint32_t count = 0;
00109 UniformStringStream ss;
00110 ss << "Triangle List Marker" << count++;
00111 manual_object_ = vis_manager_->getSceneManager()->createManualObject(ss.str());
00112 scene_node_->attachObject(manual_object_);
00113
00114 ss << "Material";
00115 material_name_ = ss.str();
00116 material_ = Ogre::MaterialManager::getSingleton().create( material_name_, ROS_PACKAGE_NAME );
00117 material_->setReceiveShadows(false);
00118 material_->getTechnique(0)->setLightingEnabled(true);
00119 material_->setCullingMode(Ogre::CULL_NONE);
00120
00121 vis_manager_->getSelectionManager()->removeObject(coll_);
00122
00123 SelectionManager* sel_man = vis_manager_->getSelectionManager();
00124 coll_ = sel_man->createHandle();
00125 sel_man->addPickTechnique(coll_, material_);
00126 sel_man->addObject( coll_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id))) );
00127 }
00128
00129 Ogre::Vector3 pos, scale;
00130 Ogre::Quaternion orient;
00131 transform(new_message, pos, orient, scale);
00132
00133 if ( owner_ && (new_message->scale.x * new_message->scale.y * new_message->scale.z == 0.0f) )
00134 {
00135 owner_->setMarkerStatus(getID(), status_levels::Warn, "Scale of 0 in one of x/y/z");
00136 }
00137
00138 setPosition(pos);
00139 setOrientation(orient);
00140 scene_node_->setScale(scale);
00141
00142
00143 if (old_message && num_points == old_message->points.size())
00144 {
00145 manual_object_->beginUpdate(0);
00146 }
00147 else
00148 {
00149 manual_object_->clear();
00150 manual_object_->estimateVertexCount(num_points);
00151 manual_object_->begin(material_name_, Ogre::RenderOperation::OT_TRIANGLE_LIST);
00152 }
00153
00154 bool has_vertex_colors = new_message->colors.size() == num_points;
00155
00156 if (has_vertex_colors)
00157 {
00158 for (size_t i = 0; i < num_points; ++i)
00159 {
00160 manual_object_->position(new_message->points[i].x, new_message->points[i].y, new_message->points[i].z);
00161 manual_object_->colour(new_message->colors[i].r, new_message->colors[i].g, new_message->colors[i].b, new_message->color.a);
00162 }
00163 }
00164 else
00165 {
00166 for (size_t i = 0; i < num_points; ++i)
00167 {
00168 manual_object_->position(new_message->points[i].x, new_message->points[i].y, new_message->points[i].z);
00169 }
00170 }
00171
00172 manual_object_->end();
00173
00174 if (has_vertex_colors)
00175 {
00176 material_->getTechnique(0)->setLightingEnabled(false);
00177 }
00178 else
00179 {
00180 material_->getTechnique(0)->setLightingEnabled(true);
00181 float r,g,b,a;
00182 r = new_message->color.r;
00183 g = new_message->color.g;
00184 b = new_message->color.b;
00185 a = new_message->color.a;
00186 material_->getTechnique(0)->setAmbient( r,g,b );
00187 material_->getTechnique(0)->setDiffuse( 0,0,0,a );
00188 }
00189
00190 if ( new_message->color.a < 0.9998 )
00191 {
00192 material_->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00193 material_->getTechnique(0)->setDepthWriteEnabled( false );
00194 }
00195 else
00196 {
00197 material_->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
00198 material_->getTechnique(0)->setDepthWriteEnabled( true );
00199 }
00200 }
00201
00202 S_MaterialPtr TriangleListMarker::getMaterials()
00203 {
00204 S_MaterialPtr materials;
00205 materials.insert( material_ );
00206 return materials;
00207 }
00208
00209
00210 }
00211