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 
00036 #include "rviz/visualization_manager.h"
00037 #include "rviz/mesh_loader.h"
00038 #include "marker_display.h"
00039 
00040 #include <OGRE/OgreSceneNode.h>
00041 #include <OGRE/OgreSceneManager.h>
00042 #include <OGRE/OgreManualObject.h>
00043 #include <OGRE/OgreMaterialManager.h>
00044 #include <OGRE/OgreTextureManager.h>
00045 #include <OGRE/OgreTechnique.h>
00046 
00047 namespace rviz
00048 {
00049 
00050 TriangleListMarker::TriangleListMarker(MarkerDisplay* owner, VisualizationManager* manager, Ogre::SceneNode* parent_node)
00051 : MarkerBase(owner, manager, parent_node)
00052 , manual_object_(0)
00053 {
00054 }
00055 
00056 TriangleListMarker::~TriangleListMarker()
00057 {
00058   vis_manager_->getSceneManager()->destroyManualObject(manual_object_);
00059 
00060   for (size_t i = 0; i < material_->getNumTechniques(); ++i)
00061   {
00062     Ogre::Technique* t = material_->getTechnique(i);
00063     // hack hack hack, really need to do a shader-based way of picking, rather than
00064     // creating a texture for each object
00065     if (t->getSchemeName() == "Pick")
00066     {
00067       Ogre::TextureManager::getSingleton().remove(t->getPass(0)->getTextureUnitState(0)->getTextureName());
00068     }
00069   }
00070 
00071   material_->unload();
00072   Ogre::MaterialManager::getSingleton().remove(material_->getName());
00073 }
00074 
00075 void TriangleListMarker::onNewMessage(const MarkerConstPtr& old_message, const MarkerConstPtr& new_message)
00076 {
00077   ROS_ASSERT(new_message->type == visualization_msgs::Marker::TRIANGLE_LIST);
00078 
00079   if (!manual_object_)
00080   {
00081     static uint32_t count = 0;
00082     std::stringstream ss;
00083     ss << "Triangle List Marker" << count++;
00084     manual_object_ = vis_manager_->getSceneManager()->createManualObject(ss.str());
00085     scene_node_->attachObject(manual_object_);
00086 
00087     ss << "Material";
00088     material_name_ = ss.str();
00089     material_ = Ogre::MaterialManager::getSingleton().create( material_name_, ROS_PACKAGE_NAME );
00090     material_->setReceiveShadows(false);
00091     material_->getTechnique(0)->setLightingEnabled(true);
00092     material_->setCullingMode(Ogre::CULL_NONE);
00093 
00094     vis_manager_->getSelectionManager()->removeObject(coll_);
00095 
00096     SelectionManager* sel_man = vis_manager_->getSelectionManager();
00097     coll_ = sel_man->createHandle();
00098     sel_man->addPickTechnique(coll_, material_);
00099     sel_man->addObject( coll_, SelectionHandlerPtr(new MarkerSelectionHandler(this, MarkerID(new_message->ns, new_message->id))) );
00100   }
00101 
00102   size_t num_points = new_message->points.size();
00103   if ((num_points % 3) != 0)
00104   {
00105     std::stringstream ss;
00106     ss << "TriMesh marker [" << getStringID() << "] has a point count which is not divisible by 3 [" << num_points <<"]";
00107     if ( owner_ )
00108     {
00109       owner_->setMarkerStatus(getID(), status_levels::Error, ss.str());
00110     }
00111     ROS_DEBUG("%s", ss.str().c_str());
00112 
00113     manual_object_->clear();
00114     scene_node_->setVisible(false);
00115     return;
00116   }
00117 
00118   Ogre::Vector3 pos, scale;
00119   Ogre::Quaternion orient;
00120   transform(new_message, pos, orient, scale);
00121 
00122   if ( owner_ &&  (new_message->scale.x * new_message->scale.y * new_message->scale.z == 0.0f) )
00123   {
00124     owner_->setMarkerStatus(getID(), status_levels::Warn, "Scale of 0 in one of x/y/z");
00125   }
00126 
00127   setPosition(pos);
00128   setOrientation(orient);
00129   scene_node_->setScale(scale);
00130 
00131   // If we have the same number of tris as previously, just update the object
00132   if (old_message && num_points == old_message->points.size())
00133   {
00134     manual_object_->beginUpdate(0);
00135   }
00136   else // Otherwise clear it and begin anew
00137   {
00138     manual_object_->clear();
00139     manual_object_->estimateVertexCount(num_points);
00140     manual_object_->begin(material_name_, Ogre::RenderOperation::OT_TRIANGLE_LIST);
00141   }
00142 
00143   bool has_vertex_colors = new_message->colors.size() == num_points;
00144 
00145   if (has_vertex_colors)
00146   {
00147     for (size_t i = 0; i < num_points; ++i)
00148     {
00149       manual_object_->position(new_message->points[i].x, new_message->points[i].y, new_message->points[i].z);
00150       manual_object_->colour(new_message->colors[i].r, new_message->colors[i].g, new_message->colors[i].b, new_message->color.a);
00151     }
00152   }
00153   else
00154   {
00155     for (size_t i = 0; i < num_points; ++i)
00156     {
00157       manual_object_->position(new_message->points[i].x, new_message->points[i].y, new_message->points[i].z);
00158     }
00159   }
00160 
00161   manual_object_->end();
00162 
00163   if (has_vertex_colors)
00164   {
00165     material_->getTechnique(0)->setLightingEnabled(false);
00166   }
00167   else
00168   {
00169     material_->getTechnique(0)->setLightingEnabled(true);
00170     float r,g,b,a;
00171     r = new_message->color.r;
00172     g = new_message->color.g;
00173     b = new_message->color.b;
00174     a = new_message->color.a;
00175     material_->getTechnique(0)->setAmbient( r,g,b );
00176     material_->getTechnique(0)->setDiffuse( 0,0,0,a );
00177   }
00178 
00179   if ( new_message->color.a < 0.9998 )
00180   {
00181     material_->getTechnique(0)->setSceneBlending( Ogre::SBT_TRANSPARENT_ALPHA );
00182     material_->getTechnique(0)->setDepthWriteEnabled( false );
00183   }
00184   else
00185   {
00186     material_->getTechnique(0)->setSceneBlending( Ogre::SBT_REPLACE );
00187     material_->getTechnique(0)->setDepthWriteEnabled( true );
00188   }
00189 }
00190 
00191 S_MaterialPtr TriangleListMarker::getMaterials()
00192 {
00193   S_MaterialPtr materials;
00194   materials.insert( material_ );
00195   return materials;
00196 }
00197 
00198 
00199 }
00200 


rviz_qt
Author(s): Dave Hershberger
autogenerated on Fri Dec 6 2013 20:56:53