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/display_context.h"
00038 #include "rviz/mesh_loader.h"
00039 #include "marker_display.h"
00040
00041 #include <OgreSceneNode.h>
00042 #include <OgreSceneManager.h>
00043 #include <OgreManualObject.h>
00044 #include <OgreMaterialManager.h>
00045 #include <OgreTextureManager.h>
00046 #include <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 material_->unload();
00061 Ogre::MaterialManager::getSingleton().remove(material_->getName());
00062 }
00063
00064 void TriangleListMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00065 {
00066 ROS_ASSERT(new_message->type == visualization_msgs::Marker::TRIANGLE_LIST);
00067
00068 size_t num_points = new_message->points.size();
00069 if( (num_points % 3) != 0 || num_points == 0 )
00070 {
00071 std::stringstream ss;
00072 if( num_points == 0 )
00073 {
00074 ss << "TriMesh marker [" << getStringID() << "] has no points.";
00075 }
00076 else
00077 {
00078 ss << "TriMesh marker [" << getStringID() << "] has a point count which is not divisible by 3 [" << num_points <<"]";
00079 }
00080 if ( owner_ )
00081 {
00082 owner_->setMarkerStatus(getID(), StatusProperty::Error, ss.str());
00083 }
00084 ROS_DEBUG("%s", ss.str().c_str());
00085
00086 scene_node_->setVisible( false );
00087 return;
00088 }
00089 else
00090 {
00091 scene_node_->setVisible( true );
00092 }
00093
00094 if (!manual_object_)
00095 {
00096 static uint32_t count = 0;
00097 UniformStringStream ss;
00098 ss << "Triangle List Marker" << count++;
00099 manual_object_ = context_->getSceneManager()->createManualObject(ss.str());
00100 scene_node_->attachObject(manual_object_);
00101
00102 ss << "Material";
00103 material_name_ = ss.str();
00104 material_ = Ogre::MaterialManager::getSingleton().create( material_name_, ROS_PACKAGE_NAME );
00105 material_->setReceiveShadows(false);
00106 material_->getTechnique(0)->setLightingEnabled(true);
00107 material_->setCullingMode(Ogre::CULL_NONE);
00108
00109 handler_.reset( new MarkerSelectionHandler( this, MarkerID( new_message->ns, new_message->id ), context_ ));
00110 }
00111
00112 Ogre::Vector3 pos, scale;
00113 Ogre::Quaternion orient;
00114 if (!transform(new_message, pos, orient, scale))
00115 {
00116 ROS_DEBUG("Unable to transform marker message");
00117 scene_node_->setVisible( false );
00118 return;
00119 }
00120
00121 if ( owner_ && (new_message->scale.x * new_message->scale.y * new_message->scale.z == 0.0f) )
00122 {
00123 owner_->setMarkerStatus(getID(), StatusProperty::Warn, "Scale of 0 in one of x/y/z");
00124 }
00125
00126 setPosition(pos);
00127 setOrientation(orient);
00128 scene_node_->setScale(scale);
00129
00130
00131 if (old_message && num_points == old_message->points.size())
00132 {
00133 manual_object_->beginUpdate(0);
00134 }
00135 else
00136 {
00137 manual_object_->clear();
00138 manual_object_->estimateVertexCount(num_points);
00139 manual_object_->begin(material_name_, Ogre::RenderOperation::OT_TRIANGLE_LIST);
00140 }
00141
00142 bool has_vertex_colors = new_message->colors.size() == num_points;
00143 bool has_face_colors = new_message->colors.size() == num_points / 3;
00144 bool any_vertex_has_alpha = false;
00145
00146 if (has_vertex_colors)
00147 {
00148 for (size_t i = 0; i < num_points; ++i)
00149 {
00150 manual_object_->position(new_message->points[i].x, new_message->points[i].y, new_message->points[i].z);
00151 any_vertex_has_alpha = any_vertex_has_alpha || (new_message->colors[i].a < 0.9998);
00152 manual_object_->colour(new_message->colors[i].r, new_message->colors[i].g, new_message->colors[i].b, new_message->color.a * new_message->colors[i].a);
00153 }
00154 }
00155 else if (has_face_colors)
00156 {
00157 for (size_t i = 0; i < num_points; ++i)
00158 {
00159 manual_object_->position(new_message->points[i].x, new_message->points[i].y, new_message->points[i].z);
00160 any_vertex_has_alpha = any_vertex_has_alpha || (new_message->colors[i/3].a < 0.9998);
00161 manual_object_->colour(new_message->colors[i/3].r, new_message->colors[i/3].g, new_message->colors[i/3].b, new_message->color.a * new_message->colors[i/3].a);
00162 }
00163 }
00164 else
00165 {
00166 for (size_t i = 0; i < num_points/3; ++i)
00167 {
00168 Ogre::Vector3 a(new_message->points[3*i].x, new_message->points[3*i].y, new_message->points[3*i].z);
00169 Ogre::Vector3 b(new_message->points[3*i+1].x, new_message->points[3*i+1].y, new_message->points[3*i+1].z);
00170 Ogre::Vector3 c(new_message->points[3*i+2].x, new_message->points[3*i+2].y, new_message->points[3*i+2].z);
00171
00172 Ogre::Vector3 normal = (b-a).crossProduct(c-a);
00173 normal.normalise();
00174
00175 manual_object_->position(a);
00176 manual_object_->normal(normal);
00177
00178 manual_object_->position(b);
00179 manual_object_->normal(normal);
00180
00181 manual_object_->position(c);
00182 manual_object_->normal(normal);
00183 }
00184 }
00185
00186 manual_object_->end();
00187
00188 if (has_vertex_colors || has_face_colors)
00189 {
00190 material_->getTechnique(0)->setLightingEnabled(false);
00191 }
00192 else
00193 {
00194 material_->getTechnique(0)->setLightingEnabled(true);
00195 float r,g,b,a;
00196 r = new_message->color.r;
00197 g = new_message->color.g;
00198 b = new_message->color.b;
00199 a = new_message->color.a;
00200 material_->getTechnique(0)->setAmbient( .5*r,.5*g,.5*b );
00201 material_->getTechnique(0)->setDiffuse( r,g,b,a );
00202 }
00203
00204 if( (!has_vertex_colors && new_message->color.a < 0.9998) || (has_vertex_colors && any_vertex_has_alpha))
00205 {
00206 material_->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00207 material_->getTechnique(0)->setDepthWriteEnabled( false );
00208 }
00209 else
00210 {
00211 material_->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
00212 material_->getTechnique(0)->setDepthWriteEnabled( true );
00213 }
00214
00215 handler_->addTrackedObject( manual_object_ );
00216 }
00217
00218 S_MaterialPtr TriangleListMarker::getMaterials()
00219 {
00220 S_MaterialPtr materials;
00221 materials.insert( material_ );
00222 return materials;
00223 }
00224
00225
00226 }
00227