diff --git a/gazebo/rendering/CMakeLists.txt b/gazebo/rendering/CMakeLists.txt index 55e52e3634..9502ab2997 100644 --- a/gazebo/rendering/CMakeLists.txt +++ b/gazebo/rendering/CMakeLists.txt @@ -95,6 +95,7 @@ set (headers DynamicRenderable.hh FPSViewController.hh GpuLaser.hh + GpuLaserCubeFace.hh GpuLaserDataIterator.hh GpuLaserDataIteratorImpl.hh Grid.hh diff --git a/gazebo/rendering/GpuLaser.cc b/gazebo/rendering/GpuLaser.cc index 4d23b879bf..6fd6e1dbfe 100644 --- a/gazebo/rendering/GpuLaser.cc +++ b/gazebo/rendering/GpuLaser.cc @@ -15,12 +15,12 @@ * */ +#include #include +#include #include -#include #include -#include #include #ifndef _WIN32 @@ -32,15 +32,9 @@ #include "gazebo/rendering/ogre_gazebo.h" #include "gazebo/common/Assert.hh" -#include "gazebo/common/Events.hh" #include "gazebo/common/Console.hh" -#include "gazebo/common/Exception.hh" -#include "gazebo/common/Mesh.hh" -#include "gazebo/common/MeshManager.hh" -#include "gazebo/common/Timer.hh" #include "gazebo/rendering/Visual.hh" -#include "gazebo/rendering/Conversions.hh" #include "gazebo/rendering/Scene.hh" #include "gazebo/rendering/GpuLaser.hh" #include "gazebo/rendering/GpuLaserPrivate.hh" @@ -48,27 +42,25 @@ using namespace gazebo; using namespace rendering; -int GpuLaserPrivate::texCount = 0; - ////////////////////////////////////////////////// GpuLaser::GpuLaser(const std::string &_namePrefix, ScenePtr _scene, const bool _autoRender) -: Camera(_namePrefix, _scene, _autoRender), - dataPtr(new GpuLaserPrivate) + : Camera(_namePrefix, std::move(_scene), _autoRender) + , horzHalfAngle {0.0} + , vertHalfAngle {0.0} + , rayCountRatio {0.0} + , hfov {0.0} + , vfov {0.0} + , chfov {0.0} + , cvfov {0.0} + , nearClip {0.0} + , farClip {0.0} + , isHorizontal {false} + , dataPtr {std::make_unique()} { - this->dataPtr->laserBuffer = NULL; - this->dataPtr->laserScan = NULL; - this->dataPtr->matFirstPass = NULL; - this->dataPtr->matSecondPass = NULL; - for (int i = 0; i < 3; ++i) - this->dataPtr->firstPassTextures[i] = NULL; - this->dataPtr->secondPassTexture = NULL; - this->dataPtr->orthoCam = NULL; - this->dataPtr->w2nd = 0; - this->dataPtr->h2nd = 0; - this->cameraCount = 0; - this->dataPtr->textureCount = 0; - this->dataPtr->visual.reset(); + this->dataPtr->material = nullptr; + this->dataPtr->horizontalRangeCount = 0; + this->dataPtr->verticalRangeCount = 0; } ////////////////////////////////////////////////// @@ -93,43 +85,21 @@ void GpuLaser::Load() void GpuLaser::Init() { Camera::Init(); - this->dataPtr->visual.reset(new Visual(this->Name()+"second_pass_canvas", - this->GetScene()->WorldVisual())); } ////////////////////////////////////////////////// void GpuLaser::Fini() { - for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i) + for (const auto& [cubeFaceId, cubeFace] : this->dataPtr->cubeMapFaces) { - if (this->dataPtr->firstPassTextures[i]) + if (!cubeFace.texture.isNull()) { Ogre::TextureManager::getSingleton().remove( - this->dataPtr->firstPassTextures[i]->getName()); - this->dataPtr->firstPassTextures[i] = nullptr; + cubeFace.texture->getName()); } } - if (this->dataPtr->secondPassTexture) - { - Ogre::TextureManager::getSingleton().remove( - this->dataPtr->secondPassTexture->getName()); - this->dataPtr->secondPassTexture = nullptr; - } - - if (this->dataPtr->orthoCam) - { - this->scene->OgreSceneManager()->destroyCamera(this->dataPtr->orthoCam); - this->dataPtr->orthoCam = nullptr; - } - - this->dataPtr->visual.reset(); - this->dataPtr->texIdx.clear(); - this->dataPtr->texCount = 0; - delete [] this->dataPtr->laserBuffer; - this->dataPtr->laserBuffer = nullptr; - delete [] this->dataPtr->laserScan; - this->dataPtr->laserScan = nullptr; + this->dataPtr->laserScan.clear(); Camera::Fini(); } @@ -137,176 +107,153 @@ void GpuLaser::Fini() ////////////////////////////////////////////////// void GpuLaser::CreateLaserTexture(const std::string &_textureName) { - this->camera->yaw(Ogre::Radian(this->horzHalfAngle)); + unsigned int cubeFaceTexIndex = 0; + for (auto& [cubeFaceId, cubeFace] : this->dataPtr->cubeMapFaces) + { + std::stringstream texName; + texName << _textureName << "_cube_face_texture_" << cubeFaceTexIndex; - this->CreateOrthoCam(); + cubeFace.texture = Ogre::TextureManager::getSingleton().createManual( + texName.str(), "General", Ogre::TEX_TYPE_2D, + this->ImageWidth(), this->ImageHeight(), 0, + Ogre::PF_FLOAT32_RGB, Ogre::TU_RENDERTARGET); - this->dataPtr->textureCount = this->cameraCount; + this->SetUpRenderTarget(cubeFace); - if (this->dataPtr->textureCount == 2) - { - this->dataPtr->cameraYaws[0] = -this->hfov/2; - this->dataPtr->cameraYaws[1] = +this->hfov; - this->dataPtr->cameraYaws[2] = 0; - this->dataPtr->cameraYaws[3] = -this->hfov/2; - } - else - { - this->dataPtr->cameraYaws[0] = -this->hfov; - this->dataPtr->cameraYaws[1] = +this->hfov; - this->dataPtr->cameraYaws[2] = +this->hfov; - this->dataPtr->cameraYaws[3] = -this->hfov; + cubeFaceTexIndex++; } - for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i) - { - std::stringstream texName; - texName << _textureName << "first_pass_" << i; - this->dataPtr->firstPassTextures[i] = - Ogre::TextureManager::getSingleton().createManual( - texName.str(), "General", Ogre::TEX_TYPE_2D, - this->ImageWidth(), this->ImageHeight(), 0, - Ogre::PF_FLOAT32_RGB, Ogre::TU_RENDERTARGET).getPointer(); + this->dataPtr->material = + static_cast(Ogre::MaterialManager::getSingleton() + .getByName("Gazebo/LaserScan") + .get()); + GZ_ASSERT(this->dataPtr->material, "Couldn't find material Gazebo/LaserScan"); - this->Set1stPassTarget( - this->dataPtr->firstPassTextures[i]->getBuffer()->getRenderTarget(), i); + this->dataPtr->material->load(); + this->dataPtr->material->setCullingMode(Ogre::CULL_NONE); +} - this->dataPtr->firstPassTargets[i]->setAutoUpdated(false); +////////////////////////////////////////////////// +void GpuLaser::PostRender() +{ + for (auto& [cubeFaceId, cubeFace] : this->dataPtr->cubeMapFaces) + { + cubeFace.texture->getBuffer()->getRenderTarget()->swapBuffers(); } - this->dataPtr->matFirstPass = (Ogre::Material*)( - Ogre::MaterialManager::getSingleton().getByName("Gazebo/LaserScan1st").get()); - - this->dataPtr->matFirstPass->load(); - this->dataPtr->matFirstPass->setCullingMode(Ogre::CULL_NONE); + constexpr unsigned int numberOfChannels = 3; - this->dataPtr->secondPassTexture = - Ogre::TextureManager::getSingleton().createManual( - _textureName + "second_pass", - "General", - Ogre::TEX_TYPE_2D, - this->dataPtr->w2nd, this->dataPtr->h2nd, 0, - Ogre::PF_FLOAT32_RGB, - Ogre::TU_RENDERTARGET).getPointer(); + IGN_PROFILE_BEGIN("blitToMemory"); + // Blit the depth buffer if needed + if (this->newData && this->captureData) + { + const unsigned int imageWidth = this->ImageWidth(); + const unsigned int imageHeight = this->ImageHeight(); - this->Set2ndPassTarget( - this->dataPtr->secondPassTexture->getBuffer()->getRenderTarget()); + const size_t laserBufferSize = this->dataPtr->horizontalRangeCount * this->dataPtr->verticalRangeCount * numberOfChannels; + const size_t depthImageBufferSize = imageWidth * imageHeight * numberOfChannels; - this->dataPtr->secondPassTarget->setAutoUpdated(false); + if (this->dataPtr->laserBuffer.empty()) + { + this->dataPtr->laserBuffer.resize(laserBufferSize); + } - this->dataPtr->matSecondPass = (Ogre::Material*)( - Ogre::MaterialManager::getSingleton().getByName("Gazebo/LaserScan2nd").get()); + for (auto& [cubeFaceId, cubeFace] : this->dataPtr->cubeMapFaces) + { + cubeFace.depthImg.resize(depthImageBufferSize); - this->dataPtr->matSecondPass->load(); + Ogre::PixelBox dstBox(imageWidth, imageHeight, 1, Ogre::PF_FLOAT32_RGB, cubeFace.depthImg.data()); - Ogre::TextureUnitState *texUnit; - for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i) - { - unsigned int texIndex = this->dataPtr->texCount++; - Ogre::Technique *technique = this->dataPtr->matSecondPass->getTechnique(0); - GZ_ASSERT(technique, "GpuLaser material script error: technique not found"); + cubeFace.texture->getBuffer()->blitToMemory(dstBox); + } + IGN_PROFILE_END(); - Ogre::Pass *pass = technique->getPass(0); - GZ_ASSERT(pass, "GpuLaser material script error: pass not found"); + // read ranges + GZ_ASSERT(this->dataPtr->horizontalRangeCount == this->dataPtr->mapping.size(), "cube face mapping size doesn't match number of horizontal rays"); - if (!pass->getTextureUnitState( - this->dataPtr->firstPassTextures[i]->getName())) + IGN_PROFILE_BEGIN("applyMapping"); + for (unsigned int azimuthIndex = 0; azimuthIndex < this->dataPtr->horizontalRangeCount; ++azimuthIndex) { - texUnit = pass->createTextureUnitState( - this->dataPtr->firstPassTextures[i]->getName(), texIndex); + GZ_ASSERT(this->dataPtr->verticalRangeCount == this->dataPtr->mapping[azimuthIndex].size(), "cube face mapping size doesn't match number of vertical rays"); - this->dataPtr->texIdx.push_back(texIndex); - - texUnit->setTextureFiltering(Ogre::TFO_NONE); - texUnit->setTextureAddressingMode(Ogre::TextureUnitState::TAM_MIRROR); - } - } - - this->CreateCanvas(); -} + for (unsigned int elevationIndex = 0; elevationIndex < this->dataPtr->verticalRangeCount; ++elevationIndex) + { + const auto& [cubeFaceId, faceCoordinates] = this->dataPtr->mapping[azimuthIndex][elevationIndex]; -////////////////////////////////////////////////// -void GpuLaser::PostRender() -{ - for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i) - { - this->dataPtr->firstPassTargets[i]->swapBuffers(); - } - this->dataPtr->secondPassTarget->swapBuffers(); + GZ_ASSERT(this->dataPtr->cubeMapFaces.find(cubeFaceId) != this->dataPtr->cubeMapFaces.end(), "Required cube face has not been initialized"); - if (this->newData && this->captureData) - { - Ogre::HardwarePixelBufferSharedPtr pixelBuffer; + const auto &cubeFace = this->dataPtr->cubeMapFaces.at(cubeFaceId); - unsigned int width = this->dataPtr->secondPassViewport->getActualWidth(); - unsigned int height = this->dataPtr->secondPassViewport->getActualHeight(); + // pixel coordinates + const auto imageX = static_cast(faceCoordinates.X() * (imageWidth - 1)); + const auto imageY = static_cast(faceCoordinates.Y() * (imageHeight - 1)); - // Get access to the buffer and make an image and write it to file - pixelBuffer = this->dataPtr->secondPassTexture->getBuffer(); + GZ_ASSERT(imageX < imageWidth, "Calculated image X exceeds image width"); + GZ_ASSERT(imageY < imageHeight, "Calculated image Y exceeds image height"); - size_t size = Ogre::PixelUtil::getMemorySize( - width, height, 1, Ogre::PF_FLOAT32_RGB); + const unsigned int depthImageBufferIndex = (imageY * imageWidth + imageX) * numberOfChannels; - // Blit the depth buffer if needed - if (!this->dataPtr->laserBuffer) - this->dataPtr->laserBuffer = new float[size]; + GZ_ASSERT(depthImageBufferIndex + 2 < cubeFace.depthImg.size(), "Depth image buffer index is out of range"); - memset(this->dataPtr->laserBuffer, 255, size); + const unsigned int laserBufferIndex = (elevationIndex * this->dataPtr->horizontalRangeCount + azimuthIndex) * numberOfChannels; - Ogre::PixelBox dstBox(width, height, - 1, Ogre::PF_FLOAT32_RGB, this->dataPtr->laserBuffer); + GZ_ASSERT(laserBufferIndex + 2 < this->dataPtr->laserBuffer.size(), "Laser buffer index is out of range"); - pixelBuffer->blitToMemory(dstBox); + this->dataPtr->laserBuffer[laserBufferIndex] = cubeFace.depthImg[depthImageBufferIndex]; + this->dataPtr->laserBuffer[laserBufferIndex + 1] = cubeFace.depthImg[depthImageBufferIndex + 1]; + this->dataPtr->laserBuffer[laserBufferIndex + 2] = cubeFace.depthImg[depthImageBufferIndex + 2]; + } + } + IGN_PROFILE_END(); - if (!this->dataPtr->laserScan) - { - int len = this->dataPtr->w2nd * this->dataPtr->h2nd * 3; - this->dataPtr->laserScan = new float[len]; + IGN_PROFILE_BEGIN("copyBufferToScan"); + if (this->dataPtr->laserScan.size() != laserBufferSize) { + this->dataPtr->laserScan.resize(laserBufferSize); } - memcpy(this->dataPtr->laserScan, this->dataPtr->laserBuffer, - this->dataPtr->w2nd * this->dataPtr->h2nd * 3 * - sizeof(this->dataPtr->laserScan[0])); + std::copy(this->dataPtr->laserBuffer.begin(), + this->dataPtr->laserBuffer.end(), + this->dataPtr->laserScan.begin()); + IGN_PROFILE_END(); - this->dataPtr->newLaserFrame(this->dataPtr->laserScan, this->dataPtr->w2nd, - this->dataPtr->h2nd, 3, "BLABLA"); + this->dataPtr->newLaserFrame( + this->dataPtr->laserScan.data(), this->dataPtr->horizontalRangeCount, + this->dataPtr->verticalRangeCount, 3, "BLABLA"); } this->newData = false; } ///////////////////////////////////////////////// -void GpuLaser::UpdateRenderTarget(Ogre::RenderTarget *_target, - Ogre::Material *_material, Ogre::Camera *_cam, - const bool _updateTex) +void GpuLaser::UpdateRenderTarget(GpuLaserCubeFace &_cube_face) { - Ogre::RenderSystem *renderSys; - Ogre::Viewport *vp = NULL; + Ogre::RenderTarget *renderTarget = _cube_face.texture->getBuffer()->getRenderTarget(); Ogre::SceneManager *sceneMgr = this->scene->OgreSceneManager(); - Ogre::Pass *pass; + Ogre::RenderSystem *renderSys = sceneMgr->getDestinationRenderSystem(); + + this->dataPtr->currentTarget = renderTarget; - renderSys = this->scene->OgreSceneManager()->getDestinationRenderSystem(); // Get pointer to the material pass - pass = _material->getBestTechnique()->getPass(0); + Ogre::Pass *pass = this->dataPtr->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->FarClip()); + this->camera->setFarClipDistance(static_cast(this->FarClip())); Ogre::AutoParamDataSource autoParamDataSource; - vp = _target->getViewport(0); + Ogre::Viewport *vp = renderTarget->getViewport(0); // Need this line to render the ground plane. No idea why it's necessary. renderSys->_setViewport(vp); sceneMgr->_setPass(pass, true, false); autoParamDataSource.setCurrentPass(pass); autoParamDataSource.setCurrentViewport(vp); - autoParamDataSource.setCurrentRenderTarget(_target); + autoParamDataSource.setCurrentRenderTarget(renderTarget); autoParamDataSource.setCurrentSceneManager(sceneMgr); - autoParamDataSource.setCurrentCamera(_cam, true); + autoParamDataSource.setCurrentCamera(this->camera, true); renderSys->setLightingEnabled(false); renderSys->_setFog(Ogre::FOG_NONE); @@ -317,20 +264,6 @@ void GpuLaser::UpdateRenderTarget(Ogre::RenderTarget *_target, pass->_updateAutoParams(&autoParamDataSource, 1); #endif - if (_updateTex) - { - pass->getFragmentProgramParameters()->setNamedConstant("tex1", - this->dataPtr->texIdx[0]); - if (this->dataPtr->texIdx.size() > 1) - { - pass->getFragmentProgramParameters()->setNamedConstant("tex2", - this->dataPtr->texIdx[1]); - if (this->dataPtr->texIdx.size() > 2) - pass->getFragmentProgramParameters()->setNamedConstant("tex3", - this->dataPtr->texIdx[2]); - } - } - // NOTE: We MUST bind parameters AFTER updating the autos if (pass->hasVertexProgram()) { @@ -359,6 +292,8 @@ void GpuLaser::UpdateRenderTarget(Ogre::RenderTarget *_target, pass->getFragmentProgramParameters(), 1); #endif } + + renderTarget->update(false); } ///////////////////////////////////////////////// @@ -376,7 +311,7 @@ void GpuLaser::notifyRenderSingleObject(Ogre::Renderable *_rend, _rend->setCustomParameter(1, Ogre::Vector4(0, 0, 0, 0)); } - Ogre::Pass *pass = this->dataPtr->currentMat->getBestTechnique()->getPass(0); + Ogre::Pass *pass = this->dataPtr->material->getBestTechnique()->getPass(0); Ogre::RenderSystem *renderSys = this->scene->OgreSceneManager()->getDestinationRenderSystem(); @@ -414,52 +349,20 @@ void GpuLaser::notifyRenderSingleObject(Ogre::Renderable *_rend, void GpuLaser::RenderImpl() { IGN_PROFILE("rendering::GpuLaser::RenderImpl"); - common::Timer firstPassTimer, secondPassTimer; - - firstPassTimer.Start(); - Ogre::SceneManager *sceneMgr = this->scene->OgreSceneManager(); sceneMgr->_suppressRenderStateChanges(true); sceneMgr->addRenderObjectListener(this); - for (unsigned int i = 0; i < this->dataPtr->textureCount; ++i) + for (auto& [cubeFaceId, cubeFace] : this->dataPtr->cubeMapFaces) { - if (this->dataPtr->textureCount > 1) - { - // Cannot call Camera::RotateYaw because it rotates in world frame, - // but we need rotation in camera local frame - this->sceneNode->roll(Ogre::Radian(this->dataPtr->cameraYaws[i])); - } - - this->dataPtr->currentMat = this->dataPtr->matFirstPass; - this->dataPtr->currentTarget = this->dataPtr->firstPassTargets[i]; - - this->UpdateRenderTarget(this->dataPtr->firstPassTargets[i], - this->dataPtr->matFirstPass, this->camera); - this->dataPtr->firstPassTargets[i]->update(false); + this->ApplyCameraSetting(cubeFace.cameraSetting); + this->UpdateRenderTarget(cubeFace); + this->RevertCameraSetting(cubeFace.cameraSetting); } - if (this->dataPtr->textureCount > 1) - this->sceneNode->roll(Ogre::Radian(this->dataPtr->cameraYaws[3])); - sceneMgr->removeRenderObjectListener(this); - - double firstPassDur = firstPassTimer.GetElapsed().Double(); - secondPassTimer.Start(); - - this->dataPtr->visual->SetVisible(true); - - this->UpdateRenderTarget(this->dataPtr->secondPassTarget, - this->dataPtr->matSecondPass, this->dataPtr->orthoCam, true); - this->dataPtr->secondPassTarget->update(false); - - this->dataPtr->visual->SetVisible(false); - sceneMgr->_suppressRenderStateChanges(false); - - double secondPassDur = secondPassTimer.GetElapsed().Double(); - this->dataPtr->lastRenderDuration = firstPassDur + secondPassDur; } ////////////////////////////////////////////////// @@ -472,14 +375,14 @@ GpuLaser::DataIter GpuLaser::LaserDataBegin() const const unsigned int rangeOffset = 0; // intensity data in G channel const unsigned int intenOffset = 1; - return DataIter(index, this->dataPtr->laserBuffer, skip, rangeOffset, - intenOffset, this->dataPtr->w2nd); + return {index, this->dataPtr->laserBuffer.data(), skip, rangeOffset, + intenOffset, this->dataPtr->horizontalRangeCount}; } ////////////////////////////////////////////////// GpuLaser::DataIter GpuLaser::LaserDataEnd() const { - const unsigned int index = this->dataPtr->h2nd * this->dataPtr->w2nd; + const unsigned int index = this->dataPtr->verticalRangeCount * this->dataPtr->horizontalRangeCount; // Data stuffed into three floats (RGB) const unsigned int skip = 3; @@ -487,273 +390,39 @@ GpuLaser::DataIter GpuLaser::LaserDataEnd() const const unsigned int rangeOffset = 0; // intensity data in G channel const unsigned int intenOffset = 1; - return DataIter(index, this->dataPtr->laserBuffer, skip, rangeOffset, - intenOffset, this->dataPtr->w2nd); -} - -///////////////////////////////////////////////// -void GpuLaser::CreateOrthoCam() -{ - this->dataPtr->pitchNodeOrtho = - this->GetScene()->WorldVisual()->GetSceneNode()->createChildSceneNode(); - - this->dataPtr->orthoCam = this->scene->OgreSceneManager()->createCamera( - this->dataPtr->pitchNodeOrtho->getName() + "_ortho_cam"); - - // Use X/Y as horizon, Z up - this->dataPtr->orthoCam->pitch(Ogre::Degree(90)); - - // Don't yaw along variable axis, causes leaning - this->dataPtr->orthoCam->setFixedYawAxis(true, Ogre::Vector3::UNIT_Z); - - this->dataPtr->orthoCam->setDirection(1, 0, 0); - - this->dataPtr->pitchNodeOrtho->attachObject(this->dataPtr->orthoCam); - this->dataPtr->orthoCam->setAutoAspectRatio(true); - - if (this->dataPtr->orthoCam) - { - this->dataPtr->orthoCam->setNearClipDistance(0.01); - this->dataPtr->orthoCam->setFarClipDistance(0.02); - this->dataPtr->orthoCam->setRenderingDistance(0.02); - - this->dataPtr->orthoCam->setProjectionType(Ogre::PT_ORTHOGRAPHIC); - } -} - -///////////////////////////////////////////////// -Ogre::Matrix4 GpuLaser::BuildScaledOrthoMatrix(const float _left, - const float _right, const float _bottom, const float _top, - const float _near, const float _far) -{ - float invw = 1 / (_right - _left); - float invh = 1 / (_top - _bottom); - float invd = 1 / (_far - _near); - - Ogre::Matrix4 proj = Ogre::Matrix4::ZERO; - proj[0][0] = 2 * invw; - proj[0][3] = -(_right + _left) * invw; - proj[1][1] = 2 * invh; - proj[1][3] = -(_top + _bottom) * invh; - proj[2][2] = -2 * invd; - proj[2][3] = -(_far + _near) * invd; - proj[3][3] = 1; - - return proj; + return {index, this->dataPtr->laserBuffer.data(), skip, rangeOffset, + intenOffset, this->dataPtr->horizontalRangeCount}; } ////////////////////////////////////////////////// -void GpuLaser::Set1stPassTarget(Ogre::RenderTarget *_target, - const unsigned int _index) +void GpuLaser::SetUpRenderTarget(GpuLaserCubeFace &_cube_face) { - this->dataPtr->firstPassTargets[_index] = _target; + Ogre::RenderTarget *renderTarget = _cube_face.texture->getBuffer()->getRenderTarget(); - if (this->dataPtr->firstPassTargets[_index]) + if (renderTarget) { // Setup the viewport to use the texture - this->dataPtr->firstPassViewports[_index] = - this->dataPtr->firstPassTargets[_index]->addViewport(this->camera); - this->dataPtr->firstPassViewports[_index]->setClearEveryFrame(true); - this->dataPtr->firstPassViewports[_index]->setOverlaysEnabled(false); - this->dataPtr->firstPassViewports[_index]->setShadowsEnabled(false); - this->dataPtr->firstPassViewports[_index]->setSkiesEnabled(false); - this->dataPtr->firstPassViewports[_index]->setBackgroundColour( - Ogre::ColourValue(this->farClip, 0.0, 1.0)); - this->dataPtr->firstPassViewports[_index]->setVisibilityMask( + Ogre::Viewport *viewport = renderTarget->addViewport(this->camera); + viewport->setClearEveryFrame(true); + viewport->setOverlaysEnabled(false); + viewport->setShadowsEnabled(false); + viewport->setSkiesEnabled(false); + viewport->setBackgroundColour(Ogre::ColourValue(static_cast(this->farClip), 0.0, 1.0)); + viewport->setVisibilityMask( GZ_VISIBILITY_ALL & ~(GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE)); } - if (_index == 0) - { - this->camera->setAspectRatio(this->RayCountRatio()); - this->camera->setFOVy(Ogre::Radian(this->LimitFOV(this->CosVertFOV()))); - } -} - -////////////////////////////////////////////////// -void GpuLaser::Set2ndPassTarget(Ogre::RenderTarget *_target) -{ - this->dataPtr->secondPassTarget = _target; - - if (this->dataPtr->secondPassTarget) - { - // Setup the viewport to use the texture - this->dataPtr->secondPassViewport = - this->dataPtr->secondPassTarget->addViewport(this->dataPtr->orthoCam); - this->dataPtr->secondPassViewport->setClearEveryFrame(true); - this->dataPtr->secondPassViewport->setOverlaysEnabled(false); - this->dataPtr->secondPassViewport->setShadowsEnabled(false); - this->dataPtr->secondPassViewport->setSkiesEnabled(false); - this->dataPtr->secondPassViewport->setBackgroundColour( - Ogre::ColourValue(0.0, 1.0, 0.0)); - this->dataPtr->secondPassViewport->setVisibilityMask( - GZ_VISIBILITY_ALL & ~(GZ_VISIBILITY_GUI | GZ_VISIBILITY_SELECTABLE)); - } - Ogre::Matrix4 p = this->BuildScaledOrthoMatrix( - 0, static_cast(this->dataPtr->w2nd / 10.0), - 0, static_cast(this->dataPtr->h2nd / 10.0), - 0.01, 0.02); + this->camera->setAspectRatio(static_cast(this->RayCountRatio())); + this->camera->setFOVy(Ogre::Radian(static_cast(this->CosVertFOV()))); - this->dataPtr->orthoCam->setCustomProjectionMatrix(true, p); + renderTarget->setAutoUpdated(false); } ///////////////////////////////////////////////// void GpuLaser::SetRangeCount(const unsigned int _w, const unsigned int _h) { - this->dataPtr->w2nd = _w; - this->dataPtr->h2nd = _h; -} - -///////////////////////////////////////////////// -void GpuLaser::CreateMesh() -{ - std::string meshName = this->Name() + "_undistortion_mesh"; - - common::Mesh *mesh = new common::Mesh(); - mesh->SetName(meshName); - - common::SubMesh *submesh = new common::SubMesh(); - - double dx, dy; - submesh->SetPrimitiveType(common::SubMesh::POINTS); - - if (this->dataPtr->h2nd == 1) - { - dy = 0; - } - else - { - dy = 0.1; - } - - dx = 0.1; - - // startX ranges from 0 to -(w2nd/10) at dx=0.1 increments - // startY ranges from h2nd/10 to 0 at dy=0.1 decrements - // see GpuLaser::Set2ndPassTarget() on how the ortho cam is set up - double startX = dx; - double startY = this->dataPtr->h2nd/10.0; - - // half of actual camera vertical FOV without padding - double phi = this->VertFOV() / 2; - double phiCamera = phi + std::abs(this->VertHalfAngle()); - double theta = this->CosHorzFOV() / 2; - - if (this->ImageHeight() == 1) - { - phi = 0; - } - - // index of ray - unsigned int ptsOnLine = 0; - - // total laser hfov - double thfov = this->dataPtr->textureCount * this->CosHorzFOV(); - double hstep = thfov / (this->dataPtr->w2nd - 1); - double vstep = 2 * phi / (this->dataPtr->h2nd - 1); - - if (this->dataPtr->h2nd == 1) - { - vstep = 0; - } - - for (unsigned int j = 0; j < this->dataPtr->h2nd; ++j) - { - double gamma = 0; - if (this->dataPtr->h2nd != 1) - { - // gamma: current vertical angle w.r.t. camera - gamma = vstep * j - phi + this->VertHalfAngle(); - } - - for (unsigned int i = 0; i < this->dataPtr->w2nd; ++i) - { - // current horizontal angle from start of laser scan - double delta = hstep * i; - - // index of texture that contains the depth value - unsigned int texture = delta / this->CosHorzFOV(); - - // cap texture index and horizontal angle - if (texture > this->dataPtr->textureCount-1) - { - texture -= 1; - delta -= hstep; - } - - startX -= dx; - if (ptsOnLine == this->dataPtr->w2nd) - { - ptsOnLine = 0; - startX = 0; - startY -= dy; - } - ptsOnLine++; - - // the texture/1000.0 value is used in the laser_2nd_pass.frag shader - // as a trick to determine which camera texture to use when stitching - // together the final depth image. - submesh->AddVertex(texture/1000.0, startX, startY); - - // first compute angle from the start of current camera's horizontal - // min angle, then set delta to be angle from center of current camera. - delta = delta - (texture * this->CosHorzFOV()); - delta = delta - theta; - - // adjust uv coordinates of depth texture to match projection of current - // laser ray the depth image plane. - double u = 0.5 - tan(delta) / (2.0 * tan(theta)); - double v = 0.5 - (tan(gamma) * cos(theta)) / - (2.0 * tan(phiCamera) * cos(delta)); - - submesh->AddTexCoord(u, v); - submesh->AddIndex(this->dataPtr->w2nd * j + i); - } - } - - mesh->AddSubMesh(submesh); - - this->dataPtr->undistMesh = mesh; - - common::MeshManager::Instance()->AddMesh(this->dataPtr->undistMesh); -} - -///////////////////////////////////////////////// -void GpuLaser::CreateCanvas() -{ - this->CreateMesh(); - - Ogre::Node *parent = this->dataPtr->visual->GetSceneNode()->getParent(); - parent->removeChild(this->dataPtr->visual->GetSceneNode()); - - this->dataPtr->pitchNodeOrtho->addChild( - this->dataPtr->visual->GetSceneNode()); - - this->dataPtr->visual->InsertMesh(this->dataPtr->undistMesh); - - std::ostringstream stream; - std::string meshName = this->dataPtr->undistMesh->GetName(); - stream << this->dataPtr->visual->GetSceneNode()->getName() - << "_ENTITY_" << meshName; - - this->dataPtr->object = (Ogre::MovableObject*) - (this->dataPtr->visual->GetSceneNode()->getCreator()->createEntity( - stream.str(), meshName)); - - this->dataPtr->visual->AttachObject(this->dataPtr->object); - this->dataPtr->object->setVisibilityFlags(GZ_VISIBILITY_ALL - & ~GZ_VISIBILITY_SELECTABLE); - - ignition::math::Pose3d pose; - pose.Pos().Set(0.01, 0, 0); - pose.Rot().Euler(ignition::math::Vector3d(0, 0, 0)); - - this->dataPtr->visual->SetPose(pose); - - this->dataPtr->visual->SetDiffuse(ignition::math::Color(0, 1, 0, 0)); - this->dataPtr->visual->SetAmbient(ignition::math::Color(0, 1, 0, 0)); - this->dataPtr->visual->SetVisible(true); - this->scene->AddVisual(this->dataPtr->visual); + this->dataPtr->horizontalRangeCount = _w; + this->dataPtr->verticalRangeCount = _h; } ////////////////////////////////////////////////// @@ -873,7 +542,7 @@ unsigned int GpuLaser::CameraCount() const ////////////////////////////////////////////////// void GpuLaser::SetCameraCount(const unsigned int _cameraCount) { - this->cameraCount = _cameraCount; + /*unused*/ } ////////////////////////////////////////////////// @@ -896,3 +565,194 @@ event::ConnectionPtr GpuLaser::ConnectNewLaserFrame( { return this->dataPtr->newLaserFrame.Connect(_subscriber); } + +////////////////////////////////////////////////// +void GpuLaser::ApplyCameraSetting(const GpuLaserCameraOrientationOffset &_setting) +{ + this->sceneNode->roll(Ogre::Radian(static_cast(_setting.azimuthOffset))); + this->sceneNode->yaw(Ogre::Radian(static_cast(_setting.elevationOffset))); +} + +////////////////////////////////////////////////// +void GpuLaser::RevertCameraSetting(const GpuLaserCameraOrientationOffset &_setting) +{ + this->sceneNode->yaw(Ogre::Radian(static_cast(-_setting.elevationOffset))); + this->sceneNode->roll(Ogre::Radian(static_cast(-_setting.azimuthOffset))); +} + +////////////////////////////////////////////////// +void GpuLaser::InitMapping(const std::set &_azimuth_values, const std::set &_elevation_values) +{ + this->dataPtr->mapping.clear(); + this->dataPtr->mapping.reserve(_azimuth_values.size()); + + if (_azimuth_values.empty()) + { + return; + } + + const double min_azimuth = *_azimuth_values.begin(); + const double front_face_azimuth = min_azimuth + M_PI_4; + + // define face orientations w.r.t. front face + const std::map cube_camera_orientations { + {GpuLaserCubeFaceId::CUBE_FRONT_FACE, {front_face_azimuth, 0.}}, + {GpuLaserCubeFaceId::CUBE_LEFT_FACE, {front_face_azimuth + M_PI_2, 0.0}}, + {GpuLaserCubeFaceId::CUBE_REAR_FACE, {front_face_azimuth + M_PI, 0.0}}, + {GpuLaserCubeFaceId::CUBE_RIGHT_FACE, {front_face_azimuth + M_PI + M_PI_2, 0.0}}, + {GpuLaserCubeFaceId::CUBE_TOP_FACE, {front_face_azimuth, -M_PI_2}}, + {GpuLaserCubeFaceId::CUBE_BOTTOM_FACE, {front_face_azimuth, M_PI_2}}, + }; + + for (const double azimuth : _azimuth_values) + { + this->dataPtr->mapping.emplace_back(); + this->dataPtr->mapping.back().reserve(_elevation_values.size()); + + for (const double elevation : _elevation_values) + { + const auto& [cube_face_id, face_coordinates] = GpuLaser::FindCubeFaceMapping(azimuth - min_azimuth, elevation); + this->dataPtr->mapping.back().emplace_back(cube_face_id, face_coordinates); + + // create cube map faces if necessary + if (this->dataPtr->cubeMapFaces.find(cube_face_id) == + this->dataPtr->cubeMapFaces.end()) { + GpuLaserCubeFace face; + face.cameraSetting = cube_camera_orientations.at(cube_face_id); + this->dataPtr->cubeMapFaces.insert({cube_face_id, face}); + } + } + } + + this->cameraCount = this->dataPtr->cubeMapFaces.size(); +} + +////////////////////////////////////////////////// +GpuLaserCubeMappingPoint GpuLaser::FindCubeFaceMapping(const double _azimuth, const double _elevation) +{ + if (_azimuth < 0) + { + throw std::runtime_error("Azimuth angle should be relative to minimum angle, i.e. it must not be negative!"); + } + + const GpuLaserCubeFaceId face_id = GpuLaser::FindCubeFace(_azimuth, _elevation); + + // center point of the face plane + ignition::math::Vector3d plane_point; + switch (face_id) + { + case GpuLaserCubeFaceId::CUBE_FRONT_FACE: + plane_point = {0.5, 0., 0.}; + break; + case GpuLaserCubeFaceId::CUBE_LEFT_FACE: + plane_point = {0., 0.5, 0.}; + break; + case GpuLaserCubeFaceId::CUBE_REAR_FACE: + plane_point = {-0.5, 0., 0.}; + break; + case GpuLaserCubeFaceId::CUBE_RIGHT_FACE: + plane_point = {0., -0.5, 0.}; + break; + case GpuLaserCubeFaceId::CUBE_TOP_FACE: + plane_point = {0., 0., 0.5}; + break; + case GpuLaserCubeFaceId::CUBE_BOTTOM_FACE: + plane_point = {0., 0., -0.5}; + break; + default: + throw std::runtime_error("Invalid face ID"); + } + + const ignition::math::Vector3d plane_normal = plane_point.Normalized(); + const ignition::math::Vector3d viewing_ray = GpuLaser::ViewingRay(_azimuth, _elevation); + + // calculate intersection of viewing ray with cube face plane + const double s = (-plane_normal).Dot(-plane_point) / plane_normal.Dot(viewing_ray); + + // offset from face center to intersection + const ignition::math::Vector3d intersection_offset = s * viewing_ray - plane_point; + + // offset in the 2D image on the face + ignition::math::Vector2d intersection_image_offset; + switch (face_id) + { + case GpuLaserCubeFaceId::CUBE_FRONT_FACE: + intersection_image_offset = {-intersection_offset.Y(), + -intersection_offset.Z()}; + break; + case GpuLaserCubeFaceId::CUBE_LEFT_FACE: + intersection_image_offset = {intersection_offset.X(), + -intersection_offset.Z()}; + break; + case GpuLaserCubeFaceId::CUBE_REAR_FACE: + intersection_image_offset = {intersection_offset.Y(), + -intersection_offset.Z()}; + break; + case GpuLaserCubeFaceId::CUBE_RIGHT_FACE: + intersection_image_offset = {-intersection_offset.X(), + -intersection_offset.Z()}; + break; + case GpuLaserCubeFaceId::CUBE_TOP_FACE: + intersection_image_offset = {-intersection_offset.Y(), + intersection_offset.X()}; + break; + case GpuLaserCubeFaceId::CUBE_BOTTOM_FACE: + intersection_image_offset = {-intersection_offset.Y(), + -intersection_offset.X()}; + break; + default: + throw std::runtime_error("Invalid face ID"); + } + + // shift offset from image center to origin + intersection_image_offset += {0.5, 0.5}; + + intersection_image_offset.Set(ignition::math::clamp(intersection_image_offset.X(), 0., 1.), + ignition::math::clamp(intersection_image_offset.Y(), 0., 1.)); + + return {face_id, intersection_image_offset}; +} + +////////////////////////////////////////////////// +GpuLaserCubeFaceId GpuLaser::FindCubeFace(const double _azimuth, const double _elevation) +{ + const ignition::math::Vector3d v = GpuLaser::ViewingRay(_azimuth, _elevation); + + // find corresponding cube face + if (std::abs(v.Z()) > std::abs(v.X()) && std::abs(v.Z()) > std::abs(v.Y())) + { + if (v.Z() >= 0) + { + return GpuLaserCubeFaceId::CUBE_TOP_FACE; + } + else + { + return GpuLaserCubeFaceId::CUBE_BOTTOM_FACE; + } + } + else if (_azimuth < M_PI_2) + { + return GpuLaserCubeFaceId::CUBE_FRONT_FACE; + } + else if (_azimuth >= M_PI_2 && _azimuth < M_PI) + { + return GpuLaserCubeFaceId::CUBE_LEFT_FACE; + } + else if (_azimuth >= M_PI && _azimuth < 3. * M_PI_2) + { + return GpuLaserCubeFaceId::CUBE_REAR_FACE; + } + else + { + return GpuLaserCubeFaceId::CUBE_RIGHT_FACE; + } +} + +////////////////////////////////////////////////// +ignition::math::Vector3d GpuLaser::ViewingRay(const double _azimuth, const double _elevation) +{ + /// subtracting M_PI_4 from azimuth because cube face horizontal center correspondents to azimuth = M_PI_4 + return {std::cos(_azimuth - M_PI_4) * std::cos(_elevation), + std::sin(_azimuth - M_PI_4) * std::cos(_elevation), + std::sin(_elevation)}; +} diff --git a/gazebo/rendering/GpuLaser.hh b/gazebo/rendering/GpuLaser.hh index 5b8b440416..029dcff6b7 100644 --- a/gazebo/rendering/GpuLaser.hh +++ b/gazebo/rendering/GpuLaser.hh @@ -17,34 +17,30 @@ #ifndef _GAZEBO_RENDERING_GPULASER_HH_ #define _GAZEBO_RENDERING_GPULASER_HH_ +#include #include +#include #include +#include #include #include "gazebo/rendering/ogre_gazebo.h" #include "gazebo/rendering/Camera.hh" +#include "gazebo/rendering/GpuLaserCubeFace.hh" #include "gazebo/rendering/GpuLaserDataIterator.hh" #include "gazebo/rendering/RenderTypes.hh" #include "gazebo/util/system.hh" namespace Ogre { - class Material; class Renderable; class Pass; class AutoParamDataSource; - class Matrix4; - class MovableObject; } namespace gazebo { - namespace common - { - class Mesh; - } - namespace rendering { // Forward declare private data. @@ -66,31 +62,31 @@ namespace gazebo ScenePtr _scene, const bool _autoRender = true); /// \brief Destructor - public: virtual ~GpuLaser(); + public: ~GpuLaser() override; // Documentation inherited - public: virtual void Load(sdf::ElementPtr _sdf); + public: void Load(sdf::ElementPtr _sdf) override; // Documentation inherited - public: virtual void Load(); + public: void Load() override; // Documentation inherited - public: virtual void Init(); + public: void Init() override; // Documentation inherited - public: virtual void Fini(); + public: void Fini() override; /// \brief Create the texture which is used to render laser data. /// \param[in] _textureName Name of the new texture. public: void CreateLaserTexture(const std::string &_textureName); // Documentation inherited - public: virtual void PostRender(); + public: void PostRender() override; /// \brief Constant iterator to access laser data public: typedef GpuLaserDataIterator DataIter; - /// \brief Return an iterator to the begining of the laser data + /// \brief Return an iterator to the beginning of the laser data public: DataIter LaserDataBegin() const; /// \brief Return an iterator to one past the end of the laser data @@ -105,8 +101,7 @@ namespace gazebo unsigned int _height, unsigned int _depth, const std::string &_format)> _subscriber); - /// \brief Set the number of samples in the width and height for the - /// first pass texture. + /// \brief Set the number of samples in width and height. /// \param[in] _w Number of samples in the horizontal sweep /// \param[in] _h Number of samples in the vertical sweep public: void SetRangeCount(const unsigned int _w, @@ -114,9 +109,9 @@ namespace gazebo /// \internal /// \brief Implementation of Ogre::RenderObjectListener - public: virtual void notifyRenderSingleObject(Ogre::Renderable *_rend, + public: void notifyRenderSingleObject(Ogre::Renderable *_rend, const Ogre::Pass *_p, const Ogre::AutoParamDataSource *_s, - const Ogre::LightList *_ll, bool _supp); + const Ogre::LightList *_ll, bool _supp) override; /// \brief Get (horizontal_max_angle + horizontal_min_angle) * 0.5 /// \return (horizontal_max_angle + horizontal_min_angle) * 0.5 @@ -194,10 +189,14 @@ namespace gazebo /// \return Number of cameras needed to generate the rays public: unsigned int CameraCount() const; - /// \brief Set the number of cameras required + /// \brief Set the number of cameras required. Has no effect for this + /// implementation since the number of cameras is calculated based on the + /// rays. + /// \deprecated The camera count cannot be set from here anymore since it + /// is determined automatically in GpuLaser::InitMapping. /// \param[in] _cameraCount The number of cameras required to generate /// the rays - public: void SetCameraCount(const unsigned int _cameraCount); + public: void SetCameraCount(const unsigned int _cameraCount) GAZEBO_DEPRECATED(11.10); /// \brief Get the ray count ratio (equivalent to aspect ratio) /// \return The ray count ratio (equivalent to aspect ratio) @@ -207,49 +206,56 @@ namespace gazebo /// \param[in] _rayCountRatio ray count ratio (equivalent to aspect ratio) public: void SetRayCountRatio(const double _rayCountRatio); + /// \brief Initializes the mapping of ray angles to cube map coordinates. + /// Each combination of values (azimuth, elevation) corresponds to one + /// laser ray. + /// \param[in] _azimuth_values Set of azimuth angles (radians). The order matters! + /// \param[in] _elevation_values Set of elevation angles (radians). The order matters! + public: void InitMapping(const std::set &_azimuth_values, const std::set &_elevation_values); + + /// \brief Finds the corresponding cube map face and the coordinates of + /// intersection of the view ray. + /// \note The azimuth must be specified relative to the minimum azimuth value! + /// \param[in] _azimuth Horizontal angle (radians) relative to minimum azimuth angle + /// \param[in] _elevation Vertical angle (radians) where zero is orthogonal to the spin axis + /// \returns Mapping for the given ray + public: static GpuLaserCubeMappingPoint FindCubeFaceMapping(const double _azimuth, const double _elevation); + + /// \brief Finds the corresponding face of the cube map for a pair of + /// azimuth and elevation angles. + /// \note The azimuth must be specified relative to the minimum azimuth value! + /// \param[in] _azimuth Horizontal angle (radians) relative to minimum azimuth angle + /// \param[in] _elevation Vertical angle (radians) where zero is orthogonal to the spin axis + /// \returns Identifier for the corresponding face. + public: static GpuLaserCubeFaceId FindCubeFace(const double _azimuth, const double _elevation); + + /// \brief Calculates a vector in the direction of the ray. + /// \note The azimuth must be specified relative to the minimum azimuth value! + /// \param[in] _azimuth Horizontal angle (radians) relative to minimum azimuth angle + /// \param[in] _elevation Vertical angle (radians) where zero is orthogonal to the spin axis + /// \returns Viewing ray vector + public: static ignition::math::Vector3d ViewingRay(const double _azimuth, const double _elevation); + // Documentation inherited. - private: virtual void RenderImpl(); + private: void RenderImpl() override; /// \brief Update a render target. - /// \param[in, out] _target Render target to update (render). - /// \param[in, out] _material Material used during render. - /// \param[in] _cam Camera to render from. - /// \param[in] _updateTex True to update the textures in the material - private: void UpdateRenderTarget(Ogre::RenderTarget *_target, - Ogre::Material *_material, - Ogre::Camera *_cam, - const bool _updateTex = false); - - /// \brief Create an ortho camera. - private: void CreateOrthoCam(); - - /// \brief Create a mesh. - private: void CreateMesh(); - - /// \brief Create a canvas. - private: void CreateCanvas(); - - /// \brief Builds scaled Orthogonal Matrix from parameters. - /// \param[in] _left Left clip. - /// \param[in] _right Right clip. - /// \param[in] _bottom Bottom clip. - /// \param[in] _top Top clip. - /// \param[in] _near Near clip. - /// \param[in] _far Far clip. - /// \return The Scaled orthogonal Ogre::Matrix4 - private: Ogre::Matrix4 BuildScaledOrthoMatrix(const float _left, - const float _right, const float _bottom, const float _top, - const float _near, const float _far); - - /// \brief Sets first pass target. - /// \param[in] _target Render target for the first pass. - /// \param[in] _index Index of the texture. - private: virtual void Set1stPassTarget(Ogre::RenderTarget *_target, - const unsigned int _index); - - /// \brief Sets second pass target. - /// \param[in] _target Render target for the second pass. - private: virtual void Set2ndPassTarget(Ogre::RenderTarget *_target); + /// \param[in, out] _cube_face Cube face for which to update the render + /// target. + private: void UpdateRenderTarget(GpuLaserCubeFace &_cube_face); + + /// \brief Setup the render target for the specified cube face. + /// \param[in] cube_face The cube face. + private: virtual void SetUpRenderTarget(GpuLaserCubeFace &_cube_face); + + /// \brief Applies the camera orientation offset by rotation in roll and yaw. + /// \param[in] _setting The camera orientation offset to apply. + private: void ApplyCameraSetting(const GpuLaserCameraOrientationOffset &_setting); + + /// \brief Inverse of GpuLaser::ApplyCameraSetting(): Reverts the given + /// camera orientation offset. + /// \param[in] _setting The camera orientation offset to revert. + private: void RevertCameraSetting(const GpuLaserCameraOrientationOffset &_setting); /// \brief Horizontal half angle. protected: double horzHalfAngle; diff --git a/gazebo/rendering/GpuLaserCubeFace.hh b/gazebo/rendering/GpuLaserCubeFace.hh new file mode 100644 index 0000000000..aef084e0c9 --- /dev/null +++ b/gazebo/rendering/GpuLaserCubeFace.hh @@ -0,0 +1,66 @@ +/* + * Enway GmbH - All Rights reserved. + * Proprietary & confidential. + */ + +#ifndef GAZEBO_GPU_LASER_CUBE_FACE_H +#define GAZEBO_GPU_LASER_CUBE_FACE_H + +#include +#include +#include + +#include + +#include "gazebo/rendering/ogre_gazebo.h" + +namespace gazebo +{ + + namespace rendering + { + + /// \brief Cube map face ID + enum class GpuLaserCubeFaceId + { + CUBE_FRONT_FACE, + CUBE_LEFT_FACE, + CUBE_REAR_FACE, + CUBE_RIGHT_FACE, + CUBE_TOP_FACE, + CUBE_BOTTOM_FACE + }; + + /// \brief Stores mapping of a single laser ray (combination of azimuth and + /// elevation) to cube map coordinates. The first element of the pair is the + /// ID of the corresponding cube map face. The second element are the + /// normalized x/y coordinates of the intersection of the laser ray with + /// that cube face (in the range [0,1]x[0,1]). + typedef std::pair + GpuLaserCubeMappingPoint; + + /// \brief Orientation offset for camera + struct GpuLaserCameraOrientationOffset + { + double azimuthOffset; + double elevationOffset; + }; + + /// \brief Holds the data for each cube face. + struct GpuLaserCubeFace + { + /// \brief The corresponding camera orientation offset + GpuLaserCameraOrientationOffset cameraSetting; + + /// \brief The texture used to render the depth image + Ogre::TexturePtr texture; + + /// \brief The depth image data + std::vector depthImg; + }; + + } + +} + +#endif //GAZEBO_GPU_LASER_CUBE_FACE_H diff --git a/gazebo/rendering/GpuLaserPrivate.hh b/gazebo/rendering/GpuLaserPrivate.hh index 5c2e70c9da..074eff1fa3 100644 --- a/gazebo/rendering/GpuLaserPrivate.hh +++ b/gazebo/rendering/GpuLaserPrivate.hh @@ -18,31 +18,22 @@ #ifndef _GAZEBO_RENDERING_GPULASER_PRIVATE_HH_ #define _GAZEBO_RENDERING_GPULASER_PRIVATE_HH_ +#include #include #include -#include "gazebo/rendering/RenderTypes.hh" +#include "gazebo/rendering/GpuLaserCubeFace.hh" #include "gazebo/common/Event.hh" namespace Ogre { - class Camera; class Material; - class MovableObject; class RenderTarget; - class SceneNode; - class Texture; - class Viewport; } namespace gazebo { - namespace common - { - class Mesh; - } - namespace rendering { /// \internal @@ -60,79 +51,30 @@ namespace gazebo const std::string &_format)> newLaserFrame; /// \brief Raw buffer of laser data. - public: float *laserBuffer; + public: std::vector laserBuffer; /// \brief Outgoing laser data, used by newLaserFrame event. - public: float *laserScan; - - /// \brief Pointer to Ogre material for the first rendering pass. - public: Ogre::Material *matFirstPass; - - /// \brief Pointer to Ogre material for the sencod rendering pass. - public: Ogre::Material *matSecondPass; - - /// \brief An array of first pass textures. - public: Ogre::Texture *firstPassTextures[3]; - - /// \brief Second pass texture. - public: Ogre::Texture *secondPassTexture; - - /// \brief First pass render targets. - public: Ogre::RenderTarget *firstPassTargets[3]; - - /// \brief Second pass render target. - public: Ogre::RenderTarget *secondPassTarget; - - /// \brief First pass viewports. - public: Ogre::Viewport *firstPassViewports[3]; + public: std::vector laserScan; - /// \brief Second pass viewport - public: Ogre::Viewport *secondPassViewport; + /// \brief The cube faces that are used by the sensor. + public: std::map cubeMapFaces; - /// \brief Number of first pass textures. - public: unsigned int textureCount; + /// \brief Stores in a grid the mapping of lidar rays to cube map + /// coordinates. The first dimension of this grid is azimuth, the second + /// dimension is elevation. + public: std::vector> mapping; - /// \brief A list of camera angles for first pass rendering. - public: double cameraYaws[4]; + /// \brief Pointer to Ogre material for the rendering pass. + public: Ogre::Material *material; /// \brief Temporary pointer to the current render target. public: Ogre::RenderTarget *currentTarget; - /// \brief Temporary pointer to the current material. - public: Ogre::Material *currentMat; - - /// \brief Ogre orthorgraphic camera used in the second pass for - /// undistortion. - public: Ogre::Camera *orthoCam; - - /// \brief Ogre scenenode where the orthorgraphic camera is attached to. - public: Ogre::SceneNode *pitchNodeOrtho; - - /// \brief Ogre mesh used to create a canvas for undistorting range values - /// in the second rendering pass. - public: common::Mesh *undistMesh; - - /// \brief Ogre movable object created from the canvas mesh. - public: Ogre::MovableObject *object; - - /// \brief Pointer to visual that holds the canvas. - public: VisualPtr visual; - - /// \brief Image width of second pass. - public: unsigned int w2nd; - - /// \brief Image height of second pass. - public: unsigned int h2nd; - - /// \brief Time taken to complete the two rendering passes. - public: double lastRenderDuration; - - /// \brief List of texture unit indices used during the second - /// rendering pass. - public: std::vector texIdx; + /// \brief Number of horizontal ranges. + public: unsigned int horizontalRangeCount; - /// Number of second pass texture units created. - public: static int texCount; + /// \brief Number of vertical ranges. + public: unsigned int verticalRangeCount; }; } } diff --git a/gazebo/rendering/GpuLaser_TEST.cc b/gazebo/rendering/GpuLaser_TEST.cc index 71f8700ba1..3ac634e87f 100644 --- a/gazebo/rendering/GpuLaser_TEST.cc +++ b/gazebo/rendering/GpuLaser_TEST.cc @@ -15,11 +15,15 @@ * */ +#include + #include + #include "gazebo/rendering/RenderingIface.hh" #include "gazebo/rendering/Scene.hh" #include "gazebo/rendering/GpuLaser.hh" #include "gazebo/test/ServerFixture.hh" +#include "test/util.hh" using namespace gazebo; class GpuLaser_TEST : public RenderingFixture @@ -78,12 +82,139 @@ TEST_F(GpuLaser_TEST, BasicGpuLaserAPITest) laserCam->SetRayCountRatio(0.344); EXPECT_NEAR(laserCam->RayCountRatio(), 0.344, 1e-6); - - laserCam->SetCameraCount(4u); - EXPECT_EQ(laserCam->CameraCount(), 4u); } } +///////////////////////////////////////////////// +// // +// Unit test calculations // +// // +///////////////////////////////////////////////// +class GpuLaserInternals_TEST : public gazebo::testing::AutoLogFixture +{ +}; + +TEST_F(GpuLaserInternals_TEST, FindCubeFaceMappingTest) +{ + using namespace rendering; + + GpuLaserCubeMappingPoint p; + + constexpr double numeric_tolerance = 1e-3; + + // ray straight ahead + ASSERT_NO_THROW(p = rendering::GpuLaser::FindCubeFaceMapping(M_PI_4, 0.)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_FRONT_FACE, p.first); + EXPECT_NEAR(0.5, p.second.X(), numeric_tolerance); + EXPECT_NEAR(0.5, p.second.Y(), numeric_tolerance); + + // ray at minimum azimuth + ASSERT_NO_THROW(p = rendering::GpuLaser::FindCubeFaceMapping(0., 0.)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_FRONT_FACE, p.first); + EXPECT_NEAR(1.0, p.second.X(), numeric_tolerance); + EXPECT_NEAR(0.5, p.second.Y(), numeric_tolerance); + + const double corner_elevation = std::atan(M_SQRT1_2); + constexpr double corner_offset = 1e-4; + + // ray at bottom left rear corner + ASSERT_NO_THROW(p = rendering::GpuLaser::FindCubeFaceMapping(M_PI + corner_offset, - corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_REAR_FACE, p.first); + EXPECT_NEAR(1.0, p.second.X(), numeric_tolerance); + EXPECT_NEAR(1.0, p.second.Y(), numeric_tolerance); +} + +TEST_F(GpuLaserInternals_TEST, ViewingRayTest) +{ + using namespace rendering; + + const double eps = 1e-10; + + ignition::math::Vector3d v; + + // ray with minimum azimuth + v = rendering::GpuLaser::ViewingRay(0., 0.); + EXPECT_DOUBLE_EQ(1., v.Length()); + EXPECT_DOUBLE_EQ(1. / M_SQRT2, v.X()); + EXPECT_DOUBLE_EQ(-1. / M_SQRT2, v.Y()); + EXPECT_DOUBLE_EQ(0., v.Z()); + + // ray straight ahead + v = rendering::GpuLaser::ViewingRay(M_PI_4, 0.); + EXPECT_DOUBLE_EQ(1., v.Length()); + EXPECT_DOUBLE_EQ(1., v.X()); + EXPECT_DOUBLE_EQ(0., v.Y()); + EXPECT_DOUBLE_EQ(0., v.Z()); + + // ray straight ahead and elevated + v = rendering::GpuLaser::ViewingRay(M_PI_4, M_PI_4); + EXPECT_DOUBLE_EQ(1., v.Length()); + EXPECT_DOUBLE_EQ(1. / M_SQRT2, v.X()); + EXPECT_DOUBLE_EQ(0., v.Y()); + EXPECT_DOUBLE_EQ(1. / M_SQRT2, v.Z()); + + // ray left + v = rendering::GpuLaser::ViewingRay(M_PI_2 + M_PI_4, 0.); + EXPECT_DOUBLE_EQ(1., v.Length()); + EXPECT_NEAR(0., v.X(), eps); + EXPECT_DOUBLE_EQ(1., v.Y()); + EXPECT_DOUBLE_EQ(0., v.Z()); +} + +TEST_F(GpuLaserInternals_TEST, FindCubeFaceTest) +{ + using namespace rendering; + + // at zero elevation + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_FRONT_FACE, GpuLaser::FindCubeFace(0., 0.)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_LEFT_FACE, GpuLaser::FindCubeFace(M_PI_2, 0.)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_REAR_FACE, GpuLaser::FindCubeFace(M_PI, 0.)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_RIGHT_FACE, GpuLaser::FindCubeFace(M_PI + M_PI_2, 0.)); + + // extreme elevation values + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_TOP_FACE, GpuLaser::FindCubeFace(0., M_PI_2)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_BOTTOM_FACE, GpuLaser::FindCubeFace(0., -M_PI_2)); + + const double corner_elevation = std::atan(M_SQRT1_2); + constexpr double corner_offset = 1e-4; + + // 4 corners of the front face + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_FRONT_FACE, GpuLaser::FindCubeFace(corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_FRONT_FACE, GpuLaser::FindCubeFace(M_PI_2 - corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_FRONT_FACE, GpuLaser::FindCubeFace(corner_offset, - corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_FRONT_FACE, GpuLaser::FindCubeFace(M_PI_2 - corner_offset, - corner_elevation + corner_offset)); + + // 4 corners of the left face + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_LEFT_FACE, GpuLaser::FindCubeFace(M_PI_2 + corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_LEFT_FACE, GpuLaser::FindCubeFace(M_PI - corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_LEFT_FACE, GpuLaser::FindCubeFace(M_PI_2 + corner_offset, - corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_LEFT_FACE, GpuLaser::FindCubeFace(M_PI - corner_offset, - corner_elevation + corner_offset)); + + // 4 corners of the rear face + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_REAR_FACE, GpuLaser::FindCubeFace(M_PI + corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_REAR_FACE, GpuLaser::FindCubeFace(M_PI + M_PI_2 - corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_REAR_FACE, GpuLaser::FindCubeFace(M_PI + corner_offset, - corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_REAR_FACE, GpuLaser::FindCubeFace(M_PI + M_PI_2 - corner_offset, - corner_elevation + corner_offset)); + + // 4 corners of the right face + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_RIGHT_FACE, GpuLaser::FindCubeFace(M_PI + M_PI_2 + corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_RIGHT_FACE, GpuLaser::FindCubeFace(M_PI + M_PI - corner_offset, corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_RIGHT_FACE, GpuLaser::FindCubeFace(M_PI + M_PI_2 + corner_offset, - corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_RIGHT_FACE, GpuLaser::FindCubeFace(M_PI + M_PI - corner_offset, - corner_elevation + corner_offset)); + + // 4 corners of the top face + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_TOP_FACE, GpuLaser::FindCubeFace(0.0, corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_TOP_FACE, GpuLaser::FindCubeFace(M_PI_2, corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_TOP_FACE, GpuLaser::FindCubeFace(M_PI, corner_elevation + corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_TOP_FACE, GpuLaser::FindCubeFace(M_PI + M_PI_2, corner_elevation + corner_offset)); + + // 4 corners of the bottom face + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_BOTTOM_FACE, GpuLaser::FindCubeFace(0.0, - corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_BOTTOM_FACE, GpuLaser::FindCubeFace(M_PI_2, - corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_BOTTOM_FACE, GpuLaser::FindCubeFace(M_PI, - corner_elevation - corner_offset)); + EXPECT_EQ(GpuLaserCubeFaceId::CUBE_BOTTOM_FACE, GpuLaser::FindCubeFace(M_PI + M_PI_2, - corner_elevation - corner_offset)); +} + ///////////////////////////////////////////////// int main(int argc, char **argv) { diff --git a/gazebo/sensors/GpuRaySensor.cc b/gazebo/sensors/GpuRaySensor.cc index bf7f2b39d1..bd736ea101 100644 --- a/gazebo/sensors/GpuRaySensor.cc +++ b/gazebo/sensors/GpuRaySensor.cc @@ -14,9 +14,11 @@ * limitations under the License. * */ +#include #include #include #include +#include #include #include #include "gazebo/physics/World.hh" @@ -82,7 +84,7 @@ void GpuRaySensor::Load(const std::string &_worldName, sdf::ElementPtr _sdf) { Sensor::Load(_worldName, _sdf); // useStrictRate is set in Sensor::Load() - if (this->useStrictRate) + if (GpuRaySensor::useStrictRate) { this->connections.push_back( event::Events::ConnectPreRenderEnded( @@ -95,7 +97,7 @@ void GpuRaySensor::Load(const std::string &_worldName) { Sensor::Load(_worldName); // useStrictRate is set in Sensor::Load() - if (this->useStrictRate) + if (GpuRaySensor::useStrictRate) { this->connections.push_back( event::Events::ConnectPreRenderEnded( @@ -184,135 +186,98 @@ void GpuRaySensor::Init() this->dataPtr->laserCam->SetFarClip(this->RangeMax()); // horizontal laser setup - double hfov = (this->AngleMax() - this->AngleMin()).Radian(); + double hfovTotal = (this->AngleMax() - this->AngleMin()).Radian(); - if (hfov > 2 * M_PI) + if (hfovTotal > 2 * M_PI) { - hfov = 2 * M_PI; - gzwarn << "Horizontal FOV for GPU laser is capped at 180 degrees.\n"; + hfovTotal = 2 * M_PI; + gzwarn << "Horizontal FOV for GPU laser is capped at 360 degrees.\n"; } - this->dataPtr->laserCam->SetHorzHalfAngle( - (this->AngleMax() + this->AngleMin()).Radian() / 2.0); + this->dataPtr->laserCam->SetHorzHalfAngle(this->HorzHalfAngle()); - // determine number of cameras to use - unsigned int cameraCount; - if (hfov > 2.8) - { - if (hfov > 5.6) - { - cameraCount = 3; - } - else - { - cameraCount = 2; - } - } - else - { - cameraCount = 1; - } - this->dataPtr->laserCam->SetCameraCount(cameraCount); - - // horizontal fov of single frame - hfov = hfov / cameraCount; - - this->dataPtr->laserCam->SetHorzFOV(hfov); - this->dataPtr->laserCam->SetCosHorzFOV(hfov); - - // Fixed minimum resolution of texture to reduce steps in ranges - // when hitting surfaces where the angle between ray and surface is small. - // Also have to keep in mind the GPU's max. texture size - unsigned int horzRangeCountPerCamera = - std::max(2048U, this->dataPtr->horzRangeCount / cameraCount); - unsigned int vertRangeCountPerCamera = this->dataPtr->vertRangeCount; + // we use a fixed square camera FOV + constexpr double hfovPerCamera = M_PI_2; + this->dataPtr->laserCam->SetHorzFOV(hfovPerCamera); // vertical laser setup - double vfov; + double vfovTotal; if (this->dataPtr->vertRayCount > 1) { - vfov = (this->VerticalAngleMax() - this->VerticalAngleMin()).Radian(); + vfovTotal = (this->VerticalAngleMax() - this->VerticalAngleMin()).Radian(); } else { - vfov = 0; + vfovTotal = 0; if (this->VerticalAngleMax() != this->VerticalAngleMin()) { gzwarn << "Only one vertical ray but vertical min. and max. angle " - "are not equal. Min. angle is used.\n"; - this->SetVerticalAngleMax(this->VerticalAngleMin().Radian()); + "are not equal. Half angle between min. and max. is used.\n"; + const double vertHalfAngle = this->VertHalfAngle(); + this->SetVerticalAngleMin(vertHalfAngle); + this->SetVerticalAngleMax(vertHalfAngle); } } - if (vfov > M_PI / 2) + if (vfovTotal > M_PI) { - vfov = M_PI / 2; - gzwarn << "Vertical FOV for GPU laser is capped at 90 degrees.\n"; + vfovTotal = M_PI; + gzwarn << "Vertical FOV for GPU laser is capped at 180 degrees.\n"; } - this->dataPtr->laserCam->SetVertFOV(vfov); - this->dataPtr->laserCam->SetVertHalfAngle((this->VerticalAngleMax() - + this->VerticalAngleMin()).Radian() / 2.0); - - this->SetVerticalAngleMin(this->dataPtr->laserCam->VertHalfAngle() - - (vfov / 2)); - this->SetVerticalAngleMax(this->dataPtr->laserCam->VertHalfAngle() + - (vfov / 2)); - - // Assume camera always stays horizontally even if vert. half angle of - // laser is not 0. Add padding to camera vfov. - double vfovCamera = vfov + 2 * std::abs( - this->dataPtr->laserCam->VertHalfAngle()); + constexpr double vfovPerCamera = M_PI_2; + this->dataPtr->laserCam->SetVertFOV(vfovPerCamera); + this->dataPtr->laserCam->SetVertHalfAngle(this->VertHalfAngle()); - // Add padding to vertical camera FOV to cover all possible rays - // for given laser vert. and horiz. FOV - vfovCamera = 2 * atan(tan(vfovCamera / 2) / cos(hfov / 2)); + // unused by this implementation, but keep for backwards compatibility + const double cosHorzFov = + 2 * atan(tan(hfovPerCamera / 2) / cos(vfovTotal / 2)); + const double cosVertFov = + 2 * atan(tan(vfovTotal / 2) / cos(hfovPerCamera / 2)); + this->dataPtr->laserCam->SetCosHorzFOV(cosHorzFov); + this->dataPtr->laserCam->SetCosVertFOV(cosVertFov); - if (vfovCamera > 2.8) - { - gzerr << "Vertical FOV of internal camera exceeds 2.8 radians.\n"; - } - - this->dataPtr->laserCam->SetCosVertFOV(vfovCamera); + // internal camera has fixed aspect ratio of one + constexpr double cameraAspectRatio = 1; + this->dataPtr->laserCam->SetRayCountRatio(cameraAspectRatio); // If vertical ray is not 1 adjust horizontal and vertical // ray count to maintain aspect ratio if (this->dataPtr->vertRayCount > 1) { - double cameraAspectRatio = tan(hfov / 2.0) / tan(vfovCamera / 2.0); - - this->dataPtr->laserCam->SetRayCountRatio(cameraAspectRatio); this->dataPtr->rangeCountRatio = cameraAspectRatio; + } - if ((horzRangeCountPerCamera / this->RangeCountRatio()) > - vertRangeCountPerCamera) - { - vertRangeCountPerCamera = - round(horzRangeCountPerCamera / this->RangeCountRatio()); - } - else - { - horzRangeCountPerCamera = - round(vertRangeCountPerCamera * this->RangeCountRatio()); - } + // take ranges per radian of FOV as a guideline for camera resolution + double rangesPerFov = 0; + if (vfovTotal > 0) + { + rangesPerFov = std::max(rangesPerFov, this->VerticalRangeCount() / vfovTotal); } - else + if (hfovTotal > 0) { - // In case of 1 vert. ray, set a very small vertical FOV for camera - this->dataPtr->laserCam->SetRayCountRatio(horzRangeCountPerCamera); + rangesPerFov = std::max(rangesPerFov, this->RangeCount() / hfovTotal); } + // ranges per camera (which has 90 deg FOV) + const unsigned int ranges = static_cast(rangesPerFov * M_PI_2); + + // ensure minimal texture size (to mitigate issues with stepped point cloud + // especially for shallow angles of incidence) + constexpr unsigned int min_texture_size = 1024; + const unsigned int camera_resolution = std::max(ranges, min_texture_size); + // Initialize camera sdf for GpuLaser this->dataPtr->cameraElem.reset(new sdf::Element); sdf::initFile("camera.sdf", this->dataPtr->cameraElem); - this->dataPtr->cameraElem->GetElement("horizontal_fov")->Set(hfov); + this->dataPtr->cameraElem->GetElement("horizontal_fov")->Set(M_PI_2); sdf::ElementPtr ptr = this->dataPtr->cameraElem->GetElement("image"); - ptr->GetElement("width")->Set(horzRangeCountPerCamera); - ptr->GetElement("height")->Set(vertRangeCountPerCamera); + ptr->GetElement("width")->Set(camera_resolution); + ptr->GetElement("height")->Set(camera_resolution); ptr->GetElement("format")->Set("FLOAT32"); ptr = this->dataPtr->cameraElem->GetElement("clip"); @@ -327,7 +292,35 @@ void GpuRaySensor::Init() this->dataPtr->laserCam->SetRangeCount( this->RangeCount(), this->VerticalRangeCount()); - this->dataPtr->laserCam->SetClipDist(this->RangeMin(), this->RangeMax()); + this->dataPtr->laserCam->SetClipDist(static_cast(this->RangeMin()), static_cast(this->RangeMax())); + + // create sets of angles and initialize cubemap + // eventually, this should also be able to handle irregular spaced rays + // but that would require changes to the SDFormat definition of a ray sensor. + // Note: The order of the angles in the two sets matters as the laser + // readings will be returned in the same order! + { + std::set azimuth_angles; + const double azimuth_angle_increment = hfovTotal / (this->dataPtr->horzRangeCount - 1); + double azimuth = this->AngleMin().Radian(); + for (unsigned int i = 0; i < this->dataPtr->horzRangeCount; i++) + { + azimuth_angles.insert(azimuth); + azimuth += azimuth_angle_increment; + } + + std::set elevation_angles; + const double elevation_angle_increment = vfovTotal / (this->dataPtr->vertRangeCount - 1); + double elevation = this->VerticalAngleMin().Radian(); + for (unsigned int i = 0; i < this->dataPtr->vertRangeCount; i++) + { + elevation_angles.insert(elevation); + elevation += elevation_angle_increment; + } + + this->dataPtr->laserCam->InitMapping(azimuth_angles, elevation_angles); + } + this->dataPtr->laserCam->CreateLaserTexture( this->ScopedName() + "_RttTex_Laser"); this->dataPtr->laserCam->CreateRenderTexture( @@ -368,7 +361,7 @@ void GpuRaySensor::Fini() void GpuRaySensor::SetActive(bool _value) { // If this sensor is reactivated - if (this->useStrictRate && _value && !this->IsActive()) + if (GpuRaySensor::useStrictRate && _value && !this->IsActive()) { // the next rendering time must be reset to ensure it is properly // computed by GpuRaySensor::NeedsUpdate. @@ -380,7 +373,7 @@ void GpuRaySensor::SetActive(bool _value) ////////////////////////////////////////////////// bool GpuRaySensor::NeedsUpdate() { - if (this->useStrictRate) + if (GpuRaySensor::useStrictRate) { double simTime; if (this->scene) @@ -443,12 +436,6 @@ event::ConnectionPtr GpuRaySensor::ConnectNewLaserFrame( return this->dataPtr->laserCam->ConnectNewLaserFrame(_subscriber); } -////////////////////////////////////////////////// -unsigned int GpuRaySensor::CameraCount() const -{ - return this->dataPtr->laserCam->CameraCount(); -} - ////////////////////////////////////////////////// bool GpuRaySensor::IsHorizontal() const { @@ -549,7 +536,7 @@ int GpuRaySensor::RayCount() const ////////////////////////////////////////////////// int GpuRaySensor::RangeCount() const { - return this->RayCount() * this->dataPtr->horzElem->Get("resolution"); + return static_cast(this->RayCount() * this->dataPtr->horzElem->Get("resolution")); } ////////////////////////////////////////////////// @@ -566,7 +553,7 @@ int GpuRaySensor::VerticalRangeCount() const { if (this->dataPtr->scanElem->HasElement("vertical")) { - int rows = (this->VerticalRayCount() * + const int rows = static_cast(this->VerticalRayCount() * this->dataPtr->vertElem->Get("resolution")); if (rows > 1) return rows; @@ -659,7 +646,7 @@ int GpuRaySensor::Fiducial(const unsigned int /*_index*/) const ////////////////////////////////////////////////// void GpuRaySensor::PrerenderEnded() { - if (this->useStrictRate && this->dataPtr->laserCam && this->IsActive() && + if (GpuRaySensor::useStrictRate && this->dataPtr->laserCam && this->IsActive() && this->NeedsUpdate()) { // compute next rendering time, take care of the case where period is zero. @@ -679,7 +666,7 @@ void GpuRaySensor::PrerenderEnded() void GpuRaySensor::Render() { IGN_PROFILE("sensors::GpuRaySensor::Render"); - if (this->useStrictRate) + if (GpuRaySensor::useStrictRate) { if (!this->dataPtr->renderNeeded) return; @@ -736,8 +723,8 @@ bool GpuRaySensor::UpdateImpl(const bool /*_force*/) scan->set_range_min(this->dataPtr->rangeMin); scan->set_range_max(this->dataPtr->rangeMax); - const int numRays = this->dataPtr->vertRangeCount * - this->dataPtr->horzRangeCount; + const int numRays = static_cast(this->dataPtr->vertRangeCount * + this->dataPtr->horzRangeCount); if (scan->ranges_size() != numRays) { // gzdbg << "Size mismatch; allocating memory\n"; @@ -803,7 +790,7 @@ rendering::GpuLaserPtr GpuRaySensor::LaserCamera() const ////////////////////////////////////////////////// double GpuRaySensor::NextRequiredTimestamp() const { - if (this->useStrictRate) + if (GpuRaySensor::useStrictRate) { if (!ignition::math::equal(this->updatePeriod.Double(), 0.0)) return this->dataPtr->nextRenderingTime; @@ -820,6 +807,24 @@ double GpuRaySensor::NextRequiredTimestamp() const void GpuRaySensor::ResetLastUpdateTime() { Sensor::ResetLastUpdateTime(); - if (this->useStrictRate) + if (GpuRaySensor::useStrictRate) this->dataPtr->nextRenderingTime = std::numeric_limits::quiet_NaN(); } + +////////////////////////////////////////////////// +unsigned int GpuRaySensor::CameraCount() const +{ + return this->dataPtr->laserCam->CameraCount(); +} + +////////////////////////////////////////////////// +double GpuRaySensor::HorzHalfAngle() const +{ + return (this->AngleMax() + this->AngleMin()).Radian() / 2.0; +} + +////////////////////////////////////////////////// +double GpuRaySensor::VertHalfAngle() const +{ + return (this->VerticalAngleMax() + this->VerticalAngleMin()).Radian() / 2.0; +} diff --git a/media/materials/programs/CMakeLists.txt b/media/materials/programs/CMakeLists.txt index 93d4ef88ac..903729966f 100644 --- a/media/materials/programs/CMakeLists.txt +++ b/media/materials/programs/CMakeLists.txt @@ -24,11 +24,9 @@ GBufferFP.glsl GBufferVP.glsl grid_fp.glsl grid_vp.glsl -laser_1st_pass_dbg.frag -laser_1st_pass.frag -laser_1st_pass.vert -laser_2nd_pass.frag -laser_2nd_pass.vert +laser_pass_dbg.frag +laser_pass.frag +laser_pass.vert ModulateFP.glsl NoFilterFP.glsl oculus.cg diff --git a/media/materials/programs/laser_2nd_pass.frag b/media/materials/programs/laser_2nd_pass.frag deleted file mode 100644 index 19496fa004..0000000000 --- a/media/materials/programs/laser_2nd_pass.frag +++ /dev/null @@ -1,27 +0,0 @@ -uniform sampler2D tex1; -uniform sampler2D tex2; -uniform sampler2D tex3; - -uniform vec4 texSize; -varying float tex; - -void main() -{ - if ((gl_TexCoord[0].s < 0.0) || (gl_TexCoord[0].s > 1.0) || - (gl_TexCoord[0].t < 0.0) || (gl_TexCoord[0].t > 1.0)) - gl_FragColor = vec4(1,1,1,1); - else - { - int int_tex = int(tex * 1000.0); - if (int_tex == 0) - //gl_FragColor=vec4(1,0,0,1); - gl_FragColor = texture2D( tex1, gl_TexCoord[0].st); - else - if (int_tex == 1) - //gl_FragColor=vec4(2,1,0,1); - gl_FragColor = texture2D( tex2, gl_TexCoord[0].st); - else - //gl_FragColor=vec4(3,2,1,1); - gl_FragColor = texture2D( tex3, gl_TexCoord[0].st); - } -} diff --git a/media/materials/programs/laser_2nd_pass.vert b/media/materials/programs/laser_2nd_pass.vert deleted file mode 100644 index 7d1dd41588..0000000000 --- a/media/materials/programs/laser_2nd_pass.vert +++ /dev/null @@ -1,8 +0,0 @@ -varying float tex; - -void main() -{ - gl_Position = ftransform(); - tex = gl_Vertex.x; - gl_TexCoord[0] = gl_MultiTexCoord0; -} diff --git a/media/materials/programs/laser_1st_pass.frag b/media/materials/programs/laser_pass.frag similarity index 100% rename from media/materials/programs/laser_1st_pass.frag rename to media/materials/programs/laser_pass.frag diff --git a/media/materials/programs/laser_1st_pass.vert b/media/materials/programs/laser_pass.vert similarity index 100% rename from media/materials/programs/laser_1st_pass.vert rename to media/materials/programs/laser_pass.vert diff --git a/media/materials/programs/laser_1st_pass_dbg.frag b/media/materials/programs/laser_pass_dbg.frag similarity index 100% rename from media/materials/programs/laser_1st_pass_dbg.frag rename to media/materials/programs/laser_pass_dbg.frag diff --git a/media/materials/scripts/gazebo.material b/media/materials/scripts/gazebo.material index 15f25eb9f6..d58af9753e 100644 --- a/media/materials/scripts/gazebo.material +++ b/media/materials/scripts/gazebo.material @@ -101,14 +101,14 @@ material Gazebo/XYZNormals } } -vertex_program Gazebo/LaserScan1stVS glsl +vertex_program Gazebo/LaserScanVS glsl { - source laser_1st_pass.vert + source laser_pass.vert } -fragment_program Gazebo/LaserScan1stFS glsl +fragment_program Gazebo/LaserScanFS glsl { - source laser_1st_pass.frag + source laser_pass.frag default_params { @@ -118,7 +118,7 @@ fragment_program Gazebo/LaserScan1stFS glsl } } -material Gazebo/LaserScan1st +material Gazebo/LaserScan { technique { @@ -126,40 +126,8 @@ material Gazebo/LaserScan1st { separate_scene_blend one zero one zero - vertex_program_ref Gazebo/LaserScan1stVS { } - fragment_program_ref Gazebo/LaserScan1stFS { } - } - } -} - -vertex_program Gazebo/LaserScan2ndVS glsl -{ - source laser_2nd_pass.vert -} - -fragment_program Gazebo/LaserScan2ndFS glsl -{ - source laser_2nd_pass.frag - - default_params - { - param_named tex1 int 0 - param_named tex2 int 1 - param_named tex3 int 2 - param_named_auto texSize texture_size 0 - } -} - -material Gazebo/LaserScan2nd -{ - technique - { - pass laser_tex_2nd - { - separate_scene_blend one zero one zero - - vertex_program_ref Gazebo/LaserScan2ndVS { } - fragment_program_ref Gazebo/LaserScan2ndFS { } + vertex_program_ref Gazebo/LaserScanVS { } + fragment_program_ref Gazebo/LaserScanFS { } } } }