triangle_list_marker.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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   // If we have the same number of tris as previously, just update the object
00131   if (old_message && num_points == old_message->points.size())
00132   {
00133     manual_object_->beginUpdate(0);
00134   }
00135   else // Otherwise clear it and begin anew
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 


rviz
Author(s): Dave Hershberger, David Gossow, Josh Faust
autogenerated on Mon Oct 6 2014 07:26:36