Revision: 7737
http://playerstage.svn.sourceforge.net/playerstage/?rev=7737&view=rev
Author: hsujohnhsu
Date: 2009-05-29 02:25:59 +0000 (Fri, 29 May 2009)
Log Message:
-----------
- renamed GetMutex to GetMRMutex.
- MonoCameraSensor supports multiple formats. Defauls is still R8G8B8.
Modified Paths:
--------------
code/gazebo/trunk/server/Global.hh
code/gazebo/trunk/server/Simulator.cc
code/gazebo/trunk/server/Simulator.hh
code/gazebo/trunk/server/World.cc
code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/rendering/OgreCamera.cc
code/gazebo/trunk/server/rendering/OgreCamera.hh
code/gazebo/trunk/server/rendering/OgreCreator.cc
code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc
Modified: code/gazebo/trunk/server/Global.hh
===================================================================
--- code/gazebo/trunk/server/Global.hh 2009-05-29 00:13:51 UTC (rev 7736)
+++ code/gazebo/trunk/server/Global.hh 2009-05-29 02:25:59 UTC (rev 7737)
@@ -80,4 +80,6 @@
#define ROUND(x) ( (int)( floor((x)+0.5) ) )
+//#define TIMING
+
#endif
Modified: code/gazebo/trunk/server/Simulator.cc
===================================================================
--- code/gazebo/trunk/server/Simulator.cc 2009-05-29 00:13:51 UTC (rev
7736)
+++ code/gazebo/trunk/server/Simulator.cc 2009-05-29 02:25:59 UTC (rev
7737)
@@ -628,6 +628,9 @@
while (!this->userQuit)
{
+#ifdef TIMING
+ double tmpT1 = this->GetWallTime();
+#endif
currTime = this->GetRealTime();
if (physicsUpdateRate == 0 ||
@@ -666,12 +669,16 @@
this->userQuit = true;
break;
}
+#ifdef TIMING
+ double tmpT2 = this->GetWallTime();
+ std::cout << " Simulator::PhysicsLoop() DT(" << tmpT2-tmpT1 << ")" <<
std::endl;
+#endif
}
}
////////////////////////////////////////////////////////////////////////////////
/// Get the simulator mutex
-boost::recursive_mutex *Simulator::GetMutex()
+boost::recursive_mutex *Simulator::GetMRMutex()
{
return this->mutex;
}
Modified: code/gazebo/trunk/server/Simulator.hh
===================================================================
--- code/gazebo/trunk/server/Simulator.hh 2009-05-29 00:13:51 UTC (rev
7736)
+++ code/gazebo/trunk/server/Simulator.hh 2009-05-29 02:25:59 UTC (rev
7737)
@@ -179,7 +179,7 @@
public: Body *GetParentBody( Entity *entity ) const;
/// \brief Get the simulator mutex
- public:boost::recursive_mutex *GetMutex();
+ public:boost::recursive_mutex *GetMRMutex();
/// \brief Function to run gui. Used by guiThread
private: void PhysicsLoop();
Modified: code/gazebo/trunk/server/World.cc
===================================================================
--- code/gazebo/trunk/server/World.cc 2009-05-29 00:13:51 UTC (rev 7736)
+++ code/gazebo/trunk/server/World.cc 2009-05-29 02:25:59 UTC (rev 7737)
@@ -363,7 +363,7 @@
// Add a new entity to the world
void World::InsertEntity( std::string xmlString)
{
- boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMutex());
+ boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMRMutex());
this->toLoadEntities.push_back( xmlString );
}
@@ -371,7 +371,7 @@
// Load all the entities that have been queued
void World::ProcessEntitiesToLoad()
{
- boost::recursive_mutex::scoped_lock lock(*Simulator::Instance()->GetMutex());
+ boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMRMutex());
std::vector< std::string >::iterator iter;
for (iter = this->toLoadEntities.begin();
Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc
===================================================================
--- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc
2009-05-29 00:13:51 UTC (rev 7736)
+++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc
2009-05-29 02:25:59 UTC (rev 7737)
@@ -128,7 +128,7 @@
data->width = this->myParent->GetImageWidth();
data->height = this->myParent->GetImageHeight();
- data->image_size = data->width * data->height * 3;
+ data->image_size = data->width * data->height *
this->myParent->GetImageDepth();
// GetFOV() returns radians
data->hfov = *(this->myParent->GetHFOV());
@@ -150,6 +150,7 @@
src = this->myParent->GetImageData(0);
dst = data->image;
+ boost::recursive_mutex::scoped_lock
mr_lock(*Simulator::Instance()->GetMRMutex());
memcpy(dst, src, data->image_size);
this->myParent->EnableSaveFrame( data->saveFrames );
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-05-29 00:13:51 UTC
(rev 7736)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-05-29 02:25:59 UTC
(rev 7737)
@@ -343,8 +343,18 @@
{
double h, kp, kd;
- // skip 0 depth contacts
- if(contactGeoms[i].depth == 0)
+ if (0)
+ std::cout << "dContactGeoms: "
+ << " geom1: " << geom1->GetName()
+ << " geom2: " << geom2->GetName()
+ << " contact points: " << numc
+ << " contact: " << i
+ << " pos: " << contactGeoms[i].pos[0]<<","<<
contactGeoms[i].pos[1]<<","<< contactGeoms[i].pos[2]<<","<<
contactGeoms[i].pos[3]
+ << " norm: " << contactGeoms[i].normal[0]<<","<<
contactGeoms[i].normal[1]<<","<< contactGeoms[i].normal[2]<<","<<
contactGeoms[i].normal[3]
+ << " depth: " << contactGeoms[i].depth
+ << std::endl;
+ // skip negative depth contacts
+ if(contactGeoms[i].depth < 0)
continue;
contact.geom = contactGeoms[i];
Modified: code/gazebo/trunk/server/rendering/OgreCamera.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCamera.cc 2009-05-29 00:13:51 UTC
(rev 7736)
+++ code/gazebo/trunk/server/rendering/OgreCamera.cc 2009-05-29 02:25:59 UTC
(rev 7737)
@@ -73,6 +73,7 @@
this->imageSizeP = new ParamT< Vector2<int> >("imageSize", Vector2<int>(320,
240),0);
this->visMaskP = new ParamT<std::string>("mask","none",0);
this->hfovP = new ParamT<Angle>("hfov", Angle(DTOR(60)),0);
+ this->imageFormatP = new ParamT<std::string>("imageFormat", "R8G8B8", 0);
Param::End();
this->captureData = false;
@@ -96,6 +97,7 @@
delete this->saveFramesP;
delete this->savePathnameP;
delete this->imageSizeP;
+ delete this->imageFormatP;
delete this->visMaskP;
delete this->hfovP;
}
@@ -114,6 +116,7 @@
this->saveFramesP->Load(node);
this->savePathnameP->Load(node);
this->imageSizeP->Load(node);
+ this->imageFormatP->Load(node);
this->visMaskP->Load(node);
this->hfovP->Load(node);
@@ -121,6 +124,18 @@
{
this->visibilityMask ^= GZ_LASER_CAMERA;
}
+
+ if (this->imageFormatP->GetValue() == "L8")
+ this->imageFormat = Ogre::PF_L8;
+ else if (this->imageFormatP->GetValue() == "R8G8B8")
+ this->imageFormat = Ogre::PF_R8G8B8;
+ else if (this->imageFormatP->GetValue() == "B8G8R8")
+ this->imageFormat = Ogre::PF_B8G8R8;
+ else
+ {
+ std::cerr << "Error parsing image format, using default
Ogre::PF_R8G8B8\n";
+ this->imageFormat = Ogre::PF_R8G8B8;
+ }
}
// Create the directory to store frames
@@ -153,6 +168,7 @@
stream << prefix << (*this->saveFramesP) << "\n";
stream << prefix << (*this->savePathnameP) << "\n";
stream << prefix << (*this->imageSizeP) << "\n";
+ stream << prefix << (*this->imageFormatP) << "\n";
stream << prefix << (*this->visMaskP) << "\n";
stream << prefix << (*this->hfovP) << "\n";
}
@@ -224,19 +240,21 @@
if (this->captureData)
{
+ boost::recursive_mutex::scoped_lock
mr_lock(*Simulator::Instance()->GetMRMutex());
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->imageSizeP->GetValue().x * this->imageSizeP->GetValue().y * 3;
+ size = this->imageSizeP->GetValue().x * this->imageSizeP->GetValue().y *
this->GetImageDepth();
// Allocate buffer
if (!this->saveFrameBuffer)
this->saveFrameBuffer = new unsigned char[size];
memset(this->saveFrameBuffer,128,size);
+
mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY);
int top = (int)((mBuffer->getHeight() - this->imageSizeP->GetValue().y) /
2.0);
@@ -252,7 +270,7 @@
this->imageSizeP->GetValue().x,
this->imageSizeP->GetValue().y,
1,
- Ogre::PF_B8G8R8,
+ this->imageFormat,
this->saveFrameBuffer)
);
@@ -353,6 +371,30 @@
}
//////////////////////////////////////////////////////////////////////////////
+/// \brief Get the height of the image
+int OgreCamera::GetImageDepth() const
+{
+ if (this->imageFormatP->GetValue() == "L8")
+ return 1;
+ else if (this->imageFormatP->GetValue() == "R8G8B8")
+ return 3;
+ else if (this->imageFormatP->GetValue() == "B8G8R8")
+ return 3;
+ else
+ {
+ std::cerr << "Error parsing image format, using default Ogre::PF_R8G8B8\n";
+ return 3;
+ }
+}
+
+//////////////////////////////////////////////////////////////////////////////
+/// \brief Get the height of the image
+std::string OgreCamera::GetImageFormat() const
+{
+ return this->imageFormatP->GetValue();
+}
+
+//////////////////////////////////////////////////////////////////////////////
/// Get the width of the texture
unsigned int OgreCamera::GetTextureWidth() const
{
@@ -371,7 +413,7 @@
// Get the image size in bytes
size_t OgreCamera::GetImageByteSize() const
{
- return this->imageSizeP->GetValue().y * this->imageSizeP->GetValue().x * 3;
+ return this->imageSizeP->GetValue().y * this->imageSizeP->GetValue().x *
this->GetImageDepth();
}
@@ -565,8 +607,8 @@
imgData->width = this->imageSizeP->GetValue().x;
imgData->height = this->imageSizeP->GetValue().y;
- imgData->depth = 1;
- imgData->format = Ogre::PF_B8G8R8;
+ imgData->depth = this->GetImageDepth();
+ imgData->format = this->imageFormat;
size = this->GetImageByteSize();
// Wrap buffer in a chunk
Modified: code/gazebo/trunk/server/rendering/OgreCamera.hh
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCamera.hh 2009-05-29 00:13:51 UTC
(rev 7736)
+++ code/gazebo/trunk/server/rendering/OgreCamera.hh 2009-05-29 02:25:59 UTC
(rev 7737)
@@ -123,6 +123,12 @@
/// \brief Get the height of the image
public: unsigned int GetImageHeight() const;
+ /// \brief Get the height of the image
+ public: int GetImageDepth() const;
+
+ /// \brief Get the height of the image
+ public: std::string GetImageFormat() const;
+
/// \brief Get the height of the texture
public: unsigned int GetTextureHeight() const;
@@ -218,8 +224,10 @@
protected: unsigned int saveCount;
protected: ParamT<bool> *saveFramesP;
protected: ParamT<std::string> *savePathnameP;
+ protected: ParamT<std::string> *imageFormatP;
protected: ParamT<std::string> *visMaskP;
+ protected: Ogre::PixelFormat imageFormat;
protected: unsigned int visibilityMask;
protected: Ogre::RenderTarget *renderTarget;
Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreCreator.cc 2009-05-29 00:13:51 UTC
(rev 7736)
+++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2009-05-29 02:25:59 UTC
(rev 7737)
@@ -759,7 +759,7 @@
}
{
- boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMutex());
+ boost::recursive_mutex::scoped_lock
lock(*Simulator::Instance()->GetMRMutex());
// Update the visuals
for (viter = this->visuals.begin(); viter != this->visuals.end(); viter++)
Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc
===================================================================
--- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2009-05-29
00:13:51 UTC (rev 7736)
+++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2009-05-29
02:25:59 UTC (rev 7737)
@@ -89,7 +89,7 @@
this->imageSizeP->GetValue().x,
this->imageSizeP->GetValue().y,
0,
- Ogre::PF_R8G8B8,
+ this->imageFormat,
Ogre::TU_RENDERTARGET);
this->renderTarget = this->renderTexture->getBuffer()->getRenderTarget();
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
Register Now for Creativity and Technology (CaT), June 3rd, NYC. CaT
is a gathering of tech-side developers & brand creativity professionals. Meet
the minds behind Google Creative Lab, Visual Complexity, Processing, &
iPhoneDevCamp as they present alongside digital heavyweights like Barbarian
Group, R/GA, & Big Spaceship. http://p.sf.net/sfu/creativitycat-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
