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 <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 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].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; ++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 || has_face_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( (!has_vertex_colors && new_message->color.a < 0.9998) || (has_vertex_colors && any_vertex_has_alpha))
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 handler_->addTrackedObject( manual_object_ );
00202 }
00203
00204 S_MaterialPtr TriangleListMarker::getMaterials()
00205 {
00206 S_MaterialPtr materials;
00207 materials.insert( material_ );
00208 return materials;
00209 }
00210
00211
00212 }
00213