1. OpenSourceRoboticsFoundation
  2. Untitled project
  3. gazebo

Commits

Ian Chen  committed 3ee63dc

Fix some cases where range values are 0

  • Participants
  • Parent commits 8aa750e
  • Branches gpu_sensors

Comments (0)

Files changed (3)

File gazebo/rendering/GpuLaser.cc

View file
 //  double blitDur = 0.0;
 //  double postRenderDur = 0.0;
   for (unsigned int i = 0; i < this->textureCount; i++)
+  {
+    // testing =============
+    unsigned int width = this->firstPassViewports[i]->getActualWidth();
+    unsigned int height = this->firstPassViewports[i]->getActualHeight();
+    Ogre::HardwarePixelBufferSharedPtr pixelBuffer;
+    pixelBuffer = this->firstPassTextures[i]->getBuffer();
+    size_t size = Ogre::PixelUtil::getMemorySize(
+                    width, height, 1, Ogre::PF_FLOAT32_RGB);
+    // Blit the depth buffer if needed
+    float *bfer = new float[size];
+
+    memset(bfer, 255, size);
+
+    Ogre::PixelBox dst_box(width, height,
+        1, Ogre::PF_FLOAT32_RGB, bfer);
+
+//    blitT.Start();
+    pixelBuffer->blitToMemory(dst_box);
+//    blitDur = blitT.GetElapsed().Double();
+
+    float *lScan =  new float[this->w2nd * this->h2nd * 3];
+
+    memcpy(lScan, bfer,
+           this->w2nd * this->h2nd * 3 * sizeof(lScan[0]));
+
+    for ( int j = 0; j < this->w2nd * this->h2nd * 3; j =j + 3)
+    {
+      gzerr << "first pass value " << j << " " << lScan[j] << " " << lScan[j+1] << " " << lScan[j+2]  << std::endl;
+
+    }
+    // testing =============
+
+
     this->firstPassTargets[i]->swapBuffers();
 
+  }
+
   this->secondPassTarget->swapBuffers();
 
   if (this->newData && this->captureData)
   // Get pointer to the material pass
   pass = _material->getBestTechnique()->getPass(0);
 
+
+
   // Render the depth texture
   // OgreSceneManager::_render function automatically sets farClip to 0.
   // Which normally equates to infinite distance. We don't want this. So
   // we have to set the distance every time.
   _cam->setFarClipDistance(this->GetFarClip());
+//  _cam->setFarClipDistance(5);
+//  _cam->setFarClipDistance(100);
+
+//  _cam->setPosition(0, 0, 2);
+
+  Ogre::Matrix4 mat = _cam->getViewMatrix(true);
+/*  gzerr << " mat " << mat[0][0] << " " << mat[0][1]  << " " << mat[0][2]
+    << " " << mat[1][0] << " " << mat[1][1] << " " << mat[1][2]
+    << " " << mat[2][0] << " " << mat[2][1] << " " << mat[2][2] <<std::endl;*/
 
   Ogre::AutoParamDataSource autoParamDataSource;
 
 
   // Need this line to render the ground plane. No idea why it's necessary.
   renderSys->_setViewport(vp);
+//  renderSys->_setViewMatrix(_cam->getViewMatrix(true));
+//  renderSys->_setProjectionMatrix(_cam->getProjectionMatrixRS());
   sceneMgr->_setPass(pass, true, false);
   autoParamDataSource.setCurrentPass(pass);
   autoParamDataSource.setCurrentViewport(vp);
   autoParamDataSource.setCurrentSceneManager(sceneMgr);
   autoParamDataSource.setCurrentCamera(_cam, true);
 
+  //gzerr << " _cam " << _cam->setPosition(0, 0, 2);
+
   renderSys->setLightingEnabled(false);
   renderSys->_setFog(Ogre::FOG_NONE);
 
   Ogre::Viewport *vp = this->currentTarget->getViewport(0);
 
   renderSys->_setViewport(vp);
+//  renderSys->_setViewMatrix(this->camera->getViewMatrix(true));
+//  renderSys->_setProjectionMatrix(this->camera->getProjectionMatrixRS());
   autoParamDataSource.setCurrentRenderable(_rend);
   autoParamDataSource.setCurrentPass(my_pass);
   autoParamDataSource.setCurrentViewport(vp);
   Ogre::SceneManager *sceneMgr = this->scene->GetManager();
 
   sceneMgr->setShadowTechnique(Ogre::SHADOWTYPE_NONE);
+
   sceneMgr->_suppressRenderStateChanges(true);
-
   sceneMgr->addRenderObjectListener(this);
 
   for (unsigned int i = 0; i < this->textureCount; ++i)
     this->firstPassViewports[_index] =
       this->firstPassTargets[_index]->addViewport(this->camera);
     this->firstPassViewports[_index]->setClearEveryFrame(true);
+    this->firstPassViewports[_index]->setOverlaysEnabled(false);
     this->firstPassViewports[_index]->setBackgroundColour(
-        Ogre::ColourValue(0.0, 0.0, 1.0));
+        Ogre::ColourValue(this->far, 0.0, 1.0));
     this->firstPassViewports[_index]->setVisibilityMask(
         GZ_VISIBILITY_ALL & ~GZ_VISIBILITY_GUI);
   }
   if (_index == 0)
   {
+    //this->vfov = Ogre::Degree(1).valueDegrees();
     this->camera->setAspectRatio(this->rayCountRatio);
     this->camera->setFOVy(Ogre::Radian(this->vfov));
   }
   this->visual->SetMaterial("Gazebo/Green");
   this->visual->SetAmbient(common::Color(0, 1, 0, 1));
   this->visual->SetVisible(true);
-  this->scene->AddVisual(this->visual);
+//  this->scene->AddVisual(this->visual);
 }
 
 //////////////////////////////////////////////////
 void GpuLaser::SetFarClip(double _far)
 {
   this->far = _far;
+  /*for (unsigned int i = 0; i < firstPassViewports.size(); ++i)
+  {
+    if (this->firstPassViewports[i])
+      this->firstPassViewports[i]->setBackgroundColour(
+         Ogre::ColourValue(_far, 0.0, 1.0));
+  }*/
 }
 
 //////////////////////////////////////////////////

File media/materials/programs/laser_1st_pass.frag

View file
 
   if (l>far)
     l = far;
-  
+    
+//  l = gl_FragCoord.z / gl_FragCoord.w;
+    
+//    float t = sqrt(point.x*point.x + point.y*point.y + point.z*point.z);
+//    gl_FragColor = vec4(l, t, far, 1.0);    
+//    gl_FragColor = vec4(point.x, point.y, point.z, 1.0);
   gl_FragColor = vec4(l, retro, 0, 1.0);
 }

File media/materials/programs/laser_1st_pass.vert

View file
   gl_Position = ftransform();
 
   // Vertex in world space
-  point = gl_ModelViewMatrix * gl_Vertex;
+   point = gl_ModelViewMatrix * gl_Vertex;   
 }