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/display_context.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, DisplayContext* context, Ogre::SceneNode* parent_node)
00052 : MarkerBase(owner, context, parent_node)
00053 , manual_object_(0)
00054 {
00055 }
00056
00057 TriangleListMarker::~TriangleListMarker()
00058 {
00059 context_->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(), StatusProperty::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_ = context_->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 context_->getSelectionManager()->removeObject(coll_);
00122
00123 SelectionManager* sel_man = context_->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(), StatusProperty::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