gl_renderer.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, Willow Garage, Inc.
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 /* Author: Suat Gedikli */
00036 
00037 #include <GL/glew.h>
00038 #ifdef __APPLE__
00039 #include <OpenGL/glu.h>
00040 #else
00041 #include <GL/glu.h>
00042 #endif
00043 #include <GL/glut.h>
00044 #include <GL/freeglut.h>
00045 #include <moveit/mesh_filter/gl_renderer.h>
00046 #include <sstream>
00047 #include <fstream>
00048 #include <stdexcept>
00049 #include <vector>
00050 #include <iostream>
00051 #include <boost/thread.hpp>
00052 #include <ros/console.h>
00053 
00054 using namespace std;
00055 
00056 
00057 mesh_filter::GLRenderer::GLRenderer (unsigned width, unsigned height, float near, float far)
00058   : width_ (width)
00059   , height_ (height)
00060   , fbo_id_ (0)
00061   , rbo_id_ (0)
00062   , rgb_id_ (0)
00063   , depth_id_ (0)
00064   , program_ (0)
00065   , near_ (near)
00066   , far_ (far)
00067   , fx_ (width >> 1) // 90 degree wide angle
00068   , fy_ (fx_)
00069   , cx_ (width >> 1)
00070   , cy_ (height >> 1)
00071 {
00072   createGLContext ();
00073   initFrameBuffers ();
00074 }
00075 
00076 mesh_filter::GLRenderer::~GLRenderer ()
00077 {
00078   if (program_)
00079     glDeleteProgram (program_);
00080 
00081   deleteFrameBuffers();
00082   deleteGLContext ();
00083 }
00084 
00085 void mesh_filter::GLRenderer::setBufferSize (unsigned width, unsigned height)
00086 {
00087   if (width_ != width || height_ != height)
00088   {
00089     width_ = width;
00090     height_ = height;
00091     deleteFrameBuffers();
00092     initFrameBuffers();
00093   }
00094 }
00095 
00096 void mesh_filter::GLRenderer::setClippingRange (float near, float far)
00097 {
00098   if (near_ <= 0)
00099     throw runtime_error ("near clipping plane distance needs to be larger than 0");
00100   if (far_ <= near_)
00101     throw runtime_error ("far clipping plane needs to be larger than near clipping plane distance");
00102   near_ = near;
00103   far_ = far;
00104 }
00105 
00106 void mesh_filter::GLRenderer::setCameraParameters (float fx, float fy, float cx, float cy)
00107 {
00108   fx_ = fx;
00109   fy_ = fy;
00110   cx_ = cx;
00111   cy_ = cy;
00112 }
00113 
00114 void mesh_filter::GLRenderer::setCameraParameters () const
00115 {
00116   float left = near_ * -cx_ / fx_;
00117   float right = near_ * (width_ - cx_) / fx_;
00118   float top = near_ * cy_ / fy_;
00119   float bottom = near_ * (cy_ - height_) / fy_;
00120 
00121   glMatrixMode (GL_PROJECTION);
00122   glLoadIdentity ();
00123   glFrustum (left, right, bottom, top, near_, far_);
00124 
00125   glMatrixMode (GL_MODELVIEW);
00126   glLoadIdentity();
00127   gluLookAt (0, 0, 0, 0, 0, 1, 0, -1, 0);
00128 }
00129 
00130 void mesh_filter::GLRenderer::initFrameBuffers ()
00131 {
00132   glGenTextures(1, &rgb_id_);
00133   glBindTexture(GL_TEXTURE_2D, rgb_id_);
00134   glTexImage2D(GL_TEXTURE_2D, 0, GL_RGBA, width_, height_, 0, GL_RGBA, GL_UNSIGNED_BYTE, NULL);
00135   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
00136   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
00137   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
00138   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
00139   glBindTexture(GL_TEXTURE_2D, 0);
00140 
00141   glGenTextures(1, &depth_id_);
00142   glBindTexture(GL_TEXTURE_2D, depth_id_);
00143   glTexImage2D(GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, width_, height_, 0, GL_DEPTH_COMPONENT, GL_FLOAT, 0);
00144   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_S, GL_CLAMP_TO_EDGE);
00145   glTexParameterf(GL_TEXTURE_2D, GL_TEXTURE_WRAP_T, GL_CLAMP_TO_EDGE);
00146   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_LINEAR);
00147   glTexParameteri(GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_LINEAR);
00148   glBindTexture(GL_TEXTURE_2D, 0);
00149 
00150   glGenFramebuffers(1, &fbo_id_);
00151   glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_);
00152   glFramebufferTexture2D(GL_FRAMEBUFFER, GL_COLOR_ATTACHMENT0, GL_TEXTURE_2D, rgb_id_, 0);
00153 
00154   glGenRenderbuffers(1, &rbo_id_);
00155   glBindRenderbuffer(GL_RENDERBUFFER, rbo_id_);
00156   glRenderbufferStorage(GL_RENDERBUFFER, GL_DEPTH_COMPONENT, width_, height_);
00157   glFramebufferRenderbuffer(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_RENDERBUFFER, rbo_id_);
00158   glFramebufferTexture2D(GL_FRAMEBUFFER, GL_DEPTH_ATTACHMENT, GL_TEXTURE_2D, depth_id_, 0);
00159   glBindRenderbuffer(GL_RENDERBUFFER, 0);
00160 
00161   GLenum DrawBuffers[2] = {GL_COLOR_ATTACHMENT0, GL_DEPTH_ATTACHMENT};
00162   glDrawBuffers(2, DrawBuffers);
00163 
00164   GLenum status = glCheckFramebufferStatus(GL_FRAMEBUFFER);
00165 
00166   if (status != GL_FRAMEBUFFER_COMPLETE) // If the frame buffer does not report back as complete
00167     throw runtime_error ("Couldn't create frame buffer");
00168 
00169   glBindFramebuffer(GL_FRAMEBUFFER, 0); // Unbind our frame buffer
00170 }
00171 
00172 void mesh_filter::GLRenderer::deleteFrameBuffers()
00173 {
00174   if (rbo_id_)
00175     glDeleteRenderbuffers (1, &rbo_id_);
00176   if (fbo_id_)
00177     glDeleteFramebuffers (1, &fbo_id_);
00178   if (depth_id_)
00179     glDeleteTextures(1, &depth_id_);
00180   if (rgb_id_)
00181     glDeleteTextures(1, &rgb_id_);
00182 
00183   rbo_id_ = fbo_id_ = depth_id_ = rgb_id_ = 0;
00184 }
00185 
00186 void mesh_filter::GLRenderer::begin () const
00187 {
00188   glPushAttrib(GL_VIEWPORT_BIT | GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT | GL_POLYGON_BIT);
00189   glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_);
00190   glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
00191   glViewport (0, 0, width_, height_);
00192   glUseProgram (program_);
00193   setCameraParameters ();
00194 }
00195 
00196 void mesh_filter::GLRenderer::callList (GLuint list) const
00197 {
00198   begin ();
00199   glCallList( list );
00200   end ();
00201 }
00202 
00203 void mesh_filter::GLRenderer::end () const
00204 {
00205   glFlush();
00206   glPopAttrib();
00207   glBindFramebuffer(GL_FRAMEBUFFER, 0);
00208 }
00209 
00210 void mesh_filter::GLRenderer::getColorBuffer(unsigned char* buffer) const
00211 {
00212   glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_);
00213   glBindTexture(GL_TEXTURE_2D, rgb_id_);
00214   glGetTexImage (GL_TEXTURE_2D, 0, GL_RGBA, GL_UNSIGNED_BYTE, buffer );
00215   glBindFramebuffer(GL_FRAMEBUFFER, 0);
00216 }
00217 
00218 void mesh_filter::GLRenderer::getDepthBuffer(float* buffer) const
00219 {
00220   glBindFramebuffer(GL_FRAMEBUFFER, fbo_id_);
00221   glBindTexture(GL_TEXTURE_2D, depth_id_);
00222   glGetTexImage (GL_TEXTURE_2D, 0, GL_DEPTH_COMPONENT, GL_FLOAT, buffer );
00223   glBindFramebuffer(GL_FRAMEBUFFER, 0);
00224 }
00225 
00226 GLuint mesh_filter::GLRenderer::setShadersFromFile (const string& vertex_filename, const string& fragment_filename)
00227 {
00228   if (program_)
00229     glDeleteProgram (program_);
00230 
00231   string vertex_source, fragment_source;
00232   readShaderCodeFromFile(vertex_filename, vertex_source);
00233   readShaderCodeFromFile(fragment_filename, fragment_source);
00234 
00235   program_ = loadShaders(vertex_source, fragment_source);
00236   return program_;
00237 }
00238 
00239 GLuint mesh_filter::GLRenderer::setShadersFromString (const string& vertex_source, const string& fragment_source)
00240 {
00241   program_ = loadShaders(vertex_source, fragment_source);
00242   return program_;
00243 }
00244 
00245 const GLuint& mesh_filter::GLRenderer::getProgramID () const
00246 {
00247   return program_;
00248 }
00249 
00250 const float& mesh_filter::GLRenderer::getNearClippingDistance() const
00251 {
00252   return near_;
00253 }
00254 
00255 const float& mesh_filter::GLRenderer::getFarClippingDistance() const
00256 {
00257   return far_;
00258 }
00259 
00260 GLuint mesh_filter::GLRenderer::createShader (GLuint shaderType, const string& ShaderCode) const
00261 {
00262   GLuint ShaderID = glCreateShader(shaderType);
00263 
00264   // Compile Shader
00265   char const * SourcePointer = ShaderCode.c_str();
00266   glShaderSource(ShaderID, 1, &SourcePointer , NULL);
00267   glCompileShader(ShaderID);
00268 
00269   // Check Shader
00270   GLint Result = GL_FALSE;
00271   glGetShaderiv(ShaderID, GL_COMPILE_STATUS, &Result);
00272   if (Result != GL_TRUE)
00273   {
00274     int InfoLogLength;
00275     glGetShaderiv(ShaderID, GL_INFO_LOG_LENGTH, &InfoLogLength);
00276     if ( InfoLogLength > 0 )
00277     {
00278       vector<char> ShaderErrorMessage(InfoLogLength+1);
00279       glGetShaderInfoLog(ShaderID, InfoLogLength, NULL, &ShaderErrorMessage[0]);
00280       stringstream errorStream;
00281       errorStream << "Could not compile shader: " << (const char*) &ShaderErrorMessage [0];
00282 
00283       glDeleteShader (ShaderID);
00284       throw runtime_error (errorStream.str());
00285     }
00286   }
00287   return ShaderID;
00288 }
00289 
00290 void mesh_filter::GLRenderer::readShaderCodeFromFile (const string& filename, string& shader) const
00291 {
00292   if (filename.empty ())
00293     shader = "";
00294   else
00295   {
00296     string ShaderCode;
00297     fstream ShaderFile(filename.c_str(), ios::in);
00298     if(ShaderFile.is_open())
00299     {
00300       stringstream buffer;
00301       buffer << ShaderFile.rdbuf();
00302       shader = buffer.str();
00303     }
00304     else
00305     {
00306       stringstream errorStream;
00307       errorStream << "Could not open shader code in file \"" << filename << "\"";
00308       throw runtime_error (errorStream.str());
00309     }
00310   }
00311 }
00312 
00313 GLuint mesh_filter::GLRenderer::loadShaders (const string& vertex_source, const string& fragment_source) const
00314 {
00315   if (vertex_source.empty () && fragment_source.empty ())
00316     return 0;
00317 
00318   GLuint ProgramID = glCreateProgram();
00319   GLuint VertexShaderID = 0;
00320   GLuint FragmentShaderID = 0;
00321 
00322   if (!vertex_source.empty ())
00323   {
00324     GLuint VertexShaderID = createShader (GL_VERTEX_SHADER, vertex_source);
00325     glAttachShader(ProgramID, VertexShaderID);
00326   }
00327 
00328   if (!fragment_source.empty ())
00329   {
00330     GLuint FragmentShaderID = createShader(GL_FRAGMENT_SHADER, fragment_source);
00331     glAttachShader(ProgramID, FragmentShaderID);
00332   }
00333 
00334   glLinkProgram(ProgramID);
00335 
00336   // Check the program
00337   GLint Result = GL_FALSE;
00338   GLint InfoLogLength;
00339   glGetProgramiv(ProgramID, GL_LINK_STATUS, &Result);
00340   glGetProgramiv(ProgramID, GL_INFO_LOG_LENGTH, &InfoLogLength);
00341   if ( InfoLogLength > 0 )
00342   {
00343     vector<char> ProgramErrorMessage(InfoLogLength+1);
00344     glGetProgramInfoLog(ProgramID, InfoLogLength, NULL, &ProgramErrorMessage[0]);
00345     std::size_t l = strnlen(&ProgramErrorMessage[0], ProgramErrorMessage.size());
00346     if (l > 0)
00347       ROS_ERROR("%s\n", &ProgramErrorMessage[0]);
00348   }
00349 
00350   if (VertexShaderID)
00351     glDeleteShader(VertexShaderID);
00352 
00353   if (FragmentShaderID)
00354     glDeleteShader(FragmentShaderID);
00355 
00356   return ProgramID;
00357 }
00358 
00359 map<boost::thread::id, pair<unsigned, GLuint> > mesh_filter::GLRenderer::context_;
00360 boost::mutex mesh_filter::GLRenderer::context_lock_;
00361 bool mesh_filter::GLRenderer::glutInitialized_ = false;
00362 
00363 void mesh_filter::GLRenderer::createGLContext ()
00364 {
00365   boost::mutex::scoped_lock _(context_lock_);
00366   if (!glutInitialized_)
00367   {
00368     char buffer[1];
00369     char* args = buffer;
00370     int n = 1;
00371 
00372     glutInit(&n, &args);
00373     glutInitDisplayMode(GLUT_SINGLE | GLUT_RGB | GLUT_DEPTH);
00374     glutInitialized_ = true;
00375   }
00376 
00377   // check if our thread is initialized
00378   boost::thread::id threadID = boost::this_thread::get_id();
00379   map<boost::thread::id, pair<unsigned, GLuint> >::iterator contextIt = context_.find(threadID);
00380 
00381   if (contextIt == context_.end())
00382   {
00383     context_ [threadID] = std::pair<unsigned, GLuint> (1, 0);
00384     map<boost::thread::id, pair<unsigned, GLuint> >::iterator contextIt = context_.find(threadID);
00385 
00386     glutInitWindowPosition (glutGet(GLUT_SCREEN_WIDTH) + 30000, 0);
00387     glutInitWindowSize(1, 1);
00388     GLuint window_id = glutCreateWindow( "mesh_filter" );
00389 
00390     GLenum err = glewInit();
00391     if (GLEW_OK != err)
00392     {
00393       stringstream errorStream;
00394       errorStream << "Unable to initialize GLEW: " << glewGetErrorString(err);
00395 
00396       throw (runtime_error (errorStream.str()));
00397     }
00398     glutIconifyWindow();
00399     glutHideWindow();
00400 
00401     for (int i = 0 ; i < 10 ; ++i)
00402       glutMainLoopEvent ();
00403 
00404     context_ [threadID] = std::pair<unsigned, GLuint> (1, window_id);
00405   }
00406   else
00407     ++ (contextIt->second.first);
00408 }
00409 
00410 void mesh_filter::GLRenderer::deleteGLContext ()
00411 {
00412   boost::mutex::scoped_lock _(context_lock_);
00413   boost::thread::id threadID = boost::this_thread::get_id();
00414   map<boost::thread::id, pair<unsigned, GLuint> >::iterator contextIt = context_.find(threadID);
00415   if (contextIt == context_.end())
00416   {
00417     stringstream errorMsg;
00418     errorMsg << "No OpenGL context exists for Thread " << threadID;
00419     throw runtime_error (errorMsg.str ());
00420   }
00421 
00422   if ( --(contextIt->second.first) == 0)
00423   {
00424     glutDestroyWindow(contextIt->second.second);
00425     context_.erase(contextIt);
00426   }
00427 }
00428 
00429 GLuint mesh_filter::GLRenderer::getColorTexture() const
00430 {
00431   return rgb_id_;
00432 }
00433 
00434 GLuint mesh_filter::GLRenderer::getDepthTexture() const
00435 {
00436   return depth_id_;
00437 }
00438 
00439 const unsigned mesh_filter::GLRenderer::getWidth () const
00440 {
00441   return width_;
00442 }
00443 
00444 const unsigned mesh_filter::GLRenderer::getHeight () const
00445 {
00446   return height_;
00447 }


perception
Author(s): Ioan Sucan , Jon Binney , Suat Gedikli
autogenerated on Wed Aug 26 2015 12:43:21