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 const std::vector<geometry_msgs::Point>& points = new_message->points;
00147 for(size_t i = 0; i < num_points; i += 3)
00148 {
00149 std::vector<Ogre::Vector3> corners(3);
00150 for(size_t c = 0; c < 3; c++)
00151 {
00152 corners[c] = Ogre::Vector3(points[i+c].x, points[i+c].y, points[i+c].z);
00153 }
00154 Ogre::Vector3 normal = (corners[1] - corners[0]).crossProduct(corners[2] - corners[0]);
00155 normal.normalise();
00156
00157 for(size_t c = 0; c < 3; c++)
00158 {
00159 manual_object_->position(corners[c]);
00160 manual_object_->normal(normal);
00161 if(has_vertex_colors)
00162 {
00163 any_vertex_has_alpha = any_vertex_has_alpha || (new_message->colors[i+c].a < 0.9998);
00164 manual_object_->colour(new_message->colors[i+c].r,
00165 new_message->colors[i+c].g,
00166 new_message->colors[i+c].b,
00167 new_message->color.a * new_message->colors[i+c].a);
00168 }
00169 else if (has_face_colors)
00170 {
00171 any_vertex_has_alpha = any_vertex_has_alpha || (new_message->colors[i/3].a < 0.9998);
00172 manual_object_->colour(new_message->colors[i/3].r,
00173 new_message->colors[i/3].g,
00174 new_message->colors[i/3].b,
00175 new_message->color.a * new_message->colors[i/3].a);
00176 }
00177 }
00178 }
00179
00180 manual_object_->end();
00181
00182 if (has_vertex_colors || has_face_colors)
00183 {
00184 material_->getTechnique(0)->setLightingEnabled(false);
00185 }
00186 else
00187 {
00188 material_->getTechnique(0)->setLightingEnabled(true);
00189 float r,g,b,a;
00190 r = new_message->color.r;
00191 g = new_message->color.g;
00192 b = new_message->color.b;
00193 a = new_message->color.a;
00194 material_->getTechnique(0)->setAmbient( r/2,g/2,b/2 );
00195 material_->getTechnique(0)->setDiffuse( r,g,b,a );
00196 }
00197
00198 if( (!has_vertex_colors && new_message->color.a < 0.9998) || (has_vertex_colors && any_vertex_has_alpha))
00199 {
00200 material_->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00201 material_->getTechnique(0)->setDepthWriteEnabled( false );
00202 }
00203 else
00204 {
00205 material_->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
00206 material_->getTechnique(0)->setDepthWriteEnabled( true );
00207 }
00208
00209 handler_->addTrackedObject( manual_object_ );
00210 }
00211
00212 S_MaterialPtr TriangleListMarker::getMaterials()
00213 {
00214 S_MaterialPtr materials;
00215 materials.insert( material_ );
00216 return materials;
00217 }
00218
00219
00220 }
00221