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