trianglemesh_visual.cpp

Go to the documentation of this file.

#include <OGRE/OgreSubEntity.h>
#include <OGRE/OgreRenderOperation.h>
#include <OGRE/OgreTextureManager.h>
#include <OGRE/OgreHardwarePixelBuffer.h>
 
#include <rviz/display_context.h>
#include <rviz/frame_manager.h>
 
#include <stdint.h>
 
#include "trianglemesh_visual.h"
 
 
namespace rviz_mesh_plugin
{
 
  TriangleMeshVisual::TriangleMeshVisual(
    rviz::DisplayContext* context,
      size_t displayID,
      size_t meshID,
      size_t randomID)
    : m_displayContext(context),
    m_prefix(displayID),
    m_postfix(meshID),
    m_random(randomID),
    m_vertex_normals_enabled(false),
    m_vertex_colors_enabled(false),
    m_triangle_colors_enabled(false),
    m_texture_coords_enabled(false),
    m_textureCounter(0),
    m_normalsScalingFactor(1)
  {
 
    ROS_INFO("Creating TriangleMeshVisual %lu_TriangleMesh_%lu_%lu",m_prefix, m_postfix, m_random);
 
    // get or create the scene node
    Ogre::SceneManager* sceneManager = m_displayContext->getSceneManager();
    Ogre::SceneNode* rootNode = sceneManager->getRootSceneNode();
 
    std::stringstream strstream;
    strstream << "TriangleMeshScene" << m_random;
    std::string sceneId = strstream.str();
    if (sceneManager->hasSceneNode(sceneId)){
      //ROS_INFO("Attaching to scene: %s", sceneId);
      m_sceneNode = (Ogre::SceneNode*)(rootNode->getChild(sceneId));
    }else{
      //ROS_INFO("Creating new scene: %s", sceneId);
      m_sceneNode = rootNode->createChildSceneNode(sceneId);
    }
 
    // create manual object and attach it to the scene node
    std::stringstream sstm;
    sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random;
    m_mesh = sceneManager->createManualObject(sstm.str());
    m_mesh->setDynamic(false);
    m_sceneNode->attachObject(m_mesh);
  }
 
  TriangleMeshVisual::~TriangleMeshVisual()
  {
    ROS_INFO("Destroying TriangleMeshVisual %lu_TriangleMesh_%lu_%lu",m_prefix, m_postfix, m_random);
 
    std::stringstream sstm;
    reset();
    sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random;
    m_displayContext->getSceneManager()->destroyManualObject(sstm.str());
    m_displayContext->getSceneManager()->destroySceneNode(m_sceneNode);
    sstm.str("");
    sstm.flush();
  }
 
  void TriangleMeshVisual::reset()
  {
 
    ROS_INFO("Resetting TriangleMeshVisual %lu_TriangleMesh_%lu_%lu",m_prefix, m_postfix, m_random);
 
 
    std::stringstream sstm;
 
    for (size_t i = 0; i < m_textureCounter; i++)
    {
      sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random << "Texture_" << i;
      Ogre::TextureManager::getSingleton().unload(sstm.str());
      Ogre::TextureManager::getSingleton().remove(sstm.str());
      sstm.str("");
      sstm.clear();
    }
 
    sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random << "Material_" << 0;
    Ogre::MaterialManager::getSingleton().unload(sstm.str());
    Ogre::MaterialManager::getSingleton().remove(sstm.str());
    sstm.str("");
    sstm.clear();
 
    sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random << "Material_" << 1;
    Ogre::MaterialManager::getSingleton().unload(sstm.str());
    Ogre::MaterialManager::getSingleton().remove(sstm.str());
    sstm.str("");
    sstm.clear();
 
    sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random << "NormalMaterial";
    Ogre::MaterialManager::getSingleton().unload(sstm.str());
    Ogre::MaterialManager::getSingleton().remove(sstm.str());
    sstm.str("");
    sstm.clear();
 
    m_textureCounter = 0;
    m_fakeTextures.clear();
    m_hasTextures = false;
    m_mesh->clear();
    sstm.str("");
    sstm.flush();
    m_meshGeneralMaterial.setNull();
    m_meshColoredTrianglesMaterial.setNull();
    m_normalMaterial.setNull();
  }
 
  void TriangleMeshVisual::showWireframe(
    Ogre::Pass* pass,
    Ogre::ColourValue wireframeColor,
    float wireframeAlpha
  ){
        pass->setAmbient(
          Ogre::ColourValue(
            wireframeColor.r,
            wireframeColor.g,
            wireframeColor.b,
            wireframeAlpha
          )
        );
        pass->setDiffuse(
          Ogre::ColourValue(
            wireframeColor.r,
            wireframeColor.g,
            wireframeColor.b,
            wireframeAlpha
          )
        );
 
        if (wireframeAlpha < 1.0){
          pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
          pass->setDepthWriteEnabled(false);
        }
        pass->setPolygonMode(Ogre::PM_WIREFRAME);
        pass->setCullingMode(Ogre::CULL_NONE);
  }
 
  void TriangleMeshVisual::showFaces(
    Ogre::Pass* pass,
    Ogre::ColourValue facesColor,
    float facesAlpha,
    bool useVertexColors
  ){
 
    pass->setDiffuse(
      Ogre::ColourValue(
        facesColor.r,
        facesColor.g,
        facesColor.b,
        facesAlpha
      )
    );
    pass->setSelfIllumination(facesColor.r, facesColor.g, facesColor.b);
 
 
    if(useVertexColors){
      pass->setLightingEnabled(false);
    }
 
    if (facesAlpha < 1.0)
    {
      pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
      pass->setDepthWriteEnabled(false);
    }
    pass->setPolygonMode(Ogre::PM_SOLID);
    pass->setCullingMode(Ogre::CULL_NONE);
  }
 
  void TriangleMeshVisual::showNormals(
    Ogre::Pass* pass,
    Ogre::ColourValue normalsColor,
    float normalsAlpha
  ){
 
    pass->setSelfIllumination(normalsColor.r, normalsColor.g, normalsColor.b);
    pass->setDiffuse(
      Ogre::ColourValue(
        normalsColor.r,
        normalsColor.g,
        normalsColor.b,
        normalsAlpha
      )
    );
    if (normalsAlpha < 1.0)
    {
      pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
      pass->setDepthWriteEnabled(false);
    }
    pass->setPolygonMode(Ogre::PM_SOLID);
    pass->setCullingMode(Ogre::CULL_NONE);
  }
 
  void TriangleMeshVisual::showTextures(
    Ogre::Pass* pass){
        //TODO
  }
 
    void TriangleMeshVisual::updateMaterial(
        bool showWireframe,
        Ogre::ColourValue wireframeColor,
        float wireframeAlpha,
        bool showFaces,
        Ogre::ColourValue facesColor,
        float facesAlpha,
        bool useVertexColors,
        bool useTriangleColors,
        bool showTextures,
        bool showNormals,
        Ogre::ColourValue normalsColor,
        float normalsAlpha,
        float normalsScalingFactor)
  {
    // remove all passes
    if (!m_meshGeneralMaterial.isNull()){
      m_meshGeneralMaterial->getTechnique(0)->removeAllPasses();
    }
 
    if(!m_meshColoredTrianglesMaterial.isNull()){
      m_meshColoredTrianglesMaterial->getTechnique(0)->removeAllPasses();
    }
 
    if (!m_normalMaterial.isNull()){
      m_normalMaterial->getTechnique(0)->removeAllPasses();
    }
 
    bool triangleColorsEnabled = m_triangle_colors_enabled && useTriangleColors;
 
    // if the material exists and the triangle colors are not enabled
    // because, if the triangle colors are are enabled we have to use the
    // mesh with the "vertex for each triangle" structure and
    // the m_meshColoredTrianglesMaterial otherwise we can use this
    // general mesh with the m_meshGeneralMaterial
    if (!m_meshGeneralMaterial.isNull() && !triangleColorsEnabled){
      Ogre::Technique* tech = m_meshGeneralMaterial->getTechnique(0);
 
      if (showFaces){
        this->showFaces(tech->createPass(), facesColor, facesAlpha, useVertexColors);
      }
 
      if (showWireframe){
        this->showWireframe(tech->createPass(), wireframeColor, wireframeAlpha);
      }
 
      if (showTextures && m_hasTextures){
        this->showTextures(tech->createPass());
      }
    }
 
    if (!m_meshColoredTrianglesMaterial.isNull() && triangleColorsEnabled){
      Ogre::Technique* tech = m_meshColoredTrianglesMaterial->getTechnique(0);
 
      if (showFaces){
        this->showFaces(tech->createPass(), facesColor, facesAlpha, true);
      }
 
      if (showWireframe){
        this->showWireframe(tech->createPass(), wireframeColor, wireframeAlpha);
      }
 
      if (showTextures && m_hasTextures){
        this->showTextures(tech->createPass());
      }
    }
 
    if(!m_normalMaterial.isNull()){
      if (showNormals){
        Ogre::Technique* tech = m_normalMaterial->getTechnique(0);
        this->showNormals(tech->createPass(), normalsColor, normalsAlpha);
        updateNormals(normalsScalingFactor);
      }
    }
  }
 
void TriangleMeshVisual::updateNormals(float ScalingFactor)
{
  Ogre::VertexData* vertexData;
  const Ogre::VertexElement* vertexElement;
  Ogre::HardwareVertexBufferSharedPtr vertexBuffer;
  unsigned char* vertexChar;
  float* vertexFloat;
 
  vertexData = m_mesh->getSection(m_mesh->getNumSections() - 1)->getRenderOperation()->vertexData;
  vertexElement = vertexData->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION);
  vertexBuffer = vertexData->vertexBufferBinding->getBuffer(vertexElement->getSource());
  vertexChar = static_cast<unsigned char*>(vertexBuffer->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
 
  size_t halfVertexCount = vertexData->vertexCount/2;
  Ogre::Vector3* vertices = new Ogre::Vector3[halfVertexCount];
  Ogre::Vector3* normals = new Ogre::Vector3[halfVertexCount];
 
  for(size_t i = 0, vIndex = 0, nIndex = 0; i < vertexData->vertexCount; i++, vertexChar += vertexBuffer->getVertexSize())
  {
    vertexElement->baseVertexPointerToElement(vertexChar, &vertexFloat);
    Ogre::Vector3 tempVector(vertexFloat[0], vertexFloat[1], vertexFloat[2]);
 
    if (i % 2 == 0)
    {
      vertices[vIndex] = tempVector;
      vIndex++;
    }
 
    else
    {
      normals[nIndex] = (tempVector - vertices[nIndex]) / m_normalsScalingFactor;
      nIndex++;
    }
  }
  vertexBuffer->unlock();
 
  m_mesh->beginUpdate(m_mesh->getNumSections() - 1);
  for (size_t i = 0; i < halfVertexCount; i++)
  {
    m_mesh->position(vertices[i].x, vertices[i].y, vertices[i].z);
    m_mesh->position(vertices[i].x + ScalingFactor * normals[i].x,
        vertices[i].y + ScalingFactor * normals[i].y,
        vertices[i].z + ScalingFactor * normals[i].z);
  }
  m_mesh->end();
  delete [] vertices;
  delete [] normals;
  m_normalsScalingFactor = ScalingFactor;
}
 
 
 
void TriangleMeshVisual::enteringColoredTriangleMesh(const mesh_msgs::TriangleMesh& mesh){
 
  if(!m_triangle_colors_enabled){
    return;
  }
 
  std::stringstream sstm;
  sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random << "Material_" << 1;
 
  m_meshColoredTrianglesMaterial =
    Ogre::MaterialManager::getSingleton().create(
        sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true
        );
 
  // start entering data
  m_mesh->begin(sstm.str(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
 
  // write vertices for each triangle
  // to enable a coloring for each triangle
 
  // write triangle colors as vertex colours
  // write vertex normals(if enabled)
 
  size_t triangle_vertex_cnt = 0;
  for(size_t i = 0; i < mesh.triangles.size(); i++){
    // write three triangle vertices
    for(size_t j = 0; j < 3; j++){
      int vertex_index = mesh.triangles[i].vertex_indices[j];
      // write vertex positions
      m_mesh->position(
          mesh.vertices[vertex_index].x,
          mesh.vertices[vertex_index].y,
          mesh.vertices[vertex_index].z
          );
      // write triangle colors
      m_mesh->colour(
          mesh.triangle_colors[i].r,
          mesh.triangle_colors[i].g,
          mesh.triangle_colors[i].b,
          mesh.triangle_colors[i].a
          );
      // write vertex normals, if enabled
      if(m_vertex_normals_enabled){
        m_mesh->normal(
            mesh.vertex_normals[vertex_index].x,
            mesh.vertex_normals[vertex_index].y,
            mesh.vertex_normals[vertex_index].z
            );
      }
    }
    // write the three trianlge vertex indices
    m_mesh->triangle(
        triangle_vertex_cnt,
        triangle_vertex_cnt+1,
        triangle_vertex_cnt+2
        );
    triangle_vertex_cnt += 3;
  }
 
  // finish entering data
  m_mesh->end();
}
 
void TriangleMeshVisual::enteringGeneralTriangleMesh(const mesh_msgs::TriangleMesh& mesh){
 
  std::stringstream sstm;
 
  sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random << "Material_" << 0;
 
  m_meshGeneralMaterial =
    Ogre::MaterialManager::getSingleton().create(
        sstm.str(), Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true
        );
 
  // start entering data
  m_mesh->begin(sstm.str(), Ogre::RenderOperation::OT_TRIANGLE_LIST);
 
  // write vertices
  // write color(if enabled)
  // write exture coords(if enabled)
  // write vertex normals(if enabled)
  for (size_t i = 0; i < mesh.vertices.size(); i++)
  {
    // write vertex
    m_mesh->position(mesh.vertices[i].x, mesh.vertices[i].y, mesh.vertices[i].z);
 
    // write vertex color, if enabled
    if(m_vertex_colors_enabled){
      m_mesh->colour(
          mesh.vertex_colors[i].r,
          mesh.vertex_colors[i].g,
          mesh.vertex_colors[i].b,
          mesh.vertex_colors[i].a
          );
    }
 
    // write texture coord, if enabled
    if(m_texture_coords_enabled){
      m_mesh->textureCoord(
          mesh.vertex_texture_coords[i].x,
          mesh.vertex_texture_coords[i].y,
          mesh.vertex_texture_coords[i].z
          );
    }
 
    // write vertex normals, if enabled
    if(m_vertex_normals_enabled){
      m_mesh->normal(
          mesh.vertex_normals[i].x,
          mesh.vertex_normals[i].y,
          mesh.vertex_normals[i].z
          );
    }
  }
 
  // write triangles
  for (size_t i = 0; i < mesh.triangles.size(); i++)
  {
    m_mesh->triangle(
        mesh.triangles[i].vertex_indices[0],
        mesh.triangles[i].vertex_indices[1],
        mesh.triangles[i].vertex_indices[2]
        );
  }
 
  // finish entering data
  m_mesh->end();
 
}
 
void TriangleMeshVisual::enteringNormals(const mesh_msgs::TriangleMesh& mesh){
 
  if(!m_vertex_normals_enabled){
    return;
  }
 
  std::stringstream sstm;
  sstm << m_prefix << "_TriangleMesh_" << m_postfix << "_" << m_random << "NormalMaterial";
  m_normalMaterial = Ogre::MaterialManager::getSingleton().create(sstm.str(),
      Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, true);
 
  // Create pointNormals
  m_mesh->begin(sstm.str(), Ogre::RenderOperation::OT_LINE_LIST);
 
  // Vertices
  for (size_t i = 0; i < mesh.vertex_normals.size(); i++)
  {
    m_mesh->position(mesh.vertices[i].x, mesh.vertices[i].y, mesh.vertices[i].z);
    m_mesh->position(
        mesh.vertices[i].x + m_normalsScalingFactor * mesh.vertex_normals[i].x,
        mesh.vertices[i].y + m_normalsScalingFactor * mesh.vertex_normals[i].y,
        mesh.vertices[i].z + m_normalsScalingFactor * mesh.vertex_normals[i].z);
    // add line to index buffer
    m_mesh->index(2*i);
    m_mesh->index(2*i+1);
  }
  m_mesh->end();
 
}
 
void TriangleMeshVisual::setMessage(const mesh_msgs::TriangleMeshStamped::ConstPtr& mesh_msg)
{
  reset();
 
  // for a better legibility of the code
  const mesh_msgs::TriangleMesh& mesh = mesh_msg->mesh;
 
  // default: vertex colors are optional and therefore disabled
  m_vertex_colors_enabled = false;
 
  // default: triangle colors are optional and therefore disabled
  m_triangle_colors_enabled = false;
 
  // default: textures and texture_coords are optional and therefore disabled
  m_texture_coords_enabled = false;
 
  // default: vertex normals are optional and therefore disabled
  m_vertex_normals_enabled = false;
 
  // check if there are enough vertices given
  if (mesh.vertices.size() < 3){
    ROS_WARN("Received not enough vertices, can't create mesh!");
    return;
  }
 
  // defines the buffer sizes
  int vertex_count = mesh.vertices.size();
  int index_count = mesh.triangles.size() * 3;
 
 
  // vertex colors
  // check if there are vertex colors for each vertex
  if(mesh.vertex_colors.size() == mesh.vertices.size()){
    ROS_INFO("Received %lu vertex colors.", mesh.vertex_colors.size());
    m_vertex_colors_enabled = true;
  }
  else if(mesh.vertex_colors.size() > 0){
    ROS_WARN("Received not as much vertex colors as vertices, ignoring the vertex colors!");
  }
 
  // triangle colors
  // check if there are vertex colors for each vertex
  if(mesh.triangle_colors.size() == mesh.triangles.size()){
    ROS_INFO("Received %lu triangle colors.", mesh.triangle_colors.size());
    m_triangle_colors_enabled = true;
 
    // space for the extra mesh
    vertex_count += 3 * mesh.triangles.size();
    index_count += 3 * mesh.triangles.size();
  }
  else if(mesh.triangle_colors.size() > 0){
    ROS_WARN("Received not as much triangle colors as triangles, ignoring the triangle colors!");
  }
 
  // texture coords
  // check if there are texture coords for each vertex
  if(mesh.vertex_texture_coords.size() == mesh.vertices.size()){
    ROS_INFO("Received %lu texture coords.", mesh.vertex_texture_coords.size());
    m_texture_coords_enabled = true; // enable texture coords
  }
  else if(mesh.vertex_texture_coords.size() > 0){
    ROS_WARN("Received not as much texture coords as vertices, ignoring texture coords!");
  }
 
  // vertex normals
  // check if there are vertex normals for each vertex
  if(mesh.vertex_normals.size() == mesh.vertices.size()){
    ROS_INFO("Received %lu vertex normals.", mesh.vertex_normals.size());
    m_vertex_normals_enabled = true;
    // space for the normals in the buffer
    vertex_count += mesh.vertices.size();
    index_count += mesh.vertices.size() * 2;
  }
  else if(mesh.vertex_normals.size() > 0){
    ROS_WARN("Received not as much vertex normals as vertices, ignoring vertex normals!");
  }
 
  // avoid memory reallocation
  m_mesh->estimateVertexCount(vertex_count);
  m_mesh->estimateIndexCount(index_count);
 
  // entering a general triangle mesh into the internal buffer
  enteringGeneralTriangleMesh(mesh);
 
  // entering a special mesh for colored triangles into the internal buffer
  if(m_triangle_colors_enabled){
    enteringColoredTriangleMesh(mesh);
  }
 
  // entering the normals into the internal buffer
  if(m_vertex_normals_enabled){
    enteringNormals(mesh);
  }
}
 
void TriangleMeshVisual::setFramePosition(const Ogre::Vector3& position)
{
  m_sceneNode->setPosition(position);
}
 
void TriangleMeshVisual::setFrameOrientation(const Ogre::Quaternion& orientation)
{
  m_sceneNode->setOrientation(orientation);
}
 
} // end namespace rviz_mesh_plugin