00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
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)
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)
00167 throw runtime_error ("Couldn't create frame buffer");
00168
00169 glBindFramebuffer(GL_FRAMEBUFFER, 0);
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
00265 char const * SourcePointer = ShaderCode.c_str();
00266 glShaderSource(ShaderID, 1, &SourcePointer , NULL);
00267 glCompileShader(ShaderID);
00268
00269
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
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
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 }