Revision: 4439
http://playerstage.svn.sourceforge.net/playerstage/?rev=4439&view=rev
Author: natepak
Date: 2008-03-25 18:04:07 -0700 (Tue, 25 Mar 2008)
Log Message:
-----------
Adding in stereo vision
Modified Paths:
--------------
code/gazebo/trunk/server/Model.cc
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/gui/Gui.cc
code/gazebo/trunk/server/rendering/OgreAdaptor.cc
code/gazebo/trunk/server/rendering/OgreAdaptor.hh
code/gazebo/trunk/server/rendering/OgreCreator.cc
code/gazebo/trunk/server/rendering/OgreVisual.cc
code/gazebo/trunk/server/sensors/camera/CameraSensor.cc
code/gazebo/trunk/server/sensors/camera/CameraSensor.hh
code/gazebo/trunk/server/sensors/camera/SConscript
Added Paths:
-----------
code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc
code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh
code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
Modified: code/gazebo/trunk/server/Model.cc
===================================================================
--- code/gazebo/trunk/server/Model.cc 2008-03-25 03:55:20 UTC (rev 4438)
+++ code/gazebo/trunk/server/Model.cc 2008-03-26 01:04:07 UTC (rev 4439)
@@ -70,20 +70,20 @@
for (biter=this->bodies.begin(); biter != this->bodies.end(); biter++)
{
- delete biter->second;
+ GZ_DELETE(biter->second);
}
this->bodies.clear();
for (jiter = this->joints.begin(); jiter != this->joints.end(); jiter++)
{
- delete jiter->second;
+ GZ_DELETE( jiter->second );
}
this->joints.clear();
for (citer = this->controllers.begin();
citer != this->controllers.end(); citer++)
{
- delete citer->second;
+ GZ_DELETE( citer->second );
}
this->controllers.clear();
}
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2008-03-25 03:55:20 UTC (rev
4438)
+++ code/gazebo/trunk/server/Simulator.cc 2008-03-26 01:04:07 UTC (rev
4439)
@@ -82,11 +82,10 @@
if (!this->loaded)
return;
- GZ_DELETE (this->gui) // enough?
- GZ_DELETE (this->xmlFile) //enough?
+ GZ_DELETE (this->gui)
+ GZ_DELETE (this->xmlFile)
gazebo::World::Instance()->Close();
gazebo::OgreAdaptor::Instance()->Close();
-
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2008-03-25 03:55:20 UTC (rev 4438)
+++ code/gazebo/trunk/server/World.cc 2008-03-26 01:04:07 UTC (rev 4439)
@@ -73,7 +73,11 @@
for (miter = this->models.begin(); miter != this->models.end(); miter++)
{
- delete (*miter);
+ if (*miter)
+ {
+ delete (*miter);
+ (*miter) = NULL;
+ }
}
this->models.clear();
Modified: code/gazebo/trunk/server/gui/Gui.cc
===================================================================
--- code/gazebo/trunk/server/gui/Gui.cc 2008-03-25 03:55:20 UTC (rev 4438)
+++ code/gazebo/trunk/server/gui/Gui.cc 2008-03-26 01:04:07 UTC (rev 4439)
@@ -86,6 +86,8 @@
/// Destructor
FLTKGui::~FLTKGui()
{
+ this->hide();
+
GZ_DELETE (this->glWindow)
GZ_DELETE (this->toolbar)
//delete this->statusbar;
Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-03-25 03:55:20 UTC
(rev 4438)
+++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-03-26 01:04:07 UTC
(rev 4439)
@@ -81,7 +81,7 @@
//GZ_DELETE (this->logManager)
// this->root->detachRenderTarget(this->window);
// this->root->shutdown();
- GZ_DELETE (this->root)
+ //GZ_DELETE (this->root)
// GZ_DELETE (this->sceneMgr) //this objects seems to be destroyed by root
// GZ_DELETE (this->camera)
// GZ_DELETE (this->viewport)
@@ -93,8 +93,9 @@
void OgreAdaptor::Close()
{
GZ_DELETE (this->frameListener)
- GZ_DELETE (this->root) // All Ogre things should be shutdown and freed with
this
+ // This causes a seg fault. Need to fix
+ //GZ_DELETE (this->root)
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.hh
===================================================================
--- code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-03-25 03:55:20 UTC
(rev 4438)
+++ code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-03-26 01:04:07 UTC
(rev 4439)
@@ -134,6 +134,7 @@
private: friend class DestroyerT<OgreAdaptor>;
private: friend class SingletonT<OgreAdaptor>;
+
};
/*/// \brief
Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-03-25 03:55:20 UTC
(rev 4438)
+++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-03-26 01:04:07 UTC
(rev 4439)
@@ -166,8 +166,8 @@
// Helper function to create a camera
Ogre::Camera *OgreCreator::CreateCamera(const std::string &name, double
nearClip, double farClip, double hfov, Ogre::RenderTarget *renderTarget)
{
- Ogre::Camera *camera;
Ogre::Viewport *cviewport;
+ Ogre::Camera *camera;
camera = OgreAdaptor::Instance()->sceneMgr->createCamera(name);
@@ -196,7 +196,6 @@
return camera;
}
-
////////////////////////////////////////////////////////////////////////////////
void OgreCreator::CreateFog(XMLConfigNode *node)
{
Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-03-25 03:55:20 UTC
(rev 4438)
+++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-03-26 01:04:07 UTC
(rev 4439)
@@ -59,7 +59,10 @@
/// Destructor
OgreVisual::~OgreVisual()
{
- this->parentNode->removeAndDestroyChild(this->sceneNode->getName());
+ if (this->parentNode && this->sceneNode)
+ {
+ //this->parentNode->removeAndDestroyChild(this->sceneNode->getName());
+ }
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/sensors/camera/CameraSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/camera/CameraSensor.cc 2008-03-25
03:55:20 UTC (rev 4438)
+++ code/gazebo/trunk/server/sensors/camera/CameraSensor.cc 2008-03-26
01:04:07 UTC (rev 4439)
@@ -42,14 +42,13 @@
using namespace gazebo;
-GZ_REGISTER_STATIC_SENSOR("camera", CameraSensor);
-
//////////////////////////////////////////////////////////////////////////////
// Constructor
CameraSensor::CameraSensor(Body *body)
: Sensor(body)
{
this->imageWidth = this->imageHeight = 0;
+ this->textureWidth = this->textureHeight = 0;
this->saveFrameBuffer = NULL;
this->saveCount = 0;
@@ -109,29 +108,6 @@
// Initialize the camera
void CameraSensor::InitChild()
{
- this->ogreTextureName = this->GetName() + "_RttTex";
- this->ogreMaterialName = this->GetName() + "_RttMat";
- // Create the render texture
- this->renderTexture = Ogre::TextureManager::getSingleton().createManual(
- this->ogreTextureName,
-
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
- Ogre::TEX_TYPE_2D,
- this->imageWidth, this->imageHeight, 0,
- Ogre::PF_R8G8B8,
- Ogre::TU_RENDERTARGET);
-
- this->renderTarget = this->renderTexture->getBuffer()->getRenderTarget();
-
- // Create the camera
- this->camera = OgreCreator::CreateCamera(this->GetName(),
- this->nearClip, this->farClip, this->hfov,
this->renderTarget);
-
- Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(
- this->ogreMaterialName,
-
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
-
- Ogre::TextureUnitState *t =
mat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName);
-
this->sceneNode=this->GetVisualNode()->GetSceneNode();
// Create a scene node to control pitch motion
this->pitchNode = this->sceneNode->createChildSceneNode(this->GetName() +
"PitchNode");
@@ -151,7 +127,6 @@
// Update the drawing
void CameraSensor::UpdateChild(UpdateParams ¶ms)
{
-
if (World::Instance()->GetWireframe())
{
this->camera->setPolygonMode(Ogre::PM_WIREFRAME);
@@ -185,12 +160,6 @@
return this->pose;
}
-////////////////////////////////////////////////////////////////////////////////
-// Return the material the camera renders to
-std::string CameraSensor::GetMaterialName() const
-{
- return this->ogreMaterialName;
-}
////////////////////////////////////////////////////////////////////////////////
// Translate the camera
@@ -217,13 +186,6 @@
}
//////////////////////////////////////////////////////////////////////////////
-/// Get the width of the image
-unsigned int CameraSensor::GetImageWidth() const
-{
- return this->imageWidth;
-}
-
-//////////////////////////////////////////////////////////////////////////////
/// Get the horizontal field of view of the camera
double CameraSensor::GetFOV() const
{
@@ -231,15 +193,10 @@
}
//////////////////////////////////////////////////////////////////////////////
-// Get the width of the texture
-unsigned int CameraSensor::GetTextureWidth() const
+/// Get the width of the image
+unsigned int CameraSensor::GetImageWidth() const
{
- Ogre::HardwarePixelBufferSharedPtr mBuffer;
-
- // Get access to the buffer and make an image and write it to file
- mBuffer = this->renderTexture->getBuffer(0, 0);
-
- return mBuffer->getWidth();
+ return this->imageWidth;
}
//////////////////////////////////////////////////////////////////////////////
@@ -250,17 +207,20 @@
}
//////////////////////////////////////////////////////////////////////////////
-// Get the height of the texture
-unsigned int CameraSensor::GetTextureHeight() const
+/// Get the width of the texture
+unsigned int CameraSensor::GetTextureWidth() const
{
- Ogre::HardwarePixelBufferSharedPtr mBuffer;
+ return this->textureWidth;
+}
- // Get access to the buffer and make an image and write it to file
- mBuffer = this->renderTexture->getBuffer(0, 0);
-
- return mBuffer->getHeight();
+//////////////////////////////////////////////////////////////////////////////
+/// \brief Get the height of the texture
+unsigned int CameraSensor::GetTextureHeight() const
+{
+ return this->textureHeight;
}
+
//////////////////////////////////////////////////////////////////////////////
// Get the image size in bytes
size_t CameraSensor::GetImageByteSize() const
@@ -268,46 +228,7 @@
return this->imageHeight * this->imageWidth * 3;
}
-//////////////////////////////////////////////////////////////////////////////
-/// Get a pointer to the image data
-const unsigned char *CameraSensor::GetImageData()
-{
- Ogre::HardwarePixelBufferSharedPtr mBuffer;
- size_t size;
- // Get access to the buffer and make an image and write it to file
- mBuffer = this->renderTexture->getBuffer(0, 0);
-
- size = this->imageWidth * this->imageHeight * 3;
-
- // Allocate buffer
- if (!this->saveFrameBuffer)
- this->saveFrameBuffer = new unsigned char[size];
-
- mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
-
- int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
- int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
- int right = left + this->imageWidth;
- int bottom = top + this->imageHeight;
-
- // Get the center of the texture in RGB 24 bit format
- mBuffer->blitToMemory(
- Ogre::Box(left, top, right, bottom),
-
- Ogre::PixelBox(
- this->imageWidth,
- this->imageHeight,
- 1,
- Ogre::PF_B8G8R8,
- this->saveFrameBuffer)
- );
-
- mBuffer->unlock();
-
- return this->saveFrameBuffer;
-}
-
//////////////////////////////////////////////////////////////////////////////
// Enable or disable saving
void CameraSensor::EnableSaveFrame(bool enable)
@@ -322,63 +243,6 @@
this->saveFrames = !this->saveFrames;
}
-
-//////////////////////////////////////////////////////////////////////////////
-// Save the current frame to disk
-void CameraSensor::SaveFrame()
-{
- Ogre::HardwarePixelBufferSharedPtr mBuffer;
- std::ostringstream sstream;
- Ogre::ImageCodec::ImageData *imgData;
- Ogre::Codec * pCodec;
- size_t size, pos;
-
- this->GetImageData();
-
- // Get access to the buffer and make an image and write it to file
- mBuffer = this->renderTexture->getBuffer(0, 0);
-
- // Create image data structure
- imgData = new Ogre::ImageCodec::ImageData();
-
- imgData->width = this->imageWidth;
- imgData->height = this->imageHeight;
- imgData->depth = 1;
- imgData->format = Ogre::PF_B8G8R8;
- size = this->GetImageByteSize();
-
- // Wrap buffer in a chunk
- Ogre::MemoryDataStreamPtr stream(new Ogre::MemoryDataStream(
this->saveFrameBuffer, size, false));
-
- char tmp[1024];
- if (!this->savePathname.empty())
- {
- sprintf(tmp, "%s/%s-%04d.jpg", this->savePathname.c_str(),
- this->GetName().c_str(), this->saveCount);
- }
- else
- {
- sprintf(tmp, "%s-%04d.jpg", this->GetName().c_str(), this->saveCount);
- }
-
- // Get codec
- Ogre::String filename = tmp;
- pos = filename.find_last_of(".");
- Ogre::String extension;
-
- while (pos != filename.length() - 1)
- extension += filename[++pos];
-
- // Get the codec
- pCodec = Ogre::Codec::getCodec(extension);
-
- // Write out
- Ogre::Codec::CodecDataPtr codecDataPtr(imgData);
- pCodec->codeToFile(stream, filename, codecDataPtr);
-
- this->saveCount++;
-}
-
////////////////////////////////////////////////////////////////////////////////
/// Get a pointer to the ogre camera
Ogre::Camera *CameraSensor::GetOgreCamera() const
Modified: code/gazebo/trunk/server/sensors/camera/CameraSensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/camera/CameraSensor.hh 2008-03-25
03:55:20 UTC (rev 4438)
+++ code/gazebo/trunk/server/sensors/camera/CameraSensor.hh 2008-03-26
01:04:07 UTC (rev 4439)
@@ -29,6 +29,7 @@
#include <OgrePrerequisites.h>
#include <OgreTexture.h>
+
#include "Pose3d.hh"
#include "Sensor.hh"
@@ -82,7 +83,7 @@
public: Pose3d GetWorldPose() const;
/// \brief Return the material the camera renders to
- public: std::string GetMaterialName() const;
+ public: virtual std::string GetMaterialName() const = 0;
/// \brief Translate the camera
public: void Translate( const Vector3 &direction );
@@ -93,9 +94,6 @@
/// \brief Rotate the camera around the pitch axis
public: void RotatePitch( float angle );
- /// \brief Set the camera FOV (horizontal)
- public: void SetFOV(double fov);
-
/// \brief Get the camera FOV (horizontal)
public: double GetFOV() const;
@@ -112,7 +110,7 @@
public: unsigned int GetTextureHeight() const;
/// \brief Get a pointer to the image data
- public: const unsigned char *GetImageData();
+ public: virtual const unsigned char *GetImageData() = 0;
/// \brief Get the image size in bytes
public: size_t GetImageByteSize() const;
@@ -134,32 +132,26 @@
public: Ogre::Camera *GetOgreCamera() const;
// Save the camera frame
- private: void SaveFrame();
+ protected: virtual void SaveFrame() = 0;
- private: double hfov;
- private: double nearClip, farClip;
- private: unsigned int imageWidth, imageHeight;
+ protected: double hfov;
+ protected: double nearClip, farClip;
+ protected: unsigned int imageWidth, imageHeight;
+ protected: unsigned int textureWidth, textureHeight;
- private: Ogre::TexturePtr renderTexture;
- private: Ogre::RenderTarget *renderTarget;
- private: Ogre::Viewport *viewport;
+ protected: Ogre::Camera *camera;
+ protected: Ogre::SceneNode *pitchNode;
- private: Ogre::Camera *camera;
- private: Ogre::SceneNode *pitchNode;
+ protected: Pose3d pose;
- private: std::string ogreTextureName;
- private: std::string ogreMaterialName;
-
- private: Pose3d pose;
-
//access to our visual node (convenience member)
- private: Ogre::SceneNode *sceneNode;
+ protected: Ogre::SceneNode *sceneNode;
// Info for saving images
- private: unsigned char *saveFrameBuffer;
- private: unsigned int saveCount;
- private: bool saveFrames;
- private: std::string savePathname;
+ protected: unsigned char *saveFrameBuffer;
+ protected: unsigned int saveCount;
+ protected: bool saveFrames;
+ protected: std::string savePathname;
};
/// \}
Added: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc
(rev 0)
+++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-03-26
01:04:07 UTC (rev 4439)
@@ -0,0 +1,223 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU
General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/* Desc: A camera sensor using OpenGL
+ * Author: Nate Koenig
+ * Date: 15 July 2003
+ * CVS: $Id: MonoCameraSensor.cc 4436 2008-03-24 17:42:45Z robotos $
+ */
+
+#include <sstream>
+#include <OgreImageCodec.h>
+#include <Ogre.h>
+
+#include "Global.hh"
+#include "World.hh"
+#include "GazeboError.hh"
+#include "Body.hh"
+#include "OgreVisual.hh"
+#include "OgreCreator.hh"
+#include "OgreFrameListener.hh"
+
+#include "SensorFactory.hh"
+#include "CameraManager.hh"
+#include "MonoCameraSensor.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_STATIC_SENSOR("camera", MonoCameraSensor);
+
+//////////////////////////////////////////////////////////////////////////////
+// Constructor
+MonoCameraSensor::MonoCameraSensor(Body *body)
+ : CameraSensor(body)
+{
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Destructor
+MonoCameraSensor::~MonoCameraSensor()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Load the camera
+void MonoCameraSensor::LoadChild( XMLConfigNode *node )
+{
+ CameraSensor::LoadChild(node);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Initialize the camera
+void MonoCameraSensor::InitChild()
+{
+
+ this->ogreTextureName = this->GetName() + "_RttTex";
+ this->ogreMaterialName = this->GetName() + "_RttMat";
+
+ // Create the render texture
+ this->renderTexture = Ogre::TextureManager::getSingleton().createManual(
+ this->ogreTextureName,
+
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ this->imageWidth, this->imageHeight, 0,
+ Ogre::PF_R8G8B8,
+ Ogre::TU_RENDERTARGET);
+
+ this->renderTarget = this->renderTexture->getBuffer()->getRenderTarget();
+
+ // Create the camera
+ this->camera = OgreCreator::CreateCamera(this->GetName(),
+ this->nearClip, this->farClip, this->hfov,
this->renderTarget);
+
+ Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(
+ this->ogreMaterialName,
+
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+
+
+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+
+ // Get access to the buffer and make an image and write it to file
+ mBuffer = this->renderTexture->getBuffer(0, 0);
+
+ this->textureWidth = mBuffer->getWidth();
+ this->textureHeight = mBuffer->getHeight();
+
+ CameraSensor::InitChild();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Finalize the camera
+void MonoCameraSensor::FiniChild()
+{
+ CameraSensor::FiniChild();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Update the drawing
+void MonoCameraSensor::UpdateChild(UpdateParams ¶ms)
+{
+ CameraSensor::UpdateChild(params);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Return the material the camera renders to
+std::string MonoCameraSensor::GetMaterialName() const
+{
+ return this->ogreMaterialName;
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+/// Get a pointer to the image data
+const unsigned char *MonoCameraSensor::GetImageData()
+{
+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+ size_t size;
+
+ // Get access to the buffer and make an image and write it to file
+ mBuffer = this->renderTexture->getBuffer(0, 0);
+
+ size = this->imageWidth * this->imageHeight * 3;
+
+ // Allocate buffer
+ if (!this->saveFrameBuffer)
+ this->saveFrameBuffer = new unsigned char[size];
+
+ mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
+
+ int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
+ int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
+ int right = left + this->imageWidth;
+ int bottom = top + this->imageHeight;
+
+ // Get the center of the texture in RGB 24 bit format
+ mBuffer->blitToMemory(
+ Ogre::Box(left, top, right, bottom),
+
+ Ogre::PixelBox(
+ this->imageWidth,
+ this->imageHeight,
+ 1,
+ Ogre::PF_B8G8R8,
+ this->saveFrameBuffer)
+ );
+
+ mBuffer->unlock();
+
+ return this->saveFrameBuffer;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Save the current frame to disk
+void MonoCameraSensor::SaveFrame()
+{
+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+ std::ostringstream sstream;
+ Ogre::ImageCodec::ImageData *imgData;
+ Ogre::Codec * pCodec;
+ size_t size, pos;
+
+ this->GetImageData();
+
+ // Get access to the buffer and make an image and write it to file
+ mBuffer = this->renderTexture->getBuffer(0, 0);
+
+ // Create image data structure
+ imgData = new Ogre::ImageCodec::ImageData();
+
+ imgData->width = this->imageWidth;
+ imgData->height = this->imageHeight;
+ imgData->depth = 1;
+ imgData->format = Ogre::PF_B8G8R8;
+ size = this->GetImageByteSize();
+
+ // Wrap buffer in a chunk
+ Ogre::MemoryDataStreamPtr stream(new Ogre::MemoryDataStream(
this->saveFrameBuffer, size, false));
+
+ char tmp[1024];
+ if (!this->savePathname.empty())
+ {
+ sprintf(tmp, "%s/%s-%04d.jpg", this->savePathname.c_str(),
+ this->GetName().c_str(), this->saveCount);
+ }
+ else
+ {
+ sprintf(tmp, "%s-%04d.jpg", this->GetName().c_str(), this->saveCount);
+ }
+
+ // Get codec
+ Ogre::String filename = tmp;
+ pos = filename.find_last_of(".");
+ Ogre::String extension;
+
+ while (pos != filename.length() - 1)
+ extension += filename[++pos];
+
+ // Get the codec
+ pCodec = Ogre::Codec::getCodec(extension);
+
+ // Write out
+ Ogre::Codec::CodecDataPtr codecDataPtr(imgData);
+ pCodec->codeToFile(stream, filename, codecDataPtr);
+
+ this->saveCount++;
+}
Added: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh
(rev 0)
+++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh 2008-03-26
01:04:07 UTC (rev 4439)
@@ -0,0 +1,101 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/* Desc: A persepective X11 OpenGL Camera Sensor
+ * Author: Nate Koenig
+ * Date: 15 July 2003
+ * CVS: $Id: CameraSensor.hh 208 2008-01-05 15:33:33Z robotos $
+ */
+
+#ifndef MONOCAMERASENSOR_HH
+#define MONOCAMERASENSOR_HH
+
+#include <OgrePrerequisites.h>
+#include <OgreTexture.h>
+
+#include "CameraSensor.hh"
+
+// Forward Declarations
+namespace Ogre
+{
+ class TexturePtr;
+ class RenderTarget;
+ class Camera;
+ class Viewport;
+ class SceneNode;
+}
+
+namespace gazebo
+{
+/// \addtogroup gazebo_sensor
+/// \brief Basic camera sensor
+/// \{
+/// \defgroup gazebo_camera Camera
+/// \brief Basic camera sensor
+// \{
+
+
+/// \brief Basic camera sensor
+///
+/// This sensor is used for simulating standard monocular cameras; is
+/// is used by both camera models (e.g., SonyVID30) and user interface
+/// models (e.g., ObserverCam).
+class MonoCameraSensor : public CameraSensor
+{
+ /// \brief Constructor
+ public: MonoCameraSensor(Body *body);
+
+ /// \brief Destructor
+ public: virtual ~MonoCameraSensor();
+
+ /// \brief Load the camera using parameter from an XMLConfig node
+ /// \param node The XMLConfig node
+ protected: virtual void LoadChild( XMLConfigNode *node );
+
+ /// \brief Initialize the camera
+ protected: virtual void InitChild();
+
+ /// \brief Update the sensor information
+ protected: virtual void UpdateChild(UpdateParams ¶ms);
+
+ /// Finalize the camera
+ protected: virtual void FiniChild();
+
+ /// \brief Return the material the camera renders to
+ public: virtual std::string GetMaterialName() const;
+
+ /// \brief Get a pointer to the image data
+ public: virtual const unsigned char *GetImageData();
+
+ // Save the camera frame
+ private: void SaveFrame();
+
+ private: Ogre::TexturePtr renderTexture;
+ private: Ogre::RenderTarget *renderTarget;
+
+ private: std::string ogreTextureName;
+ private: std::string ogreMaterialName;
+};
+
+/// \}
+/// \}
+}
+#endif
+
Modified: code/gazebo/trunk/server/sensors/camera/SConscript
===================================================================
--- code/gazebo/trunk/server/sensors/camera/SConscript 2008-03-25 03:55:20 UTC
(rev 4438)
+++ code/gazebo/trunk/server/sensors/camera/SConscript 2008-03-26 01:04:07 UTC
(rev 4439)
@@ -1,7 +1,7 @@
#Import variable
Import('env staticObjs sharedObjs')
-sources = Split('CameraSensor.cc CameraManager.cc')
+sources = Split('CameraSensor.cc MonoCameraSensor.cc CameraManager.cc
StereoCameraSensor.cc')
staticObjs.append(env.StaticObject(sources))
sharedObjs.append(env.SharedObject(sources))
Added: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
(rev 0)
+++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc
2008-03-26 01:04:07 UTC (rev 4439)
@@ -0,0 +1,295 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * GNU
General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+
+/* Desc: Stereo Camera Sensor
+ * Author: Nate Koenig
+ * Date: 25 March 2008
+ * CVS: $Id:$
+ */
+
+#include <sstream>
+#include <OgreImageCodec.h>
+#include <Ogre.h>
+
+#include "Global.hh"
+#include "World.hh"
+#include "GazeboError.hh"
+#include "Body.hh"
+#include "OgreVisual.hh"
+#include "OgreCreator.hh"
+#include "OgreAdaptor.hh"
+#include "OgreFrameListener.hh"
+
+#include "SensorFactory.hh"
+#include "CameraManager.hh"
+#include "StereoCameraSensor.hh"
+
+using namespace gazebo;
+
+GZ_REGISTER_STATIC_SENSOR("stereocamera", StereoCameraSensor);
+
+//////////////////////////////////////////////////////////////////////////////
+// Constructor
+StereoCameraSensor::StereoCameraSensor(Body *body)
+ : CameraSensor(body)
+{
+}
+
+
+//////////////////////////////////////////////////////////////////////////////
+// Destructor
+StereoCameraSensor::~StereoCameraSensor()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Load the camera
+void StereoCameraSensor::LoadChild( XMLConfigNode *node )
+{
+ CameraSensor::LoadChild(node);
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Initialize the camera
+void StereoCameraSensor::InitChild()
+{
+ this->leftOgreTextureName = this->GetName() + "_LEFTRttTex";
+ this->rightOgreTextureName = this->GetName() + "_RIGHTRttTex";
+
+ this->ogreMaterialName = this->GetName() + "_RttMat";
+
+ // Create the render texture
+ this->leftRenderTexture = Ogre::TextureManager::getSingleton().createManual(
+ this->leftOgreTextureName,
+
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ this->imageWidth, this->imageHeight, 0,
+ Ogre::PF_R8G8B8,
+ Ogre::TU_RENDERTARGET);
+
+ this->leftRenderTarget =
this->leftRenderTexture->getBuffer()->getRenderTarget();
+
+ this->rightRenderTexture = Ogre::TextureManager::getSingleton().createManual(
+ this->rightOgreTextureName,
+
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
+ Ogre::TEX_TYPE_2D,
+ this->imageWidth, this->imageHeight, 0,
+ Ogre::PF_R8G8B8,
+ Ogre::TU_RENDERTARGET);
+
+ this->rightRenderTarget =
this->rightRenderTexture->getBuffer()->getRenderTarget();
+
+ // Create the camera
+ this->camera = OgreCreator::CreateCamera(this->GetName(),
+ this->nearClip, this->farClip, this->hfov,
this->leftRenderTarget);
+
+ // Hack to make the camera use the right render target too
+ {
+ Ogre::Viewport *cviewport;
+
+ // Setup the viewport to use the texture
+ cviewport = this->rightRenderTarget->addViewport(camera);
+ cviewport->setClearEveryFrame(true);
+ cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor
);
+ cviewport->setOverlaysEnabled(false);
+ }
+
+ Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create(
+ this->ogreMaterialName,
+
Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+
+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+
+ // Get access to the buffer and make an image and write it to file
+ mBuffer = this->leftRenderTexture->getBuffer(0, 0);
+
+ this->textureWidth = mBuffer->getWidth();
+ this->textureHeight = mBuffer->getHeight();
+
+ this->leftCameraListener.Init(this, true);
+ this->rightCameraListener.Init(this, false);
+
+ this->leftRenderTarget->addListener(&this->leftCameraListener);
+ this->rightRenderTarget->addListener(&this->rightCameraListener);
+
+ CameraSensor::InitChild();
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Finalize the camera
+void StereoCameraSensor::FiniChild()
+{
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Update the drawing
+void StereoCameraSensor::UpdateChild(UpdateParams ¶ms)
+{
+ CameraSensor::UpdateChild(params);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Return the material the camera renders to
+std::string StereoCameraSensor::GetMaterialName() const
+{
+ return this->ogreMaterialName;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// Get a pointer to the image data
+const unsigned char *StereoCameraSensor::GetImageData()
+{
+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+ size_t size;
+
+ // Get access to the buffer and make an image and write it to file
+ mBuffer = this->leftRenderTexture->getBuffer(0, 0);
+
+ size = this->imageWidth * this->imageHeight * 3;
+
+ // Allocate buffer
+ if (!this->saveFrameBuffer)
+ this->saveFrameBuffer = new unsigned char[size];
+
+ mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
+
+ int top = (int)((mBuffer->getHeight() - this->imageHeight) / 2.0);
+ int left = (int)((mBuffer->getWidth() - this->imageWidth) / 2.0);
+ int right = left + this->imageWidth;
+ int bottom = top + this->imageHeight;
+
+ // Get the center of the texture in RGB 24 bit format
+ mBuffer->blitToMemory(
+ Ogre::Box(left, top, right, bottom),
+
+ Ogre::PixelBox(
+ this->imageWidth,
+ this->imageHeight,
+ 1,
+ Ogre::PF_B8G8R8,
+ this->saveFrameBuffer)
+ );
+
+ mBuffer->unlock();
+
+ return this->saveFrameBuffer;
+}
+
+//////////////////////////////////////////////////////////////////////////////
+// Save the current frame to disk
+void StereoCameraSensor::SaveFrame()
+{
+ Ogre::HardwarePixelBufferSharedPtr mBuffer;
+ std::ostringstream sstream;
+ Ogre::ImageCodec::ImageData *imgData;
+ Ogre::Codec * pCodec;
+ size_t size, pos;
+
+ this->GetImageData();
+
+ // Get access to the buffer and make an image and write it to file
+ mBuffer = this->leftRenderTexture->getBuffer(0, 0);
+
+ // Create image data structure
+ imgData = new Ogre::ImageCodec::ImageData();
+
+ imgData->width = this->imageWidth;
+ imgData->height = this->imageHeight;
+ imgData->depth = 1;
+ imgData->format = Ogre::PF_B8G8R8;
+ size = this->GetImageByteSize();
+
+ // Wrap buffer in a chunk
+ Ogre::MemoryDataStreamPtr stream(new Ogre::MemoryDataStream(
this->saveFrameBuffer, size, false));
+
+ char tmp[1024];
+ if (!this->savePathname.empty())
+ {
+ sprintf(tmp, "%s/%s-%04d.jpg", this->savePathname.c_str(),
+ this->GetName().c_str(), this->saveCount);
+ }
+ else
+ {
+ sprintf(tmp, "%s-%04d.jpg", this->GetName().c_str(), this->saveCount);
+ }
+
+ // Get codec
+ Ogre::String filename = tmp;
+ pos = filename.find_last_of(".");
+ Ogre::String extension;
+
+ while (pos != filename.length() - 1)
+ extension += filename[++pos];
+
+ // Get the codec
+ pCodec = Ogre::Codec::getCodec(extension);
+
+ // Write out
+ Ogre::Codec::CodecDataPtr codecDataPtr(imgData);
+ pCodec->codeToFile(stream, filename, codecDataPtr);
+
+ this->saveCount++;
+}
+
+void StereoCameraSensor::StereoCameraListener::Init(
+ StereoCameraSensor *cam, bool isLeft)
+{
+ this->sensor = cam;
+ this->camera = this->sensor->GetOgreCamera();
+ this->isLeftCamera = isLeft;
+}
+
+void StereoCameraSensor::StereoCameraListener::preViewportUpdate(const
Ogre::RenderTargetViewportEvent &evt)
+{
+ if (this->isLeftCamera)
+ printf("Left Pre\n");
+ else
+ printf("Rightt Pre\n");
+
+/* if(evt.source != mViewport)
+ return;
+ Real offset = mStereoMgr->getEyesSpacing()/2;
+ if(mIsLeftEye)
+ {
+ offset = -offset;
+ }
+ mCamera->setFrustumOffset(-offset,0);
+ mPos = mCamera->getPosition();
+ Vector3 pos = mPos;
+ pos += offset * mCamera->getRight();
+ mCamera->setPosition(pos);
+ mStereoMgr->updateAllDependentRenderTargets();
+ mStereoMgr->chooseDebugPlaneMaterial(mIsLeftEye);
+ */
+
+}
+
+void StereoCameraSensor::StereoCameraListener::postViewportUpdate(const
Ogre::RenderTargetViewportEvent &evt)
+{
+ if (this->isLeftCamera)
+ printf("Left Post\n");
+ else
+ printf("Rightt Post\n");
+
+/* mCamera->setFrustumOffset(0,0);
+ mCamera->setPosition(mPos);
+ */
+
+}
Added: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
===================================================================
--- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
(rev 0)
+++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh
2008-03-26 01:04:07 UTC (rev 4439)
@@ -0,0 +1,122 @@
+/*
+ * Gazebo - Outdoor Multi-Robot Simulator
+ * Copyright (C) 2003
+ * Nate Koenig & Andrew Howard
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with this program; if not, write to the Free Software
+ * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
+ *
+ */
+/* Desc: Stereo Camera Sensor
+ * Author: Nate Koenig
+ * Date: 25 March 2008
+ * SVN: $Id:$
+ */
+
+#ifndef STEREOCAMERASENSOR_HH
+#define STEREOCAMERASENSOR_HH
+
+#include <OgrePrerequisites.h>
+#include <OgreTexture.h>
+
+#include "CameraSensor.hh"
+
+// Forward Declarations
+namespace Ogre
+{
+ class TexturePtr;
+ class RenderTarget;
+ class Camera;
+ class Viewport;
+ class SceneNode;
+}
+
+namespace gazebo
+{
+/// \addtogroup gazebo_sensor
+/// \brief Stereo camera sensor
+/// \{
+/// \defgroup gazebo_stereo_camera Stereo Camera
+/// \brief Stereo camera sensor
+// \{
+
+
+/// \brief Stereo camera sensor
+///
+/// This sensor is used for simulating a stereo camera.
+class StereoCameraSensor : public CameraSensor
+{
+
+ /// \brief Constructor
+ public: StereoCameraSensor(Body *body);
+
+ /// \brief Destructor
+ public: virtual ~StereoCameraSensor();
+
+ /// \brief Load the camera using parameter from an XMLConfig node
+ /// \param node The XMLConfig node
+ protected: virtual void LoadChild( XMLConfigNode *node );
+
+ /// \brief Initialize the camera
+ protected: virtual void InitChild();
+
+ /// \brief Update the sensor information
+ protected: virtual void UpdateChild(UpdateParams ¶ms);
+
+ /// Finalize the camera
+ protected: virtual void FiniChild();
+
+ /// \brief Return the material the camera renders to
+ public: virtual std::string GetMaterialName() const;
+
+ /// \brief Get a pointer to the image data
+ public: virtual const unsigned char *GetImageData();
+
+ // Save the camera frame
+ protected: virtual void SaveFrame();
+
+ private: Ogre::TexturePtr leftRenderTexture;
+ private: Ogre::RenderTarget *leftRenderTarget;
+ private: Ogre::TexturePtr rightRenderTexture;
+ private: Ogre::RenderTarget *rightRenderTarget;
+
+
+ private: std::string leftOgreTextureName;
+ private: std::string rightOgreTextureName;
+ private: std::string ogreMaterialName;
+
+ private:
+ class StereoCameraListener : public Ogre::RenderTargetListener
+ {
+ public: StereoCameraListener() : Ogre::RenderTargetListener() {}
+ public: void Init(StereoCameraSensor *sensor, bool isLeft);
+ public: void preViewportUpdate(const
Ogre::RenderTargetViewportEvent &evt);
+ public: void postViewportUpdate(const
Ogre::RenderTargetViewportEvent &evt);
+
+ private: Ogre::Vector3 pos;
+ private: StereoCameraSensor *sensor;
+ private: Ogre::Camera *camera;
+ private: bool isLeftCamera;
+ };
+
+
+ private: StereoCameraListener leftCameraListener;
+ private: StereoCameraListener rightCameraListener;
+};
+
+/// \}
+/// \}
+}
+#endif
+
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
-------------------------------------------------------------------------
Check out the new SourceForge.net Marketplace.
It's the best place to buy or sell services for
just about anything Open Source.
http://ad.doubleclick.net/clk;164216239;13503038;w?http://sf.net/marketplace
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
