triangle_mesh_rviz_plugin: triangle_mesh_rviz_plugin: trianglemesh

00001 /* 
00002  * Copyright (c) 2010, Nico Blodow (blodow@cs.tum.edu)
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 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 "trianglemesh_display.h"
00031 #include "rviz/visualization_manager.h"
00032 #include "rviz/properties/property.h"
00033 #include "rviz/properties/property_manager.h"
00034 #include "rviz/common.h"
00035 
00036 #include <tf/transform_listener.h>
00037 
00038 #include <boost/bind.hpp>
00039 
00040 #include <OGRE/OgreSceneNode.h>
00041 #include <OGRE/OgreSceneManager.h>
00042 #include <OGRE/OgreManualObject.h>
00043 #include <OGRE/OgreBillboardSet.h>
00044 
00045 typedef struct {int a,b,c;} triangle;
00047 namespace triangle_mesh
00048 {
00049 
00050 TriangleMeshDisplay::TriangleMeshDisplay(const std::string & name, rviz::VisualizationManager * manager)
00051   : Display(name, manager)
00052   , color_(0.1f, 1.0f, 0.0f)
00053   , wf_(false)
00054   , tf_filter_(*manager->getTFClient(), "", 2, update_nh_)
00055   , count (0)
00056 {
00057   scene_node_ = scene_manager_->getRootSceneNode()->createChildSceneNode();
00058 
00059   static int count = 0;
00060   std::stringstream ss;
00061   ss << "TriangleMesh " << count++;
00062   tf_filter_.connectInput(sub_);
00063   tf_filter_.registerCallback(boost::bind(&TriangleMeshDisplay::incomingMessage, this, _1));
00064 }
00065 
00066 TriangleMeshDisplay::~TriangleMeshDisplay()
00067 {
00068   unsubscribe();
00069 }
00070 
00071 void TriangleMeshDisplay::setTopic(const std::string & topic)
00072 {
00073   unsubscribe();
00074   topic_ = topic;
00075   subscribe();
00076 
00077   propertyChanged(topic_property_);
00078 
00079   causeRender();
00080 }
00081 
00082 void TriangleMeshDisplay::setColor(const rviz::Color & color)
00083 {
00084   color_ = color;
00085 
00086   propertyChanged(color_property_);
00087   processMessage(current_message_);
00088   causeRender();
00089 }
00090 
00091 void TriangleMeshDisplay::setWireFrameEnabled(const bool & wf)
00092 {
00093   wf_ = wf;
00094 
00095   propertyChanged(wireframe_property_);
00096   processMessage(current_message_);
00097   causeRender();
00098 }
00099 
00100 void TriangleMeshDisplay::subscribe()
00101 {
00102   if (!isEnabled())
00103     return;
00104 
00105   if (!topic_.empty())
00106   {
00107     sub_.subscribe(update_nh_, topic_, 1);
00108   }
00109 }
00110 
00111 void TriangleMeshDisplay::unsubscribe()
00112 {
00113   sub_.unsubscribe();
00114 }
00115 
00116 void TriangleMeshDisplay::onEnable()
00117 {
00118   scene_node_->setVisible(true);
00119   subscribe();
00120 }
00121 
00122 void TriangleMeshDisplay::onDisable()
00123 {
00124   unsubscribe();
00125   scene_node_->setVisible(false);
00126 }
00127 
00128 void TriangleMeshDisplay::fixedFrameChanged()
00129 {
00130   tf_filter_.setTargetFrame(fixed_frame_);
00131 }
00132 
00133 void TriangleMeshDisplay::update(float wall_dt, float ros_dt)
00134 {
00135 }
00136 
00138 //std::vector<geometry_msgs::Point32> computeNormals (const triangle_mesh::TriangleMesh::ConstPtr& msg)
00139 //{
00140 //  std::vector <std::vector <int> > reverse_triangles;
00141 //  for (unsigned int i = 0; i < msg->triangles.size (); i++)
00142 //  {
00143 //
00144 //  }
00145 //  return msg->normals;
00146 //}
00147 
00148 void TriangleMeshDisplay::processMessage(const triangle_mesh_msgs::TriangleMesh::ConstPtr& msg)
00149 {
00150   if (!msg)
00151   {
00152     return;
00153   }
00154   if (count++ > 0)
00155   {
00157 //    delete vbuf.get();
00158 //    delete data;
00159 //    delete ibuf;
00160     scene_manager_->destroyEntity(entity_);
00161     Ogre::MeshManager::getSingleton().unload ("trianglemesh");
00162     Ogre::MeshManager::getSingleton().remove ("trianglemesh");
00163   }
00164   std::vector<geometry_msgs::Point32> normals;
00165   bool use_normals = false;
00166 
00168 //  if (msg->normals.size() == msg->points.size())
00169 //  {
00170 //    normals = msg->normals;
00171 //    use_normals = true;
00172 //  }
00173 //  else
00174 //  {
00175 //    normals = computeNormals (msg);
00176 //  }
00177 
00179 //  std::vector<triangle> triangles;
00180 //  for (unsigned int i = 0; i < msg->triangles.size(); i++)
00181 //  {
00182 //    triangle tr;
00183 //    tr.a = msg->triangles[i].i;
00184 //    tr.b = msg->triangles[i].j;
00185 //    tr.c = msg->triangles[i].k;
00186 //    triangles.push_back (tr);
00187 //  }
00188   mesh_ = Ogre::MeshManager::getSingleton().createManual ("trianglemesh", "General");
00189   submesh_ = mesh_->createSubMesh();
00190 
00191   // We first create a VertexData
00192   data = new Ogre::VertexData();
00193   // Then, we link it to our Mesh/SubMesh :
00194   mesh_->sharedVertexData = data;
00195   // submesh_->useSharedVertices = false; // This value is 'true' by default
00196   // submesh_->vertexData = data;
00197   // We have to provide the number of verteices we'll put into this Mesh/SubMesh
00198   data->vertexCount = msg->points.size();
00199   // Then we can create our VertexDeclaration
00200   Ogre::VertexDeclaration* decl = data->vertexDeclaration;
00201   decl->addElement (0,0, Ogre::VET_FLOAT3, Ogre::VES_POSITION);
00203   if (use_normals)
00204     decl->addElement (0,Ogre::VertexElement::getTypeSize(Ogre::VET_FLOAT3), Ogre::VET_FLOAT3, Ogre::VES_NORMAL);
00205   
00206   // create a vertex buffer
00207   vbuf = Ogre::HardwareBufferManager::getSingleton().createVertexBuffer(
00208     decl->getVertexSize(0),                     // This value is the size of a vertex in memory
00209     msg->points.size(),                         // The number of vertices you'll put into this buffer
00210     Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY // Properties
00211   );
00212  
00213   double min_x=FLT_MAX,  min_y=FLT_MAX,  min_z=FLT_MAX;
00214   double max_x=-FLT_MAX, max_y=-FLT_MAX, max_z=-FLT_MAX;
00215   int vertex_size_in_bytes = 3;
00216   if (use_normals)
00217     vertex_size_in_bytes = 6;
00218   float* array = (float*) malloc (sizeof(float)*vertex_size_in_bytes*msg->points.size());
00219   for (unsigned int i = 0; i < msg->points.size(); i++)
00220   {
00221     array[i*vertex_size_in_bytes+0] = msg->points[i].x;
00222     array[i*vertex_size_in_bytes+1] = msg->points[i].y;
00223     array[i*vertex_size_in_bytes+2] = msg->points[i].z;
00224     if (use_normals)
00225     {
00226       array[i*vertex_size_in_bytes+3] = normals[i].x;
00227       array[i*vertex_size_in_bytes+4] = normals[i].x;
00228       array[i*vertex_size_in_bytes+5] = normals[i].x;
00229     }
00230     if (msg->points[i].x < min_x) min_x = msg->points[i].x;
00231     if (msg->points[i].y < min_y) min_y = msg->points[i].y;
00232     if (msg->points[i].z < min_z) min_z = msg->points[i].z;
00233     if (msg->points[i].x > max_x) max_x = msg->points[i].x;
00234     if (msg->points[i].y > max_y) max_y = msg->points[i].y;
00235     if (msg->points[i].z > max_z) max_z = msg->points[i].z;
00236   }
00237   
00238   vbuf->writeData (0, vbuf->getSizeInBytes(), array, true); 
00239   free (array);
00240   
00241   Ogre::VertexBufferBinding* bind = data->vertexBufferBinding;
00242   bind->setBinding (0, vbuf);
00243   
00244   bool back_face_duplicate = true;
00245   int stride = 3;  
00246   if (back_face_duplicate)
00247     stride = 6;
00248 
00249   // create index buffer
00250   ibuf = Ogre::HardwareBufferManager::getSingleton().createIndexBuffer(
00251     Ogre::HardwareIndexBuffer::IT_32BIT,        // You can use several different value types here
00252     msg->triangles.size()*stride,                    // The number of indices you'll put in that buffer
00253     Ogre::HardwareBuffer::HBU_STATIC_WRITE_ONLY // Properties
00254   );
00255   
00256   submesh_->indexData->indexBuffer = ibuf;     // The pointer to the index buffer
00257 
00258   submesh_->indexData->indexCount = msg->triangles.size()*stride; // The number of indices we'll use
00259   submesh_->indexData->indexStart = 0;
00260 
00261   unsigned int *index;
00262   index = (unsigned int*) malloc (sizeof(unsigned int)*stride*msg->triangles.size());
00263   for (unsigned int i = 0; i < msg->triangles.size(); i++)
00264   {
00265     index[i*stride+0] = msg->triangles[i].i;
00266     index[i*stride+1] = msg->triangles[i].j;
00267     index[i*stride+2] = msg->triangles[i].k;
00268     if (back_face_duplicate)
00269     {
00270       index[i*stride+3] = msg->triangles[i].i;
00271       index[i*stride+4] = msg->triangles[i].k;
00272       index[i*stride+5] = msg->triangles[i].j;
00273     }
00274   }
00275 
00276   ibuf->writeData (0, ibuf->getSizeInBytes(), index, true); 
00277   free (index);
00278  
00279   mesh_->_setBounds (Ogre::AxisAlignedBox (min_x, min_y, min_z, max_x, max_y, max_z));
00280   mesh_->_setBoundingSphereRadius (std::max(max_x-min_x, std::max(max_y-min_y, max_z-min_z))/2.0f);
00281 
00282   mesh_->load ();
00283 
00284   std::stringstream ss;
00285   ss << "tm" << count;
00286   entity_ = scene_manager_->createEntity(ss.str(), "trianglemesh");
00287   for (unsigned int i = 0; i < entity_->getNumSubEntities (); i++)
00288   {
00289     entity_->getSubEntity (i)->getMaterial ()->getTechnique(0)->getPass(0)->setDiffuse(Ogre::ColourValue(color_.r_, color_.g_, color_.b_));
00290     entity_->getSubEntity (i)->getMaterial ()->getTechnique(0)->getPass(0)->setAmbient(Ogre::Real(color_.r_), Ogre::Real(color_.g_), Ogre::Real(color_.b_));
00291     if (wf_)
00292       entity_->getSubEntity (i)->getMaterial ()->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_WIREFRAME);
00293     else
00294       entity_->getSubEntity (i)->getMaterial ()->getTechnique(0)->getPass(0)->setPolygonMode(Ogre::PM_SOLID);
00295   }
00296   
00297   scene_node_->attachObject(entity_);
00298 
00299   // finally, make sure we get everything in RVIZ's coordinate system
00300   tf::Stamped<tf::Pose> pose(btTransform(btQuaternion(0.0f, 0.0f, 0.0f, 1.0f),
00301       btVector3(0, 0, 0)), msg->header.stamp,
00302       msg->header.frame_id);
00303 
00304   try
00305   {
00306     vis_manager_->getTFClient()->transformPose(fixed_frame_, pose, pose);
00307   }
00308   catch (tf::TransformException & e)
00309   {
00310     ROS_ERROR("Error transforming from frame '%s' to frame '%s'",
00311         msg->header.frame_id.c_str(), fixed_frame_.c_str());
00312   }
00313 
00314   Ogre::Vector3 position = Ogre::Vector3(pose.getOrigin().x(),
00315       pose.getOrigin().y(), pose.getOrigin().z());
00316   
00317   Ogre::Quaternion orientation = Ogre::Quaternion (pose.getRotation().w(), pose.getRotation().x(), pose.getRotation().y(), pose.getRotation().z()); 
00318 
00319   rviz::robotToOgre(position);
00320   rviz::robotToOgre(orientation);
00321 
00322   scene_node_->setOrientation (orientation);
00323   scene_node_->setPosition (position);
00324 
00325 }
00326 
00327 void TriangleMeshDisplay::incomingMessage(const triangle_mesh_msgs::TriangleMesh::ConstPtr& message)
00328 {
00329   current_message_ = message;
00330   processMessage(message);
00331 }
00332 
00333 void TriangleMeshDisplay::reset()
00334 {
00335 }
00336 
00337 void TriangleMeshDisplay::targetFrameChanged()
00338 {
00339 }
00340 
00341 void TriangleMeshDisplay::createProperties()
00342 {
00343   wireframe_property_ = property_manager_->createProperty<rviz::BoolProperty> 
00344               ("Wireframe enable", property_prefix_, boost::bind(&TriangleMeshDisplay::getWireFrameEnabled, this),
00345               boost::bind(&TriangleMeshDisplay::setWireFrameEnabled, this, _1), parent_category_, this);
00346   color_property_ = property_manager_->createProperty<rviz::ColorProperty> 
00347               ("Color", property_prefix_, boost::bind(&TriangleMeshDisplay::getColor, this),
00348               boost::bind(&TriangleMeshDisplay::setColor, this, _1), parent_category_, this);
00349   topic_property_ = property_manager_->createProperty<rviz::ROSTopicStringProperty> 
00350               ("Topic", property_prefix_, boost::bind(&TriangleMeshDisplay::getTopic, this),
00351               boost::bind(&TriangleMeshDisplay::setTopic, this, _1), parent_category_, this);
00352   rviz::ROSTopicStringPropertyPtr topic_prop = topic_property_.lock();
00353   topic_prop->setMessageType(ros::message_traits::DataType<triangle_mesh_msgs::TriangleMesh>().value());
00354 }
00355 
00356 
00357 } // namespace triangle_mesh
trianglemesh_display.cpp
