From: <na...@us...> - 2008-03-25 18:04:25
|
Revision: 4440 http://playerstage.svn.sourceforge.net/playerstage/?rev=4440&view=rev Author: natepak Date: 2008-03-25 18:04:24 -0700 (Tue, 25 Mar 2008) Log Message: ----------- Adding in stereo vision Modified Paths: -------------- code/gazebo/trunk/player/CameraInterface.cc code/gazebo/trunk/worlds/models/bandit.model Added Paths: ----------- code/gazebo/trunk/worlds/stereocamera.world Modified: code/gazebo/trunk/player/CameraInterface.cc =================================================================== --- code/gazebo/trunk/player/CameraInterface.cc 2008-03-26 01:04:07 UTC (rev 4439) +++ code/gazebo/trunk/player/CameraInterface.cc 2008-03-26 01:04:24 UTC (rev 4440) @@ -101,31 +101,24 @@ unsigned int oldCount = this->data.image_count; this->data.image_count = this->iface->data->image_size; - printf("Iface Image Size[%d]\n", this->iface->data->image_size); - if (oldCount != this->data.image_count) { delete this->data.image; this->data.image = new uint8_t[this->data.image_count]; - printf("Created array of size[%d]\n",this->data.image_count); } - printf("1\n"); // Set the image pixels memcpy(this->data.image, this->iface->data->image, this->iface->data->image_size); - printf("2\n"); size = sizeof(this->data) - sizeof(this->data.image) + this->iface->data->image_size; - printf("3\n"); // Send data to server this->driver->Publish(this->device_addr, PLAYER_MSGTYPE_DATA, PLAYER_CAMERA_DATA_STATE, (void*)&this->data, size, &this->datatime); - printf("4\n"); // Save frames if (this->save) Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-03-26 01:04:07 UTC (rev 4439) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-03-26 01:04:24 UTC (rev 4440) @@ -183,7 +183,7 @@ <body1>right_shoulder_body</body1> <body2>right_bicep_body</body2> <anchor>right_shoulder_body</anchor> - <anchorOffset>0 0 -0.052</anchorOffset> + <anchorOffset>0 0.0 -0.052</anchorOffset> <axis>0 0 1</axis> <lowStop>-90</lowStop> <highStop>90</highStop> Added: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world (rev 0) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-03-26 01:04:24 UTC (rev 4440) @@ -0,0 +1,109 @@ +<?xml version="1.0"?> + +<gazebo:world + xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > + + <verbosity>5</verbosity> + + <physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.8</gravity> + <cfm>0.008</cfm> + <erp>0.2</erp> + </physics:ode> + + <rendering:gui> + <type>fltk</type> + <size>800 600</size> + <pos>0 0</pos> + </rendering:gui> + + <rendering:ogre> + <ambient>1.0 1.0 1.0 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + + </rendering:ogre> + + <model:physical name="sphere1_model"> + <xyz>1 0 1.5</xyz> + <rpy>0.0 0.0 0.0</rpy> + <static>false</static> + + <body:sphere name="sphere1_body"> + <geom:sphere name="sphere1_geom"> + <size>0.5</size> + <mass>1.0</mass> + + <visual> + <scale>0.5 0.5 0.5</scale> + <mesh>unit_sphere</mesh> + <material>Gazebo/Rocky</material> + </visual> + </geom:sphere> + </body:sphere> + </model:physical> + + <model:physical name="box1_model"> + <xyz>1 1.5 0.5</xyz> + <canonicalBody>box1_body</canonicalBody> + <static>false</static> + + <body:box name="box1_body"> + + <geom:box name="box1_geom"> + <size>1 1 1</size> + <mass>1.0</mass> + <visual> + <mesh>unit_box</mesh> + <material>Gazebo/BumpyMetal</material> + </visual> + </geom:box> + </body:box> + </model:physical> + + <!-- Ground Plane --> + <model:physical name="plane1_model"> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + <static>true</static> + + <body:plane name="plane1_body"> + <geom:plane name="plane1_geom"> + <normal>0 0 1</normal> + <size>2000 2000</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/GrassFloor</material> + </geom:plane> + </body:plane> + </model:physical> + + <model:physical name="cam1_model"> + <xyz>-6 0 3.5</xyz> + <rpy>0 28 0</rpy> + <static>true</static> + + <body:empty name="cam1_body"> + <sensor:stereocamera name="cam1_sensor"> + <imageSize>800 600</imageSize> + <hfov>60</hfov> + <nearClip>0.1</nearClip> + <farClip>100</farClip> + <saveFrames>false</saveFrames> + <saveFramePath>frames</saveFramePath> + </sensor:stereocamera> + </body:empty> + </model:physical> + +</gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-03-26 15:53:47
|
Revision: 4444 http://playerstage.svn.sourceforge.net/playerstage/?rev=4444&view=rev Author: natepak Date: 2008-03-26 15:53:54 -0700 (Wed, 26 Mar 2008) Log Message: ----------- Updates to stereo vision Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/server/GazeboMessage.cc code/gazebo/trunk/server/GazeboMessage.hh code/gazebo/trunk/server/main.cc code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/sensors/camera/CameraSensor.hh 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 code/gazebo/trunk/worlds/stereocamera.world code/gazebo/trunk/worlds/test.world Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/SConstruct 2008-03-26 22:53:54 UTC (rev 4444) @@ -85,7 +85,8 @@ if env['mode'] == 'debug': env['CCFLAGS'] += Split('-ggdb -g3') elif env['mode'] == 'profile': - env['CCFLAGS'] += Split('-p -pg') + env['CCFLAGS'] += Split('-pg') + env['LINKFLAGS'] += Split('-pg') elif env['mode'] == 'optimized': env['CCFLAGS'] += Split('-O3') Modified: code/gazebo/trunk/server/GazeboMessage.cc =================================================================== --- code/gazebo/trunk/server/GazeboMessage.cc 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/GazeboMessage.cc 2008-03-26 22:53:54 UTC (rev 4444) @@ -39,6 +39,8 @@ GazeboMessage::GazeboMessage() { this->msgStream = &std::cout; + this->errStream = &std::cerr; + this->level = 0; } @@ -128,6 +130,16 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Get the error stream +std::ostream &GazeboMessage::Err( int msglevel ) +{ + if (msglevel <= this->level) + return *this->errStream; + else + return this->nullStream; +} + +//////////////////////////////////////////////////////////////////////////////// // Log a message std::ofstream &GazeboMessage::Log() { Modified: code/gazebo/trunk/server/GazeboMessage.hh =================================================================== --- code/gazebo/trunk/server/GazeboMessage.hh 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/GazeboMessage.hh 2008-03-26 22:53:54 UTC (rev 4444) @@ -42,6 +42,8 @@ /// Output a message #define gzmsg(level) (gazebo::GazeboMessage::Instance()->Msg(level) << "[" << __FILE__ << ":" << __LINE__ << "]\n ") + + #define gzerr(level) (gazebo::GazeboMessage::Instance()->Err(level) << "Error: [" << __FILE__ << ":" << __LINE__ << "]\n ") /// Log a message #define gzlog() (gazebo::GazeboMessage::Instance()->Log() << "[" << __FILE__ << ":" << __LINE__ << "] ") @@ -85,6 +87,10 @@ /// \brief Use this to output a message to the terminal /// \param level Level of the message public: std::ostream &Msg( int level = 0 ); + + /// \brief Use this to output an error to the terminal + /// \param level Level of the message + public: std::ostream &Err( int level = 0 ); /// \brief Use this to output a message to a log file public: std::ofstream &Log(); @@ -97,6 +103,7 @@ private: std::ostringstream nullStream; private: std::ostream *msgStream; + private: std::ostream *errStream; private: std::ofstream logStream; /// Pointer to myself Modified: code/gazebo/trunk/server/main.cc =================================================================== --- code/gazebo/trunk/server/main.cc 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/main.cc 2008-03-26 22:53:54 UTC (rev 4444) @@ -331,6 +331,8 @@ return -1; } + printf("Finalize\n"); + // Finalization and clean up try { @@ -343,5 +345,6 @@ return -1; } + printf("Quit\n"); return 0; } Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-03-26 22:53:54 UTC (rev 4444) @@ -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); Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-03-26 22:53:54 UTC (rev 4444) @@ -73,7 +73,7 @@ Pose3d pose; Vector3 size; Ogre::Vector3 meshSize; - Ogre::MovableObject *obj; + Ogre::MovableObject *obj = NULL; this->xmlNode=node; std::string meshName = node->GetString("mesh","",1); @@ -91,17 +91,20 @@ } catch (Ogre::Exception e) { + std::cerr << "Ogre Error:" << e.getFullDescription() << "\n"; gzthrow("Unable to create a mesh from " + meshName); } // Attach the entity to the node - this->AttachObject(obj); + if (obj) + this->AttachObject(obj); // Set the pose of the scene node this->SetPose(pose); // Get the size of the mesh - meshSize = obj->getBoundingBox().getSize(); + if (obj) + meshSize = obj->getBoundingBox().getSize(); // Get the desired size of the mesh if (node->GetChild("size") != NULL) Modified: code/gazebo/trunk/server/sensors/camera/CameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/CameraSensor.hh 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/sensors/camera/CameraSensor.hh 2008-03-26 22:53:54 UTC (rev 4444) @@ -109,12 +109,11 @@ /// \brief Get the height of the texture public: unsigned int GetTextureHeight() const; - /// \brief Get a pointer to the image data - public: virtual const unsigned char *GetImageData() = 0; - /// \brief Get the image size in bytes public: size_t GetImageByteSize() const; + public: virtual const unsigned char *GetImageData(unsigned int i=0) = 0; + /// \brief Get the Z-buffer value at the given image coordinate. /// /// \param x, y Image coordinate; (0, 0) specifies the top-left corner. Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-03-26 22:53:54 UTC (rev 4444) @@ -93,6 +93,8 @@ Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + mat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName); + Ogre::HardwarePixelBufferSharedPtr mBuffer; // Get access to the buffer and make an image and write it to file @@ -128,8 +130,11 @@ ////////////////////////////////////////////////////////////////////////////// /// Get a pointer to the image data -const unsigned char *MonoCameraSensor::GetImageData() +const unsigned char *MonoCameraSensor::GetImageData(unsigned int i) { + if (i!=0) + gzerr(0) << "Camera index must be zero for mono cam"; + Ogre::HardwarePixelBufferSharedPtr mBuffer; size_t size; Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh 2008-03-26 22:53:54 UTC (rev 4444) @@ -82,7 +82,7 @@ public: virtual std::string GetMaterialName() const; /// \brief Get a pointer to the image data - public: virtual const unsigned char *GetImageData(); + public: virtual const unsigned char *GetImageData(unsigned int i=0); // Save the camera frame private: void SaveFrame(); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-03-26 22:53:54 UTC (rev 4444) @@ -26,6 +26,7 @@ #include <sstream> #include <OgreImageCodec.h> +#include <GL/gl.h> #include <Ogre.h> #include "Global.hh" @@ -64,19 +65,22 @@ void StereoCameraSensor::LoadChild( XMLConfigNode *node ) { CameraSensor::LoadChild(node); + + this->baseline = node->GetDouble("baseline",0,1); } ////////////////////////////////////////////////////////////////////////////// // Initialize the camera void StereoCameraSensor::InitChild() { - this->leftOgreTextureName = this->GetName() + "_LEFTRttTex"; - this->rightOgreTextureName = this->GetName() + "_RIGHTRttTex"; + this->leftOgreTextureName = this->GetName() + "_RttTex_Stereo_Left"; + this->rightOgreTextureName = this->GetName() + "_RttTex_Stereo_Right"; - this->ogreMaterialName = this->GetName() + "_RttMat"; + this->leftOgreMaterialName = this->GetName() + "_RttMat_Stereo_Left"; + this->rightOgreMaterialName = this->GetName() + "_RttMat_Stereo_Right"; // Create the render texture - this->leftRenderTexture = Ogre::TextureManager::getSingleton().createManual( + this->renderTexture[0] = Ogre::TextureManager::getSingleton().createManual( this->leftOgreTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, @@ -84,9 +88,9 @@ Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET); - this->leftRenderTarget = this->leftRenderTexture->getBuffer()->getRenderTarget(); + this->leftRenderTarget = this->renderTexture[0]->getBuffer()->getRenderTarget(); - this->rightRenderTexture = Ogre::TextureManager::getSingleton().createManual( + this->renderTexture[1] = Ogre::TextureManager::getSingleton().createManual( this->rightOgreTextureName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, @@ -94,7 +98,7 @@ Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET); - this->rightRenderTarget = this->rightRenderTexture->getBuffer()->getRenderTarget(); + this->rightRenderTarget = this->renderTexture[1]->getBuffer()->getRenderTarget(); // Create the camera this->camera = OgreCreator::CreateCamera(this->GetName(), @@ -111,20 +115,27 @@ cviewport->setOverlaysEnabled(false); } - Ogre::MaterialPtr mat = Ogre::MaterialManager::getSingleton().create( - this->ogreMaterialName, + Ogre::MaterialPtr leftmat = Ogre::MaterialManager::getSingleton().create( + this->leftOgreMaterialName, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + Ogre::MaterialPtr rightmat = Ogre::MaterialManager::getSingleton().create( + this->rightOgreMaterialName, + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + + leftmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->leftOgreTextureName); + rightmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->rightOgreTextureName); + Ogre::HardwarePixelBufferSharedPtr mBuffer; // Get access to the buffer and make an image and write it to file - mBuffer = this->leftRenderTexture->getBuffer(0, 0); + mBuffer = this->renderTexture[0]->getBuffer(0, 0); this->textureWidth = mBuffer->getWidth(); this->textureHeight = mBuffer->getHeight(); - this->leftCameraListener.Init(this, true); - this->rightCameraListener.Init(this, false); + this->leftCameraListener.Init(this, this->leftRenderTarget, true); + this->rightCameraListener.Init(this, this->rightRenderTarget, false); this->leftRenderTarget->addListener(&this->leftCameraListener); this->rightRenderTarget->addListener(&this->rightCameraListener); @@ -149,18 +160,24 @@ // Return the material the camera renders to std::string StereoCameraSensor::GetMaterialName() const { - return this->ogreMaterialName; + return this->leftOgreMaterialName; } ////////////////////////////////////////////////////////////////////////////// /// Get a pointer to the image data -const unsigned char *StereoCameraSensor::GetImageData() +const unsigned char *StereoCameraSensor::GetImageData(unsigned int i) { Ogre::HardwarePixelBufferSharedPtr mBuffer; size_t size; + if (i > 1) + { + gzerr(0) << "Camera index must be 0=Left or 1=Right for stereo camera\n"; + i = 1; + } + // Get access to the buffer and make an image and write it to file - mBuffer = this->leftRenderTexture->getBuffer(0, 0); + mBuffer = this->renderTexture[i]->getBuffer(0, 0); size = this->imageWidth * this->imageHeight * 3; @@ -202,94 +219,166 @@ Ogre::Codec * pCodec; size_t size, pos; - this->GetImageData(); + for (int i=0; i<2; i++) + { + this->GetImageData(i); - // Get access to the buffer and make an image and write it to file - mBuffer = this->leftRenderTexture->getBuffer(0, 0); + // Get access to the buffer and make an image and write it to file + mBuffer = this->renderTexture[i]->getBuffer(0, 0); - // Create image data structure - imgData = new Ogre::ImageCodec::ImageData(); + // 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(); + 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)); + // 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(), + char tmp[1024]; + if (!this->savePathname.empty()) + { + if (i==0) + sprintf(tmp, "%s/%s-%04d-left.png", this->savePathname.c_str(), this->GetName().c_str(), this->saveCount); + else + sprintf(tmp, "%s/%s-%04d-right.png", this->savePathname.c_str(), + this->GetName().c_str(), this->saveCount); + } + else + { + if (i==0) + sprintf(tmp, "%s-%04d-left.png", this->GetName().c_str(), this->saveCount); + else + sprintf(tmp, "%s-%04d-right.png", 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); } - else - { - sprintf(tmp, "%s-%04d.jpg", this->GetName().c_str(), this->saveCount); + + this->saveCount++; +} + +//void StereCameraSensor::UpdateAllDependentRenderTargets() +//{ +/* Ogre::RenderTargetList::iterator iter; + + for( iter = mRenderTargetList.begin(); iter != mRenderTargetList.end(); ++iter ) + { (*iter)->update(); } + */ +//} - // Get codec - Ogre::String filename = tmp; - pos = filename.find_last_of("."); - Ogre::String extension; +//////////////////////////////////////////////////////////////////////////////// +/// Get the baselien of the camera +double StereoCameraSensor::GetBaseline() const +{ + return this->baseline; +} - while (pos != filename.length() - 1) - extension += filename[++pos]; +//////////////////////////////////////////////////////////////////////////////// +// Read the depth data +void StereoCameraSensor::ReadDepthImage() +{ + float *depthImage = new float[this->imageWidth * this->imageHeight]; - // Get the codec - pCodec = Ogre::Codec::getCodec(extension); + glPixelStorei(GL_PACK_ALIGNMENT, 1); + glReadBuffer(GL_BACK); + glReadPixels(0, 0, this->imageWidth, this->imageHeight, GL_DEPTH_COMPONENT, GL_FLOAT, depthImage); - // Write out - Ogre::Codec::CodecDataPtr codecDataPtr(imgData); - pCodec->codeToFile(stream, filename, codecDataPtr); + char tmp[1024]; + FILE *fp; - this->saveCount++; + // Save depth image + sprintf(tmp, "left_depth_%04d.pnm", this->saveCount++); + fp = fopen( tmp, "wb" ); + if (!fp) + { + printf( "unable to open file %s\n for writing\n", tmp ); + return; + } + + fprintf( fp, "P5\n# Gazebo\n%d %d\n255\n", this->imageWidth, this->imageHeight); + unsigned char *dst = new unsigned char[this->imageWidth]; + float a = (this->nearClip * this->farClip) / (this->farClip - this->nearClip); + float b = -this->nearClip / (this->farClip - this->nearClip); + + for (int i = this->imageHeight-1; i >= 0; i--) + { + float *src = depthImage + i * this->imageWidth; + for (int j = 0; j < this->imageWidth; j++) + { + dst[j] = src[j]; + printf("%4.2f ",src[j]); + /*if (src[j] < 1e-6) + dst[j] = 0; + else + dst[j] = (int) (255 * (a / src[j] + b)); + */ + } + printf("\n"); + fwrite( dst, 1, this->imageWidth, fp); + } + fclose( fp); + } void StereoCameraSensor::StereoCameraListener::Init( - StereoCameraSensor *cam, bool isLeft) + StereoCameraSensor *cam, Ogre::RenderTarget *target, bool isLeft) { this->sensor = cam; this->camera = this->sensor->GetOgreCamera(); + this->renderTarget = target; 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 != this->renderTarget->getViewport(0)) + { + printf("Invalid viewport"); + return; + } -/* if(evt.source != mViewport) - return; - Real offset = mStereoMgr->getEyesSpacing()/2; - if(mIsLeftEye) + double offset = this->sensor->GetBaseline()/2; + + if(this->isLeftCamera) { offset = -offset; } - mCamera->setFrustumOffset(-offset,0); - mPos = mCamera->getPosition(); - Vector3 pos = mPos; - pos += offset * mCamera->getRight(); - mCamera->setPosition(pos); - mStereoMgr->updateAllDependentRenderTargets(); - mStereoMgr->chooseDebugPlaneMaterial(mIsLeftEye); - */ + this->camera->setFrustumOffset(-offset,0); + + this->pos = this->camera->getPosition(); + Ogre::Vector3 pos = this->pos; + + pos += offset * this->camera->getRight(); + this->camera->setPosition(pos); + + //this->sensor->UpdateAllDependentRenderTargets(); + //this->sensor->chooseDebugPlaneMaterial(mIsLeftEye); } void StereoCameraSensor::StereoCameraListener::postViewportUpdate(const Ogre::RenderTargetViewportEvent &evt) { - if (this->isLeftCamera) - printf("Left Post\n"); - else - printf("Rightt Post\n"); + this->sensor->ReadDepthImage(); -/* mCamera->setFrustumOffset(0,0); - mCamera->setPosition(mPos); - */ - + this->camera->setFrustumOffset(0,0); + this->camera->setPosition(this->pos); } Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-03-26 22:53:54 UTC (rev 4444) @@ -81,32 +81,43 @@ public: virtual std::string GetMaterialName() const; /// \brief Get a pointer to the image data - public: virtual const unsigned char *GetImageData(); + public: virtual const unsigned char *GetImageData(unsigned int i=0); + /// \brief Get the baselien of the camera + public: double GetBaseline() const; + // Save the camera frame protected: virtual void SaveFrame(); - private: Ogre::TexturePtr leftRenderTexture; + private: void ReadDepthImage(); + + //private: void UpdateAllDependentRenderTargets(); + + private: Ogre::TexturePtr renderTexture[2]; 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: std::string leftOgreMaterialName; + private: std::string rightOgreMaterialName; + private: double baseline; + private: class StereoCameraListener : public Ogre::RenderTargetListener { public: StereoCameraListener() : Ogre::RenderTargetListener() {} - public: void Init(StereoCameraSensor *sensor, bool isLeft); + + public: void Init(StereoCameraSensor *sensor, Ogre::RenderTarget *target, 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: Ogre::RenderTarget *renderTarget; private: bool isLeftCamera; }; Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-03-26 22:53:54 UTC (rev 4444) @@ -36,7 +36,7 @@ </rendering:ogre> <model:physical name="sphere1_model"> - <xyz>1 0 1.5</xyz> + <xyz>4 -1 1.5</xyz> <rpy>0.0 0.0 0.0</rpy> <static>false</static> @@ -55,7 +55,7 @@ </model:physical> <model:physical name="box1_model"> - <xyz>1 1.5 0.5</xyz> + <xyz>4 1 0.5</xyz> <canonicalBody>box1_body</canonicalBody> <static>false</static> @@ -90,8 +90,8 @@ </model:physical> <model:physical name="cam1_model"> - <xyz>-6 0 3.5</xyz> - <rpy>0 28 0</rpy> + <xyz>0 0 1</xyz> + <rpy>0 0 0</rpy> <static>true</static> <body:empty name="cam1_body"> @@ -102,6 +102,7 @@ <farClip>100</farClip> <saveFrames>false</saveFrames> <saveFramePath>frames</saveFramePath> + <baseline>0.2</baseline> </sensor:stereocamera> </body:empty> </model:physical> Modified: code/gazebo/trunk/worlds/test.world =================================================================== --- code/gazebo/trunk/worlds/test.world 2008-03-26 08:59:00 UTC (rev 4443) +++ code/gazebo/trunk/worlds/test.world 2008-03-26 22:53:54 UTC (rev 4444) @@ -105,41 +105,6 @@ </include> --> - <model:physical name="pioneer2dx_model1"> - <xyz>0 0 0.145</xyz> - <rpy>0.0 0.0 0.0</rpy> - - <controller:differential_position2d name="controller1"> - <leftJoint>left_wheel_hinge</leftJoint> - <rightJoint>right_wheel_hinge</rightJoint> - <wheelSeparation>0.34</wheelSeparation> - <wheelDiameter>0.15</wheelDiameter> - <torque>5</torque> - <interface:position name="position_iface_0"/> - </controller:differential_position2d> - - <model:physical name="laser"> - <xyz>0.15 0 0.18</xyz> - - <attach> - <parentBody>chassis_body</parentBody> - <myBody>laser_body</myBody> - </attach> - - <include embedded="true"> - <xi:include href="models/sicklms200.model" /> - </include> - </model:physical> - - <!-- - The include should be last within a model. All previous statements - will override those in the included file - --> - <include embedded="true"> - <xi:include href="models/pioneer2dx.model" /> - </include> - </model:physical> - <!-- White Directional light --> <model:renderable name="directional_white"> <light> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-03-28 08:44:23
|
Revision: 4454 http://playerstage.svn.sourceforge.net/playerstage/?rev=4454&view=rev Author: natepak Date: 2008-03-28 08:43:42 -0700 (Fri, 28 Mar 2008) Log Message: ----------- Updates to the stereo camera Modified Paths: -------------- code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/worlds/models/bandit.model code/gazebo/trunk/worlds/stereocamera.world Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-03-28 09:31:36 UTC (rev 4453) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-03-28 15:43:42 UTC (rev 4454) @@ -197,6 +197,7 @@ gzthrow("Unsupported shadow technique:\n" << shadowTechnique); this->sceneMgr->setShadowTextureSelfShadow(true); + this->sceneMgr->setShadowTexturePixelFormat(Ogre::PF_FLOAT32_R); this->sceneMgr->setShadowTextureSize(node->GetInt("shadowTextureSize", 512)); this->sceneMgr->setShadowIndexBufferSize( node->GetInt("shadowIndexSize",this->sceneMgr->getShadowIndexBufferSize()) ); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-03-28 09:31:36 UTC (rev 4453) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-03-28 15:43:42 UTC (rev 4454) @@ -73,59 +73,130 @@ // Initialize the camera void StereoCameraSensor::InitChild() { - this->leftOgreTextureName = this->GetName() + "_RttTex_Stereo_Left"; - this->rightOgreTextureName = this->GetName() + "_RttTex_Stereo_Right"; + this->ogreTextureName[0] = this->GetName() + "_RttTex_Stereo_Left"; + this->ogreTextureName[1] = this->GetName() + "_RttTex_Stereo_Right"; - this->leftOgreMaterialName = this->GetName() + "_RttMat_Stereo_Left"; - this->rightOgreMaterialName = this->GetName() + "_RttMat_Stereo_Right"; + this->ogreDepthTextureName[0] = this->GetName() +"_RttTex_Stereo_Left_Depth"; + this->ogreDepthTextureName[1] = this->GetName() +"_RttTex_Stereo_Right_Depth"; - // Create the render texture + this->ogreMaterialName[0] = this->GetName() + "_RttMat_Stereo_Left"; + this->ogreMaterialName[1] = this->GetName() + "_RttMat_Stereo_Right"; + + this->ogreDepthMaterialName[0] = this->GetName()+"_RttMat_Stereo_Left_Depth"; + this->ogreDepthMaterialName[1] = this->GetName()+"_RttMat_Stereo_Right_Depth"; + + + // Create the left render texture this->renderTexture[0] = Ogre::TextureManager::getSingleton().createManual( - this->leftOgreTextureName, + this->ogreTextureName[0], Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, this->imageWidth, this->imageHeight, 0, Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET); - this->leftRenderTarget = this->renderTexture[0]->getBuffer()->getRenderTarget(); + this->renderTarget[0] = this->renderTexture[0]->getBuffer()->getRenderTarget(); + // Create the right render texture this->renderTexture[1] = Ogre::TextureManager::getSingleton().createManual( - this->rightOgreTextureName, + this->ogreTextureName[1], Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, this->imageWidth, this->imageHeight, 0, Ogre::PF_R8G8B8, Ogre::TU_RENDERTARGET); - this->rightRenderTarget = this->renderTexture[1]->getBuffer()->getRenderTarget(); + this->renderTarget[1] = this->renderTexture[1]->getBuffer()->getRenderTarget(); + // Create the left depth render texture + this->depthRenderTexture[0] = + Ogre::TextureManager::getSingleton().createManual( + this->ogreDepthTextureName[0], + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, + this->imageWidth, this->imageHeight, 0, + Ogre::PF_FLOAT32_R, + //Ogre::PF_DEPTH, + //Ogre::PF_R8G8B8, + Ogre::TU_RENDERTARGET); + + this->depthRenderTarget[0] = this->depthRenderTexture[0]->getBuffer()->getRenderTarget(); + + // Create the right depth render texture + this->depthRenderTexture[1] = + Ogre::TextureManager::getSingleton().createManual( + this->ogreDepthTextureName[1], + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, + this->imageWidth, this->imageHeight, 0, + Ogre::PF_FLOAT32_R, + //Ogre::PF_DEPTH, + //Ogre::PF_R8G8B8, + Ogre::TU_RENDERTARGET); + + this->depthRenderTarget[1] = this->depthRenderTexture[1]->getBuffer()->getRenderTarget(); + + + // Create the camera this->camera = OgreCreator::CreateCamera(this->GetName(), - this->nearClip, this->farClip, this->hfov, this->leftRenderTarget); + this->nearClip, this->farClip, this->hfov, this->renderTarget[0]); // 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 = this->renderTarget[1]->addViewport(camera); cviewport->setClearEveryFrame(true); cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); cviewport->setOverlaysEnabled(false); + + cviewport = this->depthRenderTarget[0]->addViewport(camera); + cviewport->setClearEveryFrame(true); + cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); + cviewport->setOverlaysEnabled(false); + + cviewport = this->depthRenderTarget[1]->addViewport(camera); + cviewport->setClearEveryFrame(true); + cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); + cviewport->setOverlaysEnabled(false); } Ogre::MaterialPtr leftmat = Ogre::MaterialManager::getSingleton().create( - this->leftOgreMaterialName, - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + this->ogreMaterialName[0], + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + Ogre::MaterialPtr rightmat = Ogre::MaterialManager::getSingleton().create( - this->rightOgreMaterialName, - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + this->ogreMaterialName[1], + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + leftmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName[0]); + rightmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName[1]); - leftmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->leftOgreTextureName); - rightmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->rightOgreTextureName); + Ogre::MaterialPtr leftdepthmat = Ogre::MaterialManager::getSingleton().create( + this->ogreDepthMaterialName[0], + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + Ogre::MaterialPtr rightdepthmat=Ogre::MaterialManager::getSingleton().create( + this->ogreDepthMaterialName[1], + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + + //leftdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreDepthTextureName[0]); + Ogre::TextureUnitState *t = leftdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(OgreAdaptor::Instance()->sceneMgr->getShadowTexture(0)->getName()); + leftdepthmat->getTechnique(0)->getPass(0)->setLightingEnabled(false); + t->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); + + //leftdepthmat->getTechnique(0)->getPass(0)->setDepthWriteEnabled(true); + //leftdepthmat->getTechnique(0)->getPass(0)->setColourWriteEnabled(false); + + rightdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreDepthTextureName[1]); + rightdepthmat->getTechnique(0)->getPass(0)->setLightingEnabled(false); + //rightdepthmat->getTechnique(0)->getPass(0)->setDepthWriteEnabled(true); + //rightdepthmat->getTechnique(0)->getPass(0)->setColourWriteEnabled(false); + + + Ogre::HardwarePixelBufferSharedPtr mBuffer; // Get access to the buffer and make an image and write it to file @@ -134,11 +205,11 @@ this->textureWidth = mBuffer->getWidth(); this->textureHeight = mBuffer->getHeight(); - this->leftCameraListener.Init(this, this->leftRenderTarget, true); - this->rightCameraListener.Init(this, this->rightRenderTarget, false); + this->leftCameraListener.Init(this, this->renderTarget[0], true); + this->rightCameraListener.Init(this, this->renderTarget[1], false); - this->leftRenderTarget->addListener(&this->leftCameraListener); - this->rightRenderTarget->addListener(&this->rightCameraListener); + this->renderTarget[0]->addListener(&this->leftCameraListener); + this->renderTarget[1]->addListener(&this->rightCameraListener); CameraSensor::InitChild(); } @@ -160,7 +231,7 @@ // Return the material the camera renders to std::string StereoCameraSensor::GetMaterialName() const { - return this->leftOgreMaterialName; + return this->ogreDepthMaterialName[0]; } ////////////////////////////////////////////////////////////////////////////// @@ -177,7 +248,7 @@ } // Get access to the buffer and make an image and write it to file - mBuffer = this->renderTexture[i]->getBuffer(0, 0); + mBuffer = this->depthRenderTexture[i]->getBuffer(0, 0); size = this->imageWidth * this->imageHeight * 3; @@ -324,13 +395,10 @@ float *src = depthImage + i * this->imageWidth; for (int j = 0; j < this->imageWidth; j++) { - dst[j] = src[j]; - printf("%4.2f ",src[j]); - /*if (src[j] < 1e-6) + if (src[j] < 1e-6) dst[j] = 0; else dst[j] = (int) (255 * (a / src[j] + b)); - */ } printf("\n"); fwrite( dst, 1, this->imageWidth, fp); @@ -377,7 +445,7 @@ void StereoCameraSensor::StereoCameraListener::postViewportUpdate(const Ogre::RenderTargetViewportEvent &evt) { - this->sensor->ReadDepthImage(); + //this->sensor->ReadDepthImage(); this->camera->setFrustumOffset(0,0); this->camera->setPosition(this->pos); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-03-28 09:31:36 UTC (rev 4453) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-03-28 15:43:42 UTC (rev 4454) @@ -94,15 +94,18 @@ //private: void UpdateAllDependentRenderTargets(); private: Ogre::TexturePtr renderTexture[2]; - private: Ogre::RenderTarget *leftRenderTarget; - private: Ogre::RenderTarget *rightRenderTarget; + private: Ogre::TexturePtr depthRenderTexture[2]; + private: Ogre::RenderTarget *renderTarget[2]; + private: Ogre::RenderTarget *depthRenderTarget[2]; - private: std::string leftOgreTextureName; - private: std::string rightOgreTextureName; - private: std::string leftOgreMaterialName; - private: std::string rightOgreMaterialName; + private: std::string ogreTextureName[2]; + private: std::string ogreMaterialName[2]; + private: std::string ogreDepthTextureName[2]; + private: std::string ogreDepthMaterialName[2]; + + private: double baseline; private: Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-03-28 09:31:36 UTC (rev 4453) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-03-28 15:43:42 UTC (rev 4454) @@ -235,8 +235,8 @@ <anchor>right_forearm_body</anchor> <anchorOffset>0 0 -0.05</anchorOffset> <axis>0 0 1</axis> - <lowStop>-1</lowStop> - <highStop>90</highStop> + <lowStop>-90</lowStop> + <highStop>1</highStop> </joint:hinge> <body:box name="right_hand_body"> Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-03-28 09:31:36 UTC (rev 4453) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-03-28 15:43:42 UTC (rev 4454) @@ -32,6 +32,7 @@ <sky> <material>Gazebo/CloudySky</material> </sky> + <!--<shadowTechnique>textureAdditive</shadowTechnique>--> </rendering:ogre> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-04-01 15:22:37
|
Revision: 4466 http://playerstage.svn.sourceforge.net/playerstage/?rev=4466&view=rev Author: natepak Date: 2008-04-01 15:22:40 -0700 (Tue, 01 Apr 2008) Log Message: ----------- Fixed the terrain geometry Modified Paths: -------------- code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/HeightmapGeom.cc code/gazebo/trunk/worlds/terrain.world code/gazebo/trunk/worlds/test.world Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-03-31 20:32:03 UTC (rev 4465) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-04-01 22:22:40 UTC (rev 4466) @@ -113,7 +113,6 @@ pose.pos = node->GetVector3("xyz",Vector3(0,0,0)); pose.rot = node->GetRotation("rpy",Quatern()); - // TODO: This should probably be true....but "true" breaks trimesh postions. this->SetPose(pose, true); @@ -279,7 +278,6 @@ // Set the pose relative to the body void Geom::SetPose(const Pose3d &pose, bool updateCoM) { - if (this->placeable && this->geomId) { Pose3d localPose; Modified: code/gazebo/trunk/server/physics/HeightmapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-03-31 20:32:03 UTC (rev 4465) +++ code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-04-01 22:22:40 UTC (rev 4466) @@ -28,6 +28,7 @@ #include <Ogre.h> #include <iostream> #include <string.h> +#include <math.h> #include "GazeboError.hh" #include "OgreAdaptor.hh" @@ -147,16 +148,19 @@ this->terrainVertSize = tmpImage.getWidth(); + float nf = log(this->terrainVertSize-1)/log(2); + int ni = log(this->terrainVertSize-1)/log(2); // Make sure the heightmap image size is (2^n)+1 in size - if ( (this->terrainVertSize-1) & (this->terrainVertSize-2) != 0) + if ( nf - ni != 0) { gzthrow("Heightmap image size must be (2^n)+1\n"); } // Calculate a good tile size - tileSize = (this->terrainVertSize-1)/8; + tileSize = pow( 2, ni/2 ); + if (tileSize <= 2) { tileSize = 4; @@ -168,13 +172,18 @@ this->terrainScale = this->terrainSize / this->terrainVertSize; - this->odeVertSize = terrainVertSize * 1; + this->odeVertSize = this->terrainVertSize * 4; this->odeScale = this->terrainSize / this->odeVertSize; std::ostringstream stream; - + + /*std::cout << "Terrain Scale[" << this->terrainScale << "]\n"; + std::cout << "ODE Scale[" << this->odeScale << "]\n"; std::cout << "Terrain Image[" << this->terrainImage << "] Size[" << this->terrainSize << "]\n"; + printf("Terrain Size[%f %f %f]\n", this->terrainSize.x, this->terrainSize.y, this->terrainSize.z); + printf("VertSize[%d] Tile Size[%d]\n", this->terrainVertSize, tileSize); + */ stream << "WorldTexture=" << worldTexture << "\n"; //The detail texture @@ -249,12 +258,16 @@ this->SetGeom(this->geomId, false); + this->SetStatic(true); + //Rotate so Z is up, not Y (which is the default orientation) - dMatrix3 R; - dRSetIdentity(R); - dRFromAxisAndAngle(R, 1, 0, 0, DTOR(90)); + Quatern quat; + Pose3d pose = this->GetPose(); - dGeomSetRotation(this->geomId, R); + quat.SetFromEuler(Vector3(DTOR(90),0,0)); + pose.rot = pose.rot * quat; + this->body->SetPose(pose); + delete [] mstr; } Modified: code/gazebo/trunk/worlds/terrain.world =================================================================== --- code/gazebo/trunk/worlds/terrain.world 2008-03-31 20:32:03 UTC (rev 4465) +++ code/gazebo/trunk/worlds/terrain.world 2008-04-01 22:22:40 UTC (rev 4466) @@ -81,8 +81,8 @@ </model:physical> <model:physical name="cam2_model"> - <xyz>210 -75 1.39</xyz> - <rpy>0 2.5 -87</rpy> + <xyz>211.42 -78.48 2.57</xyz> + <rpy>0 13 0</rpy> <static>true</static> <body:empty name="cam2_body"> @@ -99,8 +99,8 @@ </model:physical> <model:physical name="pioneer2dx_model2"> - <xyz>210 -78.18 1.15</xyz> - <rpy>0 0 -130</rpy> + <xyz>214.76 -79.34 1.29</xyz> + <rpy>0 0 20</rpy> <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> <rightJoint>right_wheel_hinge</rightJoint> Modified: code/gazebo/trunk/worlds/test.world =================================================================== --- code/gazebo/trunk/worlds/test.world 2008-03-31 20:32:03 UTC (rev 4465) +++ code/gazebo/trunk/worlds/test.world 2008-04-01 22:22:40 UTC (rev 4466) @@ -5,24 +5,22 @@ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" - xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" - xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" - xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > <verbosity>5</verbosity> <physics:ode> - <stepTime>0.03</stepTime> - <gravity>0 0 -9.8</gravity> + <stepTime>0.02</stepTime> + <gravity>0 0 -9.80665</gravity> <cfm>10e-5</cfm> - <erp>0.8</erp> + <erp>0.3</erp> </physics:ode> <rendering:gui> @@ -32,16 +30,43 @@ </rendering:gui> <rendering:ogre> - <ambient>0.5 0.5 0.5 1.0</ambient> + <ambient>1.0 1.0 1.0 1.0</ambient> <sky> <material>Gazebo/CloudySky</material> </sky> + </rendering:ogre> - <!-- Ground Plane --> + <model:physical name="sphere1_model"> + <xyz>0 0 9.0</xyz> + <static>false</static> + <body:sphere name="sphere1_body"> + <geom:sphere name="sphere1_geom"> + <size>0.2</size> + <mass>1.0</mass> + + <visual> + <scale>0.2 0.2 0.2</scale> + <material>Gazebo/Rocky</material> + <mesh>unit_sphere</mesh> + </visual> + </geom:sphere> + </body:sphere> + </model:physical> + + <model:physical name="terrain_model"> + <body:heightmap name ="terrain_body"> + <geom:heightmap name="terrain_geom"> + <image>test.jpg</image> + <worldTexture>test.jpg</worldTexture> + <detailTexture>test.jpg</detailTexture> + <size>17 17 1.0</size> + </geom:heightmap> + </body:heightmap> + </model:physical> + <model:physical name="plane1_model"> <xyz>0 0 0</xyz> - <rpy>0 0 0</rpy> <static>true</static> <body:plane name="plane1_body"> @@ -55,66 +80,138 @@ </body:plane> </model:physical> - <!-- The camera --> - <model:physical name="cam1_model"> - <xyz>0.07 -1.10 0.5</xyz> - <rpy>0 12 52.5</rpy> + <model:physical name="cam2_model"> + <xyz>-1.62 10.92 0.77</xyz> + <rpy>0 16 -79</rpy> <static>true</static> - <body:empty name="cam1_body"> - <sensor:camera name="cam1_sensor"> - <imageSize>640 480</imageSize> + <body:empty name="cam2_body"> + <sensor:camera name="cam2_sensor"> + <imageSize>800 600</imageSize> <hfov>60</hfov> <nearClip>0.1</nearClip> <farClip>100</farClip> - <!--<saveFrames>true</saveFrames> + <saveFrames>false</saveFrames> <saveFramePath>frames</saveFramePath> - --> </sensor:camera> </body:empty> </model:physical> - <model:physical name="cylinder1_model"> - <xyz>1 0 0.8</xyz> + <model:physical name="pioneer2dx_model1"> + <xyz>-12 0 0.145</xyz> <rpy>0.0 0.0 0.0</rpy> - <body:box name="cylinder1_body"> - <geom:box name="cylinder1_geom"> - <size>0.8 0.8 0.8</size> - <mass>0.1</mass> + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.34</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_0"/> + </controller:differential_position2d> - <mu1>0.1</mu1> + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> - <visual> - <mesh>unit_box</mesh> - <size>0.8 0.8 0.8</size> - <material>Gazebo/RustyBarrel</material> - </visual> - </geom:box> - </body:box> + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <!-- + The include should be last within a model. All previous statements + will override those in the included file + --> + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> </model:physical> + <model:physical name="pioneer2dx_model2"> + <xyz>12 0 0.145</xyz> + <rpy>0.0 0.0 180.0</rpy> - <!-- - Include the complete model described in the .model file - This assumes the root node is a <model:...> - --> - <!-- <include embedded="false"> - <xi:include href="pioneer2dx.model" /> - </include> - --> + <model:physical name="laser2"> + <xyz>0.15 0 0.18</xyz> - <!-- White Directional light --> - <model:renderable name="directional_white"> - <light> - <type>directional</type> - <direction>0 -0.6 -0.4</direction> - <diffuseColor>1.0 1.0 1.0</diffuseColor> - <specularColor>0.2 0.2 0.2</specularColor> - <attenuation>1000 1.0 0.0 0</attenuation> - </light> - </model:renderable> + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + <!-- + The include should be last within a model. All previous statements + will override those in the included file + --> + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + + <model:physical name="pioneer2dx_model3"> + <xyz>0 5 0.145</xyz> + <rpy>0.0 0.0 -90.0</rpy> + + <model:physical name="laser3"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <!-- + The include should be last within a model. All previous statements + will override those in the included file + --> + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + <model:physical name="pioneer2dx_model4"> + <xyz>0 -5 0.145</xyz> + <rpy>0.0 0.0 90.0</rpy> + + <model:physical name="laser4"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <!-- + The include should be last within a model. All previous statements + will override those in the included file + --> + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + + </gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ro...@us...> - 2008-04-02 05:07:28
|
Revision: 4469 http://playerstage.svn.sourceforge.net/playerstage/?rev=4469&view=rev Author: robotos Date: 2008-04-02 05:07:33 -0700 (Wed, 02 Apr 2008) Log Message: ----------- Two changes: Controller plugins: Patch 1909966 by David Olsen Simplify Gazebo error (gzthrow) usage Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/server/GazeboError.hh code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/XMLConfig.cc code/gazebo/trunk/server/controllers/ControllerFactory.cc code/gazebo/trunk/server/controllers/ControllerFactory.hh code/gazebo/trunk/server/controllers/SConscript code/gazebo/trunk/server/gui/SConscript code/gazebo/trunk/server/physics/SConscript code/gazebo/trunk/server/rendering/SConscript code/gazebo/trunk/server/sensors/SConscript code/gazebo/trunk/server/sensors/Sensor.cc Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/SConstruct 2008-04-02 12:07:33 UTC (rev 4469) @@ -56,6 +56,7 @@ #LIBS=Split('gazebo boost_python') LIBS=Split('gazebo'), + LINKFLAGS=Split('-export-dynamic'), TARFLAGS = '-c -z', TARSUFFIX = '.tar.gz', @@ -134,6 +135,13 @@ if not conf.CheckCHeader('ode/ode.h'): print " Error: Install ODE (http://www.ode.org)" Exit(1) + + if not conf.CheckLibWithHeader('ltdl','ltdl.h','CXX'): + print " Warning: Failed to find ltdl, no plugin support will be included" + env["HAVE_LTDL"]=False + else: + env["HAVE_LTDL"]=True + env["CCFLAGS"].append("-DHAVE_LTDL") # Check for trimesh support in ODE if not conf.CheckODELib(): Modified: code/gazebo/trunk/server/GazeboError.hh =================================================================== --- code/gazebo/trunk/server/GazeboError.hh 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/GazeboError.hh 2008-04-02 12:07:33 UTC (rev 4469) @@ -34,27 +34,32 @@ namespace gazebo { - /// \addtogroup gazebo_server /// \brief Gazebo error class /// \{ + //TODO: global variable, static in the class would be better, if only the linker didn't oppose to it ... + static std::ostringstream throwStream; /// Throw an error - #define gzthrow(msg) throw gazebo::GazeboError(__FILE__,__LINE__,std::string(msg)) + #define gzthrow(msg) throwStream << "Exception: " << msg << std::endl << std::flush;\ + throw gazebo::GazeboError(__FILE__,__LINE__,throwStream.str()) + /// \brief Class to handle errors /// /** - Use <tt>gzthrow(std::string)</tt> to throw errors. + Use <tt>gzthrow(data1 << data2)</tt> to throw errors. Example: \verbatim + Recommended new way: + gzthrow("This is an error message of type[" << type << "]"); + Old way: std::ostringstream stream; stream << "This is an error message of type[" << type << "]\n"; gzthrow(stream.str()); - or if type is a string, simply: - gzthrow("This is an error message of type[" + type + "]\n"); + The final "\n" is not needed anymore, the code should be changed to the new type. \endverbatim */ @@ -86,7 +91,7 @@ /// \brief Return the error string /// \return The error string public: std::string GetErrorStr() const; - + /// \brief The error function private: std::string file; Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/Model.cc 2008-04-02 12:07:33 UTC (rev 4469) @@ -544,6 +544,11 @@ // Get the unique name of the controller std::string controllerName = node->GetString("name",std::string(),1); + + // See if the controller is in a plugin + std::string pluginName = node->GetString("plugin","",0); + if (pluginName != "") + ControllerFactory::LoadPlugin(pluginName, controllerType); // Create the controller based on it's type controller = ControllerFactory::NewController(controllerType, this); Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/SConscript 2008-04-02 12:07:33 UTC (rev 4469) @@ -24,20 +24,23 @@ ] headers.append( - ['#/server/Global.hh', - '#/server/Vector3.hh', + ['#/server/Entity.hh', + '#/server/GazeboError.hh', + '#/server/GazeboMessage.hh', + '#/server/Global.hh', + '#/server/Model.hh', + '#/server/Pose3d.hh', '#/server/Quatern.hh', - '#/server/Pose3d.hh', - '#/server/World.hh', - '#/server/XMLConfig.hh', + '#/server/Simulator.hh', + '#/server/SingletonT.hh', + '#/server/StaticPluginRegister.hh', + '#/server/StringValue.hh', '#/server/Time.hh', - '#/server/Entity.hh', - '#/server/GazeboError.hh', '#/server/UpdateParams.hh', - '#/server/GazeboMessage.hh', - '#/server/Model.hh', - '#/server/Simulator.hh', - '#/server/StringValue.hh' + '#/server/Vector2.hh', + '#/server/Vector3.hh', + '#/server/World.hh', + '#/server/XMLConfig.hh' ] ) staticObjs.append( env.StaticObject(sources) ) Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/Simulator.cc 2008-04-02 12:07:33 UTC (rev 4469) @@ -101,17 +101,13 @@ // Load the world file this->xmlFile=new gazebo::XMLConfig(); - try { xmlFile->Load(worldFileName); } catch (GazeboError e) { - std::ostringstream stream; - stream << "The XML config file can not be loaded, please make sure is a correct file\n" - << e << "\n"; - gzthrow(stream.str()); + gzthrow("The XML config file can not be loaded, please make sure is a correct file\n" << e); } XMLConfigNode *rootNode(xmlFile->GetRootNode()); @@ -126,10 +122,7 @@ } catch (GazeboError e) { - std::ostringstream stream; - stream << "Error loading the GUI\n" - << e << "\n"; - gzthrow(stream.str()); + gzthrow( "Error loading the GUI\n" << e); } //Initialize RenderingEngine @@ -139,11 +132,8 @@ } catch (gazebo::GazeboError e) { - std::ostringstream stream; - stream << "Failed to Initialize the OGRE Rendering system\n" - << e << "\n"; - gzthrow(stream.str()); - } + gzthrow("Failed to Initialize the OGRE Rendering system\n" << e ); + } //Preload basic shapes that can be used anywhere OgreCreator::CreateBasicShapes(); @@ -155,10 +145,7 @@ } catch (GazeboError e) { - std::ostringstream stream; - stream << "Error loading the GUI\n" - << e << "\n"; - gzthrow(stream.str()); + gzthrow("Failed to load the GUI\n" << e); } this->loaded=true; @@ -178,10 +165,8 @@ if (xmlFile->Save(filename)<0) { - std::ostringstream stream; - stream << "The XML file coult not be written back to " << filename << std::endl; - gzthrow(stream.str()); - } + gzthrow("The XML file could not be written back to " << filename ); + } } @@ -376,7 +361,7 @@ int x = childNode->GetTupleInt("pos",0,0); int y = childNode->GetTupleInt("pos",1,0); std::string type = childNode->GetString("type","fltk",1); - + gzmsg(1) << "Creating GUI:\n\tType[" << type << "] Pos[" << x << " " << y << "] Size[" << width << " " << height << "]\n"; if (type != "fltk") { @@ -393,7 +378,7 @@ else { // Create a dummy GUI - gzmsg(1) << "Creating a dummy GUI\n"; + gzmsg(1) <<"Creating a dummy GUI"; this->gui = GuiFactory::NewGui(std::string("dummy"), 0, 0, 0, 0, std::string()); } } Modified: code/gazebo/trunk/server/XMLConfig.cc =================================================================== --- code/gazebo/trunk/server/XMLConfig.cc 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/XMLConfig.cc 2008-04-02 12:07:33 UTC (rev 4469) @@ -65,7 +65,6 @@ // Load world from file void XMLConfig::Load( const std::string &filename ) { - std::ostringstream stream; this->filename = filename; // Enable line numbering @@ -75,8 +74,7 @@ this->xmlDoc = xmlParseFile( this->filename.c_str() ); if (xmlDoc == NULL) { - stream << "Unable to parse xml file: " << this->filename; - gzthrow(stream.str()); + gzthrow( "Unable to parse xml file: " << this->filename); } // Create xpath evaluation context @@ -97,8 +95,7 @@ this->root = this->CreateNodes( NULL, xmlDocGetRootElement(this->xmlDoc) ); if (this->root == NULL) { - stream << "Empty document [" << this->filename << "]"; - gzthrow(stream.str()); + gzthrow( "Empty document [" << this->filename << "]"); } } @@ -111,9 +108,7 @@ this->xmlDoc = xmlParseDoc( (xmlChar*)(str.c_str()) ); if (xmlDoc == NULL) { - std::ostringstream stream; - stream << "unable to parse [" << str << "]"; - gzthrow(stream.str()); + gzthrow("unable to parse [" << str << "]"); } // Create wrappers for all the nodes (recursive) @@ -122,9 +117,7 @@ if (this->root == NULL) { - std::ostringstream stream; - stream << "Empty document [" << str << "\n"; - gzthrow(stream.str()); + gzthrow( "Empty document [" << str<< "]") ; } } @@ -409,8 +402,7 @@ { XMLConfigNode *node; - std::cout << "name = [" << (const char*) this->xmlNode->name - << "]\n"; + gzmsg(2) << "name = [" << (const char*) this->xmlNode->name << "]\n"; // Recurse for (node = this->childFirst; node != NULL; node = node->next) @@ -482,9 +474,7 @@ if (!value && require) { - std::ostringstream stream; - stream << "unable to find required string attribute[" << key << "] in world file node[" << this->GetName() << "]"; - gzthrow(stream.str()); + gzthrow( "unable to find required string attribute[" << key << "] in world file node[" << this->GetName() << "]"); } else if ( !value ) return def; @@ -501,9 +491,7 @@ if (!value && require) { - std::ostringstream stream; - stream << "unable to find required char attribute[" << key << "] in world file node[" << this->GetName() << "]"; - gzthrow(stream.str()); + gzthrow("unable to find required char attribute[" << key << "] in world file node[" << this->GetName() << "]"); } else if ( !value ) return def; @@ -553,9 +541,7 @@ if (!value && require) { - std::ostringstream stream; - stream << "unable to find required int attribute[" << key << "] in world file node[" << this->GetName() << "]"; - gzthrow(stream.str()); + gzthrow ("unable to find required int attribute[" << key << "] in world file node[" << this->GetName() << "]"); } else if ( !value ) return def; @@ -572,9 +558,7 @@ if (!value && require) { - std::ostringstream stream; - stream << "unable to find required double attribute[" << key << "] in world file node[" << this->GetName() << "]"; - gzthrow(stream.str()); + gzthrow( "unable to find required double attribute[" << key << "] in world file node[" << this->GetName() << "]"); } else if ( !value ) return def; @@ -590,9 +574,7 @@ if (!value && require) { - std::ostringstream stream; - stream << "unable to find required float attribute[" << key << "] in world file node[" << this->GetName() << "]"; - gzthrow(stream.str()); + gzthrow( "unable to find required float attribute[" << key << "] in world file node[" << this->GetName() << "]"); } else if ( !value ) return def; @@ -611,9 +593,7 @@ if (!value && require) { xmlFree(value); - std::ostringstream stream; - stream << "unable to find required bool attribute[" << key << "] in world file node[" << this->GetName() << "]"; - gzthrow(stream.str()); + gzthrow( "unable to find required bool attribute[" << key << "] in world file node[" << this->GetName() << "]"); } else if ( !value ) { @@ -925,9 +905,7 @@ newNode = xmlNewNode(0, (xmlChar*) key); //I hope we don't need namespaces here if (!newNode) { - std::ostringstream stream; - stream << "unable to create an element [" << key << "] in world file node[" << this->GetName() << "]"; - gzthrow(stream.str()); + gzthrow( "unable to create an element [" << key << "] in world file node[" << this->GetName() << "]"); } xmlNodeSetContent(newNode, (xmlChar*) value); xmlAddChild(this->xmlNode, newNode); Modified: code/gazebo/trunk/server/controllers/ControllerFactory.cc =================================================================== --- code/gazebo/trunk/server/controllers/ControllerFactory.cc 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/controllers/ControllerFactory.cc 2008-04-02 12:07:33 UTC (rev 4469) @@ -30,6 +30,9 @@ #include "gazebo.h" #include "Controller.hh" #include "ControllerFactory.hh" +#ifdef HAVE_LTDL +#include <ltdl.h> +#endif // HAVE_LTDL using namespace gazebo; @@ -60,3 +63,52 @@ return NULL; } + +//////////////////////////////////////////////////////////////////////////////// +// Load a controller plugin. Used by Model and Sensor when creating controllers. +void ControllerFactory::LoadPlugin(const std::string &plugin, const std::string &classname) +{ +#ifdef HAVE_LTDL + + static bool init_done = false; + + if (!init_done) + { + int errors = lt_dlinit(); + if (errors) + { + std::ostringstream stream; + stream << "Error(s) initializing dynamic loader (" << errors << ", " << lt_dlerror() << ")"; + gzthrow(stream.str()); + } + else + init_done = true; + } + + lt_dlhandle handle = lt_dlopenext(plugin.c_str()); + + if (!handle) + { + std::ostringstream stream; + stream << "Failed to load " << plugin << ": " << lt_dlerror(); + gzthrow(stream.str()); + } + + std::string registerName = "RegisterPluginController"; + void *(*registerFunc)() = (void *(*)())lt_dlsym(handle, registerName.c_str()); + if(!registerFunc) + { + std::ostringstream stream; + stream << "Failed to resolve " << registerName << ": " << lt_dlerror(); + gzthrow(stream.str()); + } + + // Register the new controller. + registerFunc(); + +#else // HAVE_LTDL + + gzthrow("Cannot load plugins as libtool is not installed."); + +#endif // HAVE_LTDL +} Modified: code/gazebo/trunk/server/controllers/ControllerFactory.hh =================================================================== --- code/gazebo/trunk/server/controllers/ControllerFactory.hh 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/controllers/ControllerFactory.hh 2008-04-02 12:07:33 UTC (rev 4469) @@ -58,6 +58,9 @@ /// \brief Create a new instance of a controller. Used by the world when /// reading the world file. public: static Controller *NewController(const std::string &classname, Entity *parent); + + /// \brief Load a controller plugin. Used by Model and Sensor when creating controllers. + public: static void LoadPlugin(const std::string &plugin, const std::string &classname); // A list of registered controller classes private: static std::map<std::string, ControllerFactoryFn> controllers; @@ -81,6 +84,22 @@ }\ StaticPluginRegister Registered##classname (Register##classname); +/// \brief Dynamic controller registration macro +/// +/// Use this macro to register plugin controllers with the server. +/// \param name Controller type name, as it appears in the world file. +/// \param classname C++ class name for the controller. +#define GZ_REGISTER_DYNAMIC_CONTROLLER(name, classname) \ +Controller *New##classname(Entity *entity) \ +{ \ + return new classname(entity); \ +} \ +extern "C" void RegisterPluginController(); \ +void RegisterPluginController() \ +{\ + ControllerFactory::RegisterController("dynamic", name, New##classname);\ +} + /// \} } Modified: code/gazebo/trunk/server/controllers/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/SConscript 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/controllers/SConscript 2008-04-02 12:07:33 UTC (rev 4469) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env staticObjs sharedObjs headers') dirs = Split('position2d laser camera factory gripper actarray ptz') @@ -11,5 +11,11 @@ sources = Split('Controller.cc ControllerFactory.cc') +headers.append( + ['#/server/controllers/ControllerFactory.hh', + '#/server/controllers/ControllerStub.hh', + '#/server/controllers/Controller.hh' + ] ) + staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/gui/SConscript =================================================================== --- code/gazebo/trunk/server/gui/SConscript 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/gui/SConscript 2008-04-02 12:07:33 UTC (rev 4469) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env staticObjs sharedObjs headers') #sources = Split('Gui.cc GuiFactory.cc GLWindow.cc MainMenu.cc Toolbar.cc StatusBar.cc') @@ -8,7 +8,16 @@ #if conf.CheckCHeader('GL/glx.h'): sources = Split('Gui.cc DummyGui.cc GuiFactory.cc GLWindow.cc MainMenu.cc Toolbar.cc StatusBar.cc') #env = conf.Finish() - +headers.append( + ['server/gui/DummyGui.hh', + 'server/gui/GLWindow.hh', + 'server/gui/GuiFactory.hh', + 'server/gui/Gui.hh', + 'server/gui/MainMenu.hh', + 'server/gui/StatusBar.hh', + 'server/gui/Toolbar.hh' + ] ) + staticObjs.append(env.StaticObject(sources)) sharedObjs.append(env.SharedObject(sources)) Modified: code/gazebo/trunk/server/physics/SConscript =================================================================== --- code/gazebo/trunk/server/physics/SConscript 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/physics/SConscript 2008-04-02 12:07:33 UTC (rev 4469) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env staticObjs sharedObjs headers') dirs = Split('ode') @@ -8,5 +8,25 @@ sources = Split('BallJoint.cc Body.cc BoxGeom.cc ContactParams.cc CylinderGeom.cc Geom.cc Hinge2Joint.cc HingeJoint.cc Joint.cc PhysicsEngine.cc PlaneGeom.cc SliderJoint.cc SphereGeom.cc UniversalJoint.cc RayGeom.cc TrimeshGeom.cc HeightmapGeom.cc') +headers.append( + ['server/physics/BallJoint.hh', + 'server/physics/Body.hh', + 'server/physics/BoxGeom.hh', + 'server/physics/ContactParams.hh', + 'server/physics/CylinderGeom.hh', + 'server/physics/Geom.hh', + 'server/physics/HeightmapGeom.hh', + 'server/physics/Hinge2Joint.hh', + 'server/physics/HingeJoint.hh', + 'server/physics/Joint.hh', + 'server/physics/PhysicsEngine.hh', + 'server/physics/PlaneGeom.hh', + 'server/physics/RayGeom.hh', + 'server/physics/SliderJoint.hh', + 'server/physics/SphereGeom.hh', + 'server/physics/TrimeshGeom.hh', + 'server/physics/UniversalJoint.hh' + ] ) + staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/rendering/SConscript =================================================================== --- code/gazebo/trunk/server/rendering/SConscript 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/rendering/SConscript 2008-04-02 12:07:33 UTC (rev 4469) @@ -1,7 +1,19 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env staticObjs sharedObjs headers') sources = Split('OgreCreator.cc OgreAdaptor.cc OgreFrameListener.cc OgreDynamicRenderable.cc OgreDynamicLines.cc OgreSimpleShape.cc OgreHUD.cc MovableText.cc OgreVisual.cc') +headers.append( + ['#/server/rendering/MovableText.hh', + '#/server/rendering/OgreAdaptor.hh', + '#/server/rendering/OgreCreator.hh', + '#/server/rendering/OgreDynamicLines.hh', + '#/server/rendering/OgreDynamicRenderable.hh', + '#/server/rendering/OgreFrameListener.hh', + '#/server/rendering/OgreHUD.hh', + '#/server/rendering/OgreSimpleShape.hh', + '#/server/rendering/OgreVisual.hh' + ] ) + staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/sensors/SConscript =================================================================== --- code/gazebo/trunk/server/sensors/SConscript 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/sensors/SConscript 2008-04-02 12:07:33 UTC (rev 4469) @@ -1,5 +1,5 @@ #Import variable -Import('env staticObjs sharedObjs') +Import('env staticObjs sharedObjs headers') dirs = Split('camera ray') @@ -8,5 +8,11 @@ sources = Split('Sensor.cc SensorFactory.cc') +headers.append( + ['server/sensors/SensorFactory.hh', + 'server/sensors/SensorStub.hh', + 'server/sensors/Sensor.hh' + ] ) + staticObjs.append( env.StaticObject(sources) ) sharedObjs.append( env.SharedObject(sources) ) Modified: code/gazebo/trunk/server/sensors/Sensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/Sensor.cc 2008-04-01 22:53:29 UTC (rev 4468) +++ code/gazebo/trunk/server/sensors/Sensor.cc 2008-04-02 12:07:33 UTC (rev 4469) @@ -127,6 +127,11 @@ stream << "No interface defined for " << controllerName << "controller"; gzthrow(stream.str()); }*/ + + // See if the controller is in a plugin + std::string pluginName = node->GetString("plugin","",0); + if (pluginName != "") + ControllerFactory::LoadPlugin(pluginName, controllerType); // Create the controller based on it's type this->controller = ControllerFactory::NewController(controllerType, this); This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-04-07 11:04:19
|
Revision: 6279 http://playerstage.svn.sourceforge.net/playerstage/?rev=6279&view=rev Author: natepak Date: 2008-04-07 11:04:20 -0700 (Mon, 07 Apr 2008) Log Message: ----------- Updates to stereo camera Modified Paths: -------------- code/gazebo/trunk/TODO code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/GazeboError.hh code/gazebo/trunk/server/controllers/camera/SConscript code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/worlds/stereocamera.world Added Paths: ----------- code/gazebo/trunk/server/controllers/camera/stereo/ code/gazebo/trunk/server/controllers/camera/stereo/SConscript code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh Modified: code/gazebo/trunk/TODO =================================================================== --- code/gazebo/trunk/TODO 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/TODO 2008-04-07 18:04:20 UTC (rev 6279) @@ -1,14 +1,11 @@ Open: - -- Use the Contact information in the Collision Callback. See CVS version of Gazebo. +- Removing the sky results in a black screen - Apply Linear and Angular Damping, see OGREODE sources. - A static geom which is offset does not actually move the geom, so collision detection does not work - Force sensors - Why order on hinge and hinge2 joints matters? Should we avoid this? - Add spring force between bodies of a model to keep the together. See ODE example test_joints.cpp - -- Try out code on AMD64 machine - Python scripts to models, allow dynamic (movable) meshes - Slider has to be the first in the XML file, when loading a bunch of joints in a model. - Add note in Controller tutorial about the Controller Factory @@ -120,3 +117,4 @@ - Add in Pioneer blender models - Add check for libxml2 - Add uninstall (scons -c automatically handles this!) +- Use the Contact information in the Collision Callback. See CVS version of Gazebo. Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-04-07 18:04:20 UTC (rev 6279) @@ -56,6 +56,7 @@ GZ_REGISTER_IFACE("gripper", GripperIface); GZ_REGISTER_IFACE("actarray", ActarrayIface); GZ_REGISTER_IFACE("ptz", PTZIface); +GZ_REGISTER_IFACE("stereocamera", StereoCameraIface); ////////////////////////////////////////////////////////////////////////////// // Create an interface Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-04-07 18:04:20 UTC (rev 6279) @@ -1313,7 +1313,88 @@ /// \} +/***************************************************************************/ +/// \addtogroup libgazebo_iface +/// \{ +/** \defgroup stereo_iface Stereo + \brief Stereo vision interface + +The stereo interface allows a client to read data from a stereo camera unit +\{ +*/ + +#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 3 +#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480 + +/// \brief Stereo data +class StereoCameraData +{ + /// Data timestamp + public: double time; + + /// Width of image in pixels + public: unsigned int width; + + /// Height of image in pixels + public: unsigned int height; + + /// Right image size + public: unsigned int right_rgb_size; + + /// Right image (R8G8B8) + public: unsigned char right_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; + + /// Left image size + public: unsigned int left_rgb_size; + + /// left image (R8G8B8) + public: unsigned char left_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; + + /// Left disparity size + public: unsigned int left_disparity_size; + + /// Left disparity (float) + public: float left_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; + + /// Right Disparity size + public: unsigned int right_disparity_size; + + /// Right disparity (float) + public: float right_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; +}; + + +/// \brief Stereo interface +class StereoCameraIface : public Iface +{ + /// \brief Constructor + public: StereoCameraIface():Iface("stereo", sizeof(StereoCameraIface)+sizeof(StereoCameraData)) {} + + /// \brief Destructor + public: virtual ~StereoCameraIface() {this->data = NULL;} + + /// \brief Create the server + public: virtual void Create(Server *server, std::string id) + { + Iface::Create(server,id); + this->data = (StereoCameraData*)this->mMap; + } + + /// \brief Open the iface + public: virtual void Open(Client *client, std::string id) + { + Iface::Open(client,id); + this->data = (StereoCameraData*)this->mMap; + } + + /// Pointer to the stereo data + public: StereoCameraData *data; +}; + +/** \} */ +/// \} + } Modified: code/gazebo/trunk/server/GazeboError.hh =================================================================== --- code/gazebo/trunk/server/GazeboError.hh 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/server/GazeboError.hh 2008-04-07 18:04:20 UTC (rev 6279) @@ -41,8 +41,7 @@ //TODO: global variable, static in the class would be better, if only the linker didn't oppose to it ... static std::ostringstream throwStream; /// Throw an error - #define gzthrow(msg) throwStream << "Exception: " << msg << std::endl << std::flush;\ - throw gazebo::GazeboError(__FILE__,__LINE__,throwStream.str()) + #define gzthrow(msg) {throwStream << "Exception: " << msg << std::endl << std::flush; throw gazebo::GazeboError(__FILE__,__LINE__,throwStream.str()); } /// \brief Class to handle errors Modified: code/gazebo/trunk/server/controllers/camera/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/camera/SConscript 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/server/controllers/camera/SConscript 2008-04-07 18:04:20 UTC (rev 6279) @@ -1,7 +1,7 @@ #Import variable Import('env staticObjs sharedObjs') -dirs = Split('generic') +dirs = Split('generic stereo') for subdir in dirs : SConscript('%s/SConscript' % subdir) Added: code/gazebo/trunk/server/controllers/camera/stereo/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/SConscript (rev 0) +++ code/gazebo/trunk/server/controllers/camera/stereo/SConscript 2008-04-07 18:04:20 UTC (rev 6279) @@ -0,0 +1,7 @@ +#Import variable +Import('env staticObjs sharedObjs') + +sources = Split('Stereo_Camera.cc') + +staticObjs.append(env.StaticObject(sources)) +sharedObjs.append(env.SharedObject(sources)) Added: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc (rev 0) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-04-07 18:04:20 UTC (rev 6279) @@ -0,0 +1,149 @@ +/* + * 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 controller. + * Author: Nathan Koenig + * Date: 06 April 2008 + * SVN info: $Id: Stereo_Camera.cc 4436 2008-03-24 17:42:45Z robotos $ + */ + +#include <algorithm> +#include <assert.h> + +#include "Sensor.hh" +#include "Global.hh" +#include "XMLConfig.hh" +#include "Simulator.hh" +#include "gazebo.h" +#include "GazeboError.hh" +#include "ControllerFactory.hh" +#include "StereoCameraSensor.hh" +#include "Stereo_Camera.hh" + +using namespace gazebo; + +GZ_REGISTER_STATIC_CONTROLLER("stereo_camera", Stereo_Camera); + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +Stereo_Camera::Stereo_Camera(Entity *parent) + : Controller(parent) +{ + this->myParent = dynamic_cast<StereoCameraSensor*>(this->parent); + + if (!this->myParent) + gzthrow("Stereo_Camera controller requires a Stereo Camera Sensor as its parent"); +} + +//////////////////////////////////////////////////////////////////////////////// +// Destructor +Stereo_Camera::~Stereo_Camera() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void Stereo_Camera::LoadChild(XMLConfigNode *node) +{ + this->cameraIface = dynamic_cast<StereoCameraIface*>(this->ifaces[0]); + + if (!this->cameraIface) + gzthrow("Stereo_Camera controller requires a StereoCameraIface"); +} + +//////////////////////////////////////////////////////////////////////////////// +// Initialize the controller +void Stereo_Camera::InitChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Update the controller +void Stereo_Camera::UpdateChild(UpdateParams ¶ms) +{ + this->PutCameraData(); +} + +//////////////////////////////////////////////////////////////////////////////// +// Finalize the controller +void Stereo_Camera::FiniChild() +{ +} + +//////////////////////////////////////////////////////////////////////////////// +// Put laser data to the interface +void Stereo_Camera::PutCameraData() +{ + StereoCameraData *data = this->cameraIface->data; + const unsigned char *rgb_src; + unsigned char *rgb_dst; + + const float *disp_src; + float *disp_dst; + + int i, j, k; + + this->cameraIface->Lock(1); + + // Data timestamp + data->time = Simulator::Instance()->GetSimTime(); + + data->width = this->myParent->GetImageWidth(); + data->height = this->myParent->GetImageHeight(); + + data->right_rgb_size = data->width * data->height * 3; + data->left_rgb_size = data->width * data->height * 3; + + data->right_disparity_size = data->width * data->height; + data->left_disparity_size = data->width * data->height; + + // Make sure there is room to store the image + assert (data->right_rgb_size <= sizeof(data->right_rgb)); + assert (data->left_rgb_size <= sizeof(data->left_rgb)); + + assert (data->right_disparity_size <= sizeof(data->right_disparity)); + assert (data->left_disparity_size <= sizeof(data->left_disparity)); + + // Copy the left pixel data to the interface + rgb_src = this->myParent->GetImageData(0); + rgb_dst = data->left_rgb; + memcpy(rgb_dst, rgb_src, data->left_rgb_size); + + // Copy the right pixel data to the interface + rgb_src = this->myParent->GetImageData(1); + rgb_dst = data->right_rgb; + memcpy(rgb_dst, rgb_src, data->right_rgb_size); + + // Copy the left disparity data to the interface + disp_src = this->myParent->GetDisparityData(0); + disp_dst = data->left_disparity; + memcpy(disp_dst, disp_src, data->left_disparity_size); + + // Copy the right disparity data to the interface + disp_src = this->myParent->GetDisparityData(0); + disp_dst = data->right_disparity; + memcpy(disp_dst, disp_src, data->right_disparity_size); + + this->cameraIface->Unlock(); + + // New data is available + this->cameraIface->Post(); +} Added: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh (rev 0) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-04-07 18:04:20 UTC (rev 6279) @@ -0,0 +1,109 @@ +/* + * 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 stereo camera controller + * Author: Nathan Koenig + * Date: 06 April 2008 + * SVN: $Id:$ + */ + +#ifndef STEREO_CAMERA_HH +#define STEREO_CAMERA_HH + +#include "Controller.hh" + +namespace gazebo +{ + class CameraIface; + class CameraSensor; + +/// @addtogroup gazebo_controller +/// @{ +/** \defgroup stereocamera stereo camera + + \brief Stereo camera controller. + + This is a controller that collects data from a Stereo Camera Sensor and populates a libgazebo stereo camera interfaace. This controller should only be used as a child of a stereo camera sensor + + \verbatim + <model:physical name="camera_model"> + <body:empty name="camera_body"> + <sensor:stereocamera name="stereo_camera_sensor"> + + <controller:stereo_camera name="controller-name"> + <interface:stereocamera name="iface-name"/> + </controller:stereo_camera> + + </sensor:stereocamera> + </body:empty> + </model:phyiscal> + \endverbatim + +\{ +*/ + +/// \brief Stereo camera controller. +/// +/// This is a controller that simulates a stereo camera +class Stereo_Camera : public Controller +{ + /// \brief Constructor + /// \param parent The parent entity, must be a Model or a Sensor + public: Stereo_Camera(Entity *parent); + + /// \brief Destructor + public: virtual ~Stereo_Camera(); + + /// \brief Load the controller + /// \param node XML config node + /// \return 0 on success + protected: virtual void LoadChild(XMLConfigNode *node); + + /// \brief Init the controller + /// \return 0 on success + protected: virtual void InitChild(); + + /// \brief Update the controller + /// \return 0 on success + protected: virtual void UpdateChild(UpdateParams ¶ms); + + /// \brief Finalize the controller + /// \return 0 on success + protected: virtual void FiniChild(); + + /// \brief Put camera data to the iface + private: void PutCameraData(); + + /// The camera interface + private: StereoCameraIface *cameraIface; + + /// The parent sensor + private: StereoCameraSensor *myParent; + +}; + +/** /} */ +/// @} + +} + +#endif + Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-04-07 18:04:20 UTC (rev 6279) @@ -187,13 +187,15 @@ // Settings for shadow mapping std::string shadowTechnique = node->GetString("shadowTechnique", "stencilAdditive"); + if (shadowTechnique == std::string("stencilAdditive")) this->sceneMgr->setShadowTechnique( Ogre::SHADOWTYPE_STENCIL_ADDITIVE ); else if (shadowTechnique == std::string("textureAdditive")) this->sceneMgr->setShadowTechnique( Ogre::SHADOWTYPE_TEXTURE_ADDITIVE ); else if (shadowTechnique == std::string("none")) this->sceneMgr->setShadowTechnique( Ogre::SHADOWTYPE_NONE ); - else gzthrow(std::string("Unsupported shadow technique: ") + shadowTechnique + "\n"); + else + gzthrow(std::string("Unsupported shadow technique: ") + shadowTechnique + "\n"); this->sceneMgr->setShadowTextureSelfShadow(true); this->sceneMgr->setShadowTexturePixelFormat(Ogre::PF_FLOAT32_R); @@ -476,7 +478,7 @@ { OgreHUD::Instance()->Update(); - root->renderOneFrame(); + this->root->renderOneFrame(); return 0; } Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-04-07 18:04:20 UTC (rev 6279) @@ -180,7 +180,7 @@ camera->setDirection(1,0,0); camera->setNearClipDistance(nearClip); - camera->setFarClipDistance(farClip+1000); + camera->setFarClipDistance(farClip); // Setup the viewport to use the texture cviewport = renderTarget->addViewport(camera); Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-07 18:04:20 UTC (rev 6279) @@ -21,9 +21,9 @@ /* Desc: Stereo Camera Sensor * Author: Nate Koenig * Date: 25 March 2008 - * CVS: $Id:$ + * SVN: $Id:$ */ - +#include <arpa/inet.h> #include <sstream> #include <OgreImageCodec.h> #include <GL/gl.h> @@ -51,6 +51,10 @@ StereoCameraSensor::StereoCameraSensor(Body *body) : CameraSensor(body) { + this->depthBuffer[0] = NULL; + this->depthBuffer[1] = NULL; + this->rgbBuffer[0] = NULL; + this->rgbBuffer[1] = NULL; } @@ -58,6 +62,14 @@ // Destructor StereoCameraSensor::~StereoCameraSensor() { + for (int i=0; i<2; i++) + { + if (this->depthBuffer[i]) + delete [] this->depthBuffer[i]; + + if (this->rgbBuffer[i]) + delete [] this->rgbBuffer[i]; + } } ////////////////////////////////////////////////////////////////////////////// @@ -66,6 +78,8 @@ { CameraSensor::LoadChild(node); + std::cout << "Image Size[" << this->imageWidth << " " << this->imageHeight << "]\n"; + this->baseline = node->GetDouble("baseline",0,1); } @@ -73,149 +87,72 @@ // Initialize the camera void StereoCameraSensor::InitChild() { - this->ogreTextureName[0] = this->GetName() + "_RttTex_Stereo_Left"; - //this->ogreTextureName[1] = this->GetName() + "_RttTex_Stereo_Right"; + Ogre::Viewport *cviewport; + Ogre::MaterialPtr matPtr; + Ogre::TextureUnitState *texUnit; + Ogre::HardwarePixelBufferSharedPtr mBuffer; + int i; - this->ogreDepthTextureName[0] = this->GetName() +"_RttTex_Stereo_Left_Depth"; - //this->ogreDepthTextureName[1] = this->GetName() +"_RttTex_Stereo_Right_Depth"; + this->textureName[LEFT] = this->GetName() + "_RttTex_Stereo_Left"; + this->textureName[RIGHT] = this->GetName() + "_RttTex_Stereo_Right"; + this->textureName[D_LEFT] = this->GetName() +"_RttTex_Stereo_Left_Depth"; + this->textureName[D_RIGHT] = this->GetName() +"_RttTex_Stereo_Right_Depth"; - this->ogreMaterialName[0] = this->GetName() + "_RttMat_Stereo_Left"; - //this->ogreMaterialName[1] = this->GetName() + "_RttMat_Stereo_Right"; + this->materialName[LEFT] = this->GetName() + "_RttMat_Stereo_Left"; + this->materialName[RIGHT] = this->GetName() + "_RttMat_Stereo_Right"; + this->materialName[D_LEFT] = this->GetName()+"_RttMat_Stereo_Left_Depth"; + this->materialName[D_RIGHT] = this->GetName()+"_RttMat_Stereo_Right_Depth"; - this->ogreDepthMaterialName[0] = this->GetName()+"_RttMat_Stereo_Left_Depth"; - //this->ogreDepthMaterialName[1] = this->GetName()+"_RttMat_Stereo_Right_Depth"; + // Create the render textures for the color textures + for (i = 0; i<2; i++) + { + this->renderTexture[i] = this->CreateRTT(this->textureName[i], false); + this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); + } - // Create the left render texture - this->renderTexture[0] = Ogre::TextureManager::getSingleton().createManual( - this->ogreTextureName[0], - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, - this->imageWidth, this->imageHeight, 0, - Ogre::PF_R8G8B8, - //Ogre::PF_L8, - //Ogre::PF_FLOAT16_R, - Ogre::TU_RENDERTARGET); + // Create the render texture for the depth textures + for (i = 2; i<4; i++) + { + this->renderTexture[i] = this->CreateRTT(this->textureName[i], true); + this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); + } - this->renderTarget[0] = this->renderTexture[0]->getBuffer()->getRenderTarget(); -/* - // Create the right render texture - this->renderTexture[1] = Ogre::TextureManager::getSingleton().createManual( - this->ogreTextureName[1], - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, - this->imageWidth, this->imageHeight, 0, - Ogre::PF_R8G8B8, - Ogre::TU_RENDERTARGET); - - this->renderTarget[1] = this->renderTexture[1]->getBuffer()->getRenderTarget(); - */ - - // Create the left depth render texture - /*this->depthRenderTexture[0] = - Ogre::TextureManager::getSingleton().createManual( - this->ogreDepthTextureName[0], - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, - this->imageWidth, this->imageHeight, 0, - Ogre::PF_FLOAT32_R, - //Ogre::PF_DEPTH, - //Ogre::PF_R8G8B8, - Ogre::TU_RENDERTARGET); - - this->depthRenderTarget[0] = this->depthRenderTexture[0]->getBuffer()->getRenderTarget(); - */ - - // Create the right depth render texture - /*this->depthRenderTexture[1] = - Ogre::TextureManager::getSingleton().createManual( - this->ogreDepthTextureName[1], - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, - Ogre::TEX_TYPE_2D, - this->imageWidth, this->imageHeight, 0, - Ogre::PF_FLOAT32_R, - //Ogre::PF_DEPTH, - //Ogre::PF_R8G8B8, - Ogre::TU_RENDERTARGET); - - this->depthRenderTarget[1] = this->depthRenderTexture[1]->getBuffer()->getRenderTarget(); - */ - - // Create the camera this->camera = OgreCreator::CreateCamera(this->GetName(), - this->nearClip, this->farClip, this->hfov, this->renderTarget[0]); + this->nearClip, this->farClip, this->hfov, + this->renderTarget[0]); + //this->camera->setUseRenderingDistance(true); - // Hack to make the camera use the right render target too - /*{ - Ogre::Viewport *cviewport; + // Hack to make the camera use the right render target too. + for (i=0; i<4; i++) + { + this->renderTarget[i]->setAutoUpdated(false); - // Setup the viewport to use the texture - cviewport = this->renderTarget[1]->addViewport(camera); - cviewport->setClearEveryFrame(true); - cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); - cviewport->setOverlaysEnabled(false); + if (i > 0) + { + // Setup the viewport to use the texture + cviewport = this->renderTarget[i]->addViewport(camera); + cviewport->setClearEveryFrame(true); + cviewport->setOverlaysEnabled(false); + cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); + } - cviewport = this->depthRenderTarget[0]->addViewport(camera); - cviewport->setClearEveryFrame(true); - cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); - cviewport->setOverlaysEnabled(false); + // Create materials for all the render textures. + matPtr = Ogre::MaterialManager::getSingleton().create( + this->materialName[i], + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - cviewport = this->depthRenderTarget[1]->addViewport(camera); - cviewport->setClearEveryFrame(true); - cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); - cviewport->setOverlaysEnabled(false); - }*/ + matPtr->getTechnique(0)->getPass(0)->createTextureUnitState( + this->textureName[i] ); + } - Ogre::MaterialPtr leftmat = Ogre::MaterialManager::getSingleton().create( - this->ogreMaterialName[0], - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + Ogre::MaterialPtr tmpMat = Ogre::MaterialManager::getSingleton().load("Gazebo/DepthMap", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); + // Get pointer to the depth map material + this->depthMaterial = tmpMat.get(); - Ogre::MaterialPtr depthMat = Ogre::MaterialManager::getSingleton().getByName("Gazebo/DepthMap"); - - /*Ogre::MaterialPtr rightmat = Ogre::MaterialManager::getSingleton().create( - this->ogreMaterialName[1], - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - */ - - //depthMat->getTechnique(0)->getPass(0)->getTextureUnitState(0)->setTextureName(this->renderTexture[0]->getName()); - depthMat->getTechnique(0)->getPass(0)->createTextureUnitState(this->renderTexture[0]->getName()); - /*leftmat->getTechnique(0)->getPass(0)->removeAllTextureUnitStates(); - leftmat->getTechnique(0)->getPass(0)->setLightingEnabled(false); - Ogre::TextureUnitState *texUnit = leftmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName[0]); - texUnit->setTextureBorderColour(Ogre::ColourValue::White); - texUnit->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); - */ - - //rightmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreTextureName[1]); - - //Ogre::MaterialPtr leftdepthmat = Ogre::MaterialManager::getSingleton().create( - //this->ogreDepthMaterialName[0], - //Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - - /*Ogre::MaterialPtr rightdepthmat=Ogre::MaterialManager::getSingleton().create( - this->ogreDepthMaterialName[1], - Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - */ - - //Ogre::TextureUnitState *t = leftdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreDepthTextureName[0]); - //Ogre::TextureUnitState *t = leftdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(OgreAdaptor::Instance()->sceneMgr->getShadowTexture(0)->getName()); - //leftdepthmat->getTechnique(0)->getPass(0)->setLightingEnabled(false); - //t->setTextureAddressingMode(Ogre::TextureUnitState::TAM_CLAMP); - - //leftdepthmat->getTechnique(0)->getPass(0)->setDepthWriteEnabled(true); - //leftdepthmat->getTechnique(0)->getPass(0)->setColourWriteEnabled(false); - - //rightdepthmat->getTechnique(0)->getPass(0)->createTextureUnitState(this->ogreDepthTextureName[1]); - //rightdepthmat->getTechnique(0)->getPass(0)->setLightingEnabled(false); - //rightdepthmat->getTechnique(0)->getPass(0)->setDepthWriteEnabled(true); - //rightdepthmat->getTechnique(0)->getPass(0)->setColourWriteEnabled(false); - - - Ogre::HardwarePixelBufferSharedPtr mBuffer; - // Get access to the buffer and make an image and write it to file mBuffer = this->renderTexture[0]->getBuffer(0, 0); @@ -227,7 +164,20 @@ //this->renderTarget[0]->addListener(&this->leftCameraListener); //this->renderTarget[1]->addListener(&this->rightCameraListener); + + this->rgbBufferSize = this->imageWidth*this->imageHeight * 3; + this->depthBufferSize = this->imageWidth*this->imageHeight; + // Allocate buffers + if (!this->depthBuffer[0]) + this->depthBuffer[0] = new float[this->depthBufferSize]; + if (!this->depthBuffer[1]) + this->depthBuffer[1] = new float[this->depthBufferSize]; + if (!this->rgbBuffer[0]) + this->rgbBuffer[0] = new unsigned char[this->rgbBufferSize]; + if (!this->rgbBuffer[1]) + this->rgbBuffer[1] = new unsigned char[this->rgbBufferSize]; + CameraSensor::InitChild(); } @@ -241,67 +191,177 @@ // Update the drawing void StereoCameraSensor::UpdateChild(UpdateParams ¶ms) { + OgreAdaptor *adapt = OgreAdaptor::Instance(); + Ogre::RenderSystem *renderSys = adapt->root->getRenderSystem(); + Ogre::Viewport *vp = NULL; + Ogre::SceneManager *sceneMgr = adapt->sceneMgr; + Ogre::AutoParamDataSource autoParamDataSource; + Ogre::Pass *pass; + int i; + CameraSensor::UpdateChild(params); - this->renderTarget[0]->writeContentsToFile("texture.png"); + sceneMgr->_suppressRenderStateChanges(true); + + // Get pointer to the material pass + pass = this->depthMaterial->getBestTechnique()->getPass(0); + + for (i=2; i<3; i++) + { + vp = this->renderTarget[i]->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(this->renderTarget[i]); + autoParamDataSource.setCurrentSceneManager(sceneMgr); + autoParamDataSource.setCurrentCamera(this->camera); + pass->_updateAutoParamsNoLights(autoParamDataSource); + + // These two lines don't seem to do anything useful + renderSys->_setProjectionMatrix(this->camera->getProjectionMatrixRS()); + renderSys->_setViewMatrix(this->camera->getViewMatrix(true)); + + // NOTE: We MUST bind parameters AFTER updating the autos + if (pass->hasVertexProgram()) + { + renderSys->bindGpuProgram( pass->getVertexProgram()->_getBindingDelegate() ); + renderSys->bindGpuProgramParameters(Ogre::GPT_VERTEX_PROGRAM, + pass->getVertexProgramParameters()); + } + + if (pass->hasFragmentProgram()) + { + renderSys->bindGpuProgram( pass->getFragmentProgram()->_getBindingDelegate() ); + renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM, + pass->getFragmentProgramParameters()); + } + + // Render the texture + this->renderTarget[i]->update(); + + // 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. + this->camera->setFarClipDistance( this->farClip ); + } + + sceneMgr->_suppressRenderStateChanges(false); + + this->FillBuffers(); } //////////////////////////////////////////////////////////////////////////////// // Return the material the camera renders to std::string StereoCameraSensor::GetMaterialName() const { - return "Gazebo/DepthMap";//this->ogreMaterialName[0]; + return this->materialName[D_LEFT]; } ////////////////////////////////////////////////////////////////////////////// /// Get a pointer to the image data const unsigned char *StereoCameraSensor::GetImageData(unsigned int i) { - Ogre::HardwarePixelBufferSharedPtr mBuffer; - size_t size; if (i > 1) + gzthrow("Index must be 0 for left, or 1 for right disparity image\n"); + + return this->rgbBuffer[i]; +} + +////////////////////////////////////////////////////////////////////////////// +// Get a pointer to the disparity data +const float *StereoCameraSensor::GetDisparityData(unsigned int i) +{ + if (i > 1) + gzthrow("Index must be 0 for left, or 1 for right disparity image\n"); + return this->depthBuffer[i]; +} + +////////////////////////////////////////////////////////////////////////////// +// Fill all RGB and Disparity buffers +void StereoCameraSensor::FillBuffers() +{ + Ogre::HardwarePixelBufferSharedPtr hardwareBuffer; + int top, left, right, bottom, i; + + top = (int)((hardwareBuffer->getHeight() - this->imageHeight) / 2.0); + left = (int)((hardwareBuffer->getWidth() - this->imageWidth) / 2.0); + right = left + this->imageWidth; + bottom = top + this->imageHeight; + + Ogre::Box box(left, top, right, bottom); + + for (i=0; i<4; i++) { - gzerr(0) << "Camera index must be 0=Left or 1=Right for stereo camera\n"; - i = 1; + // Get access to the buffer and make an image and write it to file + hardwareBuffer = this->renderTexture[i]->getBuffer(0, 0); + + hardwareBuffer->lock(Ogre::HardwarePixelBuffer::HBL_DISCARD); + + if (i < 2) + { + hardwareBuffer->blitToMemory ( box, + Ogre::PixelBox( this->imageWidth, this->imageHeight, + 1, Ogre::PF_R8G8B8, this->rgbBuffer[i]) + ); + } + else + { + hardwareBuffer->blitToMemory (box, + Ogre::PixelBox( this->imageWidth, this->imageHeight, + 1, Ogre::PF_FLOAT32_R, this->depthBuffer[i-2]) + ); + } + + hardwareBuffer->unlock(); } +} - // Get access to the buffer and make an image and write it to file - mBuffer = this->depthRenderTexture[i]->getBuffer(0, 0); +//////////////////////////////////////////////////////////////////////////////// +// Save a single frame to disk +void StereoCameraSensor::SaveFrame() +{ + char tmp[1024]; + FILE *fp; + + sprintf(tmp, "frame%04d.pgm", this->saveCount); - size = this->imageWidth * this->imageHeight * 3; + fp = fopen( tmp, "wb" ); - // Allocate buffer - if (!this->saveFrameBuffer) - this->saveFrameBuffer = new unsigned char[size]; + if (!fp) + { + printf( "unable to open file %s\n for writing", tmp ); + return; + } - mBuffer->lock(Ogre::HardwarePixelBuffer::HBL_READ_ONLY); + fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", this->imageWidth, this->imageHeight); - 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; + for (int i = 0; i<this->imageHeight; i++) + { + for (int j =0; j<this->imageWidth; j++) + { + int index = i*this->imageWidth + j; + //float ptr = (float)(*(this->depthBuffer + i * this->imageWidth + j)); - // Get the center of the texture in RGB 24 bit format - mBuffer->blitToMemory( - Ogre::Box(left, top, right, bottom), + unsigned char value = this->saveFrameBuffer[i*this->imageWidth+j]; + fwrite( &value, 1, 1, fp ); + fwrite( &value, 1, 1, fp ); + fwrite( &value, 1, 1, fp ); + } + } - Ogre::PixelBox( - this->imageWidth, - this->imageHeight, - 1, - Ogre::PF_B8G8R8, - this->saveFrameBuffer) - ); + fclose( fp ); + this->saveCount++; - mBuffer->unlock(); - - return this->saveFrameBuffer; + return; } ////////////////////////////////////////////////////////////////////////////// // Save the current frame to disk -void StereoCameraSensor::SaveFrame() +/*void StereoCameraSensor::SaveFrame() { Ogre::HardwarePixelBufferSharedPtr mBuffer; std::ostringstream sstream; @@ -309,12 +369,12 @@ Ogre::Codec * pCodec; size_t size, pos; -// for (int i=0; i<2; i++) + for (int i=0; i<4; i++) { - this->GetImageData(0); + this->GetImageData(i); // Get access to the buffer and make an image and write it to file - mBuffer = this->depthRenderTexture[0]->getBuffer(0, 0); + mBuffer = this->renderTexture[i]->getBuffer(0, 0); // Create image data structure imgData = new Ogre::ImageCodec::ImageData(); @@ -322,7 +382,11 @@ imgData->width = this->imageWidth; imgData->height = this->imageHeight; imgData->depth = 1; - imgData->format = Ogre::PF_B8G8R8; + if (i<2) + imgData->format = Ogre::PF_B8G8R8; + else + imgData->format = Ogre::PF_FLOAT32_R; + size = this->GetImageByteSize(); // Wrap buffer in a chunk @@ -364,16 +428,7 @@ this->saveCount++; } - -//void StereCameraSensor::UpdateAllDependentRenderTargets() -//{ -/* Ogre::RenderTargetList::iterator iter; - - for( iter = mRenderTargetList.begin(); iter != mRenderTargetList.end(); ++iter ) - { (*iter)->update(); - } */ -//} //////////////////////////////////////////////////////////////////////////////// /// Get the baselien of the camera @@ -383,7 +438,7 @@ } -void StereoCameraSensor::StereoCameraListener::Init( +/*void StereoCameraSensor::StereoCameraListener::Init( StereoCameraSensor *cam, Ogre::RenderTarget *target, bool isLeft) { this->sensor = cam; @@ -426,3 +481,23 @@ this->camera->setFrustumOffset(0,0); this->camera->setPosition(this->pos); } +*/ + +Ogre::TexturePtr StereoCameraSensor::CreateRTT(const std::string &name, bool depth) +{ + Ogre::PixelFormat pf; + + if (depth) + pf = Ogre::PF_FLOAT32_R; + else + pf = Ogre::PF_R8G8B8; + + // Create the left render texture + return Ogre::TextureManager::getSingleton().createManual( + name, + Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, + Ogre::TEX_TYPE_2D, + this->imageWidth, this->imageHeight, 0, + pf, + Ogre::TU_RENDERTARGET); +} Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-04-07 18:04:20 UTC (rev 6279) @@ -40,6 +40,7 @@ class Camera; class Viewport; class SceneNode; + class Material; } namespace gazebo @@ -58,6 +59,8 @@ class StereoCameraSensor : public CameraSensor { + enum Sides {LEFT, RIGHT, D_LEFT, D_RIGHT}; + /// \brief Constructor public: StereoCameraSensor(Body *body); @@ -81,34 +84,40 @@ public: virtual std::string GetMaterialName() const; /// \brief Get a pointer to the image data + /// \param i 0=left, 1=right public: virtual const unsigned char *GetImageData(unsigned int i=0); + /// \brief Get a point to the disparity data + /// \param i 0=left, 1=right + public: const float *GetDisparityData(unsigned int i=0); + /// \brief Get the baselien of the camera public: double GetBaseline() const; // Save the camera frame protected: virtual void SaveFrame(); - private: void ReadDepthImage(); + /// \brief Fill all the image buffers + private: void FillBuffers(); + private: Ogre::TexturePtr CreateRTT( const std::string &name, bool depth); + //private: void UpdateAllDependentRenderTargets(); - private: Ogre::TexturePtr renderTexture[2]; - private: Ogre::TexturePtr depthRenderTexture[2]; + private: Ogre::TexturePtr renderTexture[4]; + private: Ogre::RenderTarget *renderTarget[4]; + private: Ogre::Material *depthMaterial; - private: Ogre::RenderTarget *renderTarget[2]; - private: Ogre::RenderTarget *depthRenderTarget[2]; + private: std::string textureName[4]; + private: std::string materialName[4]; - - private: std::string ogreTextureName[2]; - private: std::string ogreMaterialName[2]; - private: std::string ogreDepthTextureName[2]; - private: std::string ogreDepthMaterialName[2]; - - + private: unsigned int depthBufferSize; + private: unsigned int rgbBufferSize; + private: float *depthBuffer[2]; + private: unsigned char *rgbBuffer[2]; private: double baseline; - private: + /*private: class StereoCameraListener : public Ogre::RenderTargetListener { public: StereoCameraListener() : Ogre::RenderTargetListener() {} @@ -127,6 +136,7 @@ private: StereoCameraListener leftCameraListener; private: StereoCameraListener rightCameraListener; + */ }; /// \} Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-04-03 22:03:22 UTC (rev 6278) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-04-07 18:04:20 UTC (rev 6279) @@ -29,14 +29,15 @@ <rendering:ogre> <ambient>1.0 1.0 1.0 1.0</ambient> - <sky> + <!--<sky> <material>Gazebo/CloudySky</material> </sky> + --> </rendering:ogre> <model:physical name="sphere1_model"> - <xyz>4 -1 1.5</xyz> + <xyz>90 -1 1.5</xyz> <rpy>0.0 0.0 0.0</rpy> <static>false</static> @@ -103,6 +104,10 @@ <saveFrames>false</saveFrames> <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> + + <controller:stereo_camera name="stereo_camera_controller"> + <interface:stereocamera name="stereo_camera_interface" /> + </controller:stereo_camera> </sensor:stereocamera> </body:empty> </model:physical> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-04-07 18:06:03
|
Revision: 6283 http://playerstage.svn.sourceforge.net/playerstage/?rev=6283&view=rev Author: natepak Date: 2008-04-07 17:58:08 -0700 (Mon, 07 Apr 2008) Log Message: ----------- Fixing the stereo camera Modified Paths: -------------- code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreAdaptor.hh code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/worlds/stereocamera.world Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-04-07 22:06:06 UTC (rev 6282) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-04-08 00:58:08 UTC (rev 6283) @@ -68,6 +68,7 @@ this->camera=NULL; this->viewport=NULL; this->root=NULL; + this->renderSys = NULL; } @@ -399,34 +400,35 @@ { Ogre::RenderSystemList *rsList = this->root->getAvailableRenderers(); int c = 0; - Ogre::RenderSystem *selectedRenderSystem = NULL; + this->renderSys = NULL; + do { if (c == (int)rsList->size()) break; - selectedRenderSystem = rsList->at(c); + this->renderSys = rsList->at(c); c++; } - while (selectedRenderSystem->getName().compare("OpenGL Rendering Subsystem")!= 0); + while (this->renderSys->getName().compare("OpenGL Rendering Subsystem")!= 0); - if (selectedRenderSystem == NULL) + if (this->renderSys == NULL) { gzthrow( "unable to find rendering system" ); } - selectedRenderSystem->setConfigOption("Full Screen","No"); - selectedRenderSystem->setConfigOption("FSAA","2"); + this->renderSys->setConfigOption("Full Screen","No"); + this->renderSys->setConfigOption("FSAA","2"); // Set the preferred RRT mode. Options are: "PBuffer", "FBO", and "Copy", can be set in the .gazeborc file - selectedRenderSystem->setConfigOption("RTT Preferred Mode", Global::RTTMode); + this->renderSys->setConfigOption("RTT Preferred Mode", Global::RTTMode); if (create && this->videoMode != "None") { - selectedRenderSystem->setConfigOption("Video Mode",this->videoMode); - this->root->setRenderSystem(selectedRenderSystem); + this->renderSys->setConfigOption("Video Mode",this->videoMode); + this->root->setRenderSystem(this->renderSys); } else { @@ -479,6 +481,7 @@ OgreHUD::Instance()->Update(); this->root->renderOneFrame(); + return 0; } Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-04-07 22:06:06 UTC (rev 6282) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-04-08 00:58:08 UTC (rev 6283) @@ -50,6 +50,7 @@ class SceneNode; class RenderTarget; class ColourValue; + class RenderSystem; } namespace gazebo @@ -95,22 +96,25 @@ private: void SetupRenderSystem(bool create); private: void CreateWindow(); - /// \brief Pointer to the root scene node + /// Pointer to the root scene node public: Ogre::Root *root; - /// \brief Pointer to the scene manager + /// Pointer to the scene manager public: Ogre::SceneManager *sceneMgr; - /// \brief Pointer to the render window + /// Pointer to the rendering system + public: Ogre::RenderSystem *renderSys; + + /// Pointer to the render window public: Ogre::RenderWindow *window; - /// \brief Pointer to the camera + /// Pointer to the camera public: Ogre::Camera *camera; - /// \brief Pointer to the viewport + /// Pointer to the viewport public: Ogre::Viewport *viewport; - /// \brief Pointer to the input reader + /// Pointer to the input reader public: Ogre::InputReader *inputDevice; private: Ogre::LogManager *logManager; Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-07 22:06:06 UTC (rev 6282) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-08 00:58:08 UTC (rev 6283) @@ -103,16 +103,17 @@ this->materialName[D_LEFT] = this->GetName()+"_RttMat_Stereo_Left_Depth"; this->materialName[D_RIGHT] = this->GetName()+"_RttMat_Stereo_Right_Depth"; + i = D_LEFT; // Create the render textures for the color textures - for (i = 0; i<2; i++) + /*for (i = 0; i<2; i++) { this->renderTexture[i] = this->CreateRTT(this->textureName[i], false); this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); - } + }*/ // Create the render texture for the depth textures - for (i = 2; i<4; i++) + //for (i = 2; i<4; i++) { this->renderTexture[i] = this->CreateRTT(this->textureName[i], true); this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); @@ -121,23 +122,23 @@ // Create the camera this->camera = OgreCreator::CreateCamera(this->GetName(), this->nearClip, this->farClip, this->hfov, - this->renderTarget[0]); + this->renderTarget[i]); //this->camera->setUseRenderingDistance(true); // Hack to make the camera use the right render target too. - for (i=0; i<4; i++) + //for (i=2; i<3; i++) { this->renderTarget[i]->setAutoUpdated(false); - if (i > 0) + /*if (i > 0) { // Setup the viewport to use the texture cviewport = this->renderTarget[i]->addViewport(camera); cviewport->setClearEveryFrame(true); cviewport->setOverlaysEnabled(false); cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); - } + }*/ // Create materials for all the render textures. matPtr = Ogre::MaterialManager::getSingleton().create( @@ -148,17 +149,17 @@ this->textureName[i] ); } - Ogre::MaterialPtr tmpMat = Ogre::MaterialManager::getSingleton().load("Gazebo/DepthMap", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); // Get pointer to the depth map material - this->depthMaterial = tmpMat.get(); + this->depthMaterial = Ogre::MaterialManager::getSingleton().load("Gazebo/DepthMap", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - // Get access to the buffer and make an image and write it to file - mBuffer = this->renderTexture[0]->getBuffer(0, 0); + mBuffer = this->renderTexture[i]->getBuffer(0, 0); this->textureWidth = mBuffer->getWidth(); this->textureHeight = mBuffer->getHeight(); + std::cout << "Texture size[" << this->textureWidth << " " << this->textureHeight << "]\n"; + //this->leftCameraListener.Init(this, this->renderTarget[0], true); //this->rightCameraListener.Init(this, this->renderTarget[1], false); @@ -195,34 +196,42 @@ Ogre::RenderSystem *renderSys = adapt->root->getRenderSystem(); Ogre::Viewport *vp = NULL; Ogre::SceneManager *sceneMgr = adapt->sceneMgr; - Ogre::AutoParamDataSource autoParamDataSource; Ogre::Pass *pass; int i; CameraSensor::UpdateChild(params); - sceneMgr->_suppressRenderStateChanges(true); + //sceneMgr->_suppressRenderStateChanges(true); // Get pointer to the material pass pass = this->depthMaterial->getBestTechnique()->getPass(0); - for (i=2; i<3; i++) + i = D_LEFT; + + //for (i=2; i<3; i++) { + Ogre::AutoParamDataSource autoParamDataSource; + vp = this->renderTarget[i]->getViewport(0); // Need this line to render the ground plane. No idea why it's necessary. - renderSys->_setViewport(vp); + /*renderSys->_setViewport(vp); sceneMgr->_setPass(pass, true, false); autoParamDataSource.setCurrentPass(pass); autoParamDataSource.setCurrentViewport(vp); autoParamDataSource.setCurrentRenderTarget(this->renderTarget[i]); autoParamDataSource.setCurrentSceneManager(sceneMgr); autoParamDataSource.setCurrentCamera(this->camera); - pass->_updateAutoParamsNoLights(autoParamDataSource); + printf("8\n"); + //pass->_updateAutoParamsNoLights(autoParamDataSource); + printf("9\n"); + printf("10\n"); // These two lines don't seem to do anything useful renderSys->_setProjectionMatrix(this->camera->getProjectionMatrixRS()); + printf("11\n"); renderSys->_setViewMatrix(this->camera->getViewMatrix(true)); + printf("12\n"); // NOTE: We MUST bind parameters AFTER updating the autos if (pass->hasVertexProgram()) @@ -239,8 +248,12 @@ pass->getFragmentProgramParameters()); } + */ + + printf("i[%d]\n",i); // Render the texture this->renderTarget[i]->update(); + printf("13\n"); // OgreSceneManager::_render function automatically sets farClip to 0. // Which normally equates to infinite distance. We don't want this. So @@ -248,9 +261,9 @@ this->camera->setFarClipDistance( this->farClip ); } - sceneMgr->_suppressRenderStateChanges(false); + //sceneMgr->_suppressRenderStateChanges(false); - this->FillBuffers(); + //this->FillBuffers(); } //////////////////////////////////////////////////////////////////////////////// @@ -299,7 +312,7 @@ // Get access to the buffer and make an image and write it to file hardwareBuffer = this->renderTexture[i]->getBuffer(0, 0); - hardwareBuffer->lock(Ogre::HardwarePixelBuffer::HBL_DISCARD); + // hardwareBuffer->lock(Ogre::HardwarePixelBuffer::HBL_NORMAL); if (i < 2) { @@ -316,7 +329,7 @@ ); } - hardwareBuffer->unlock(); + //hardwareBuffer->unlock(); } } @@ -497,7 +510,7 @@ name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, - this->imageWidth, this->imageHeight, 0, + 512, 512, 0, pf, Ogre::TU_RENDERTARGET); } Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-04-07 22:06:06 UTC (rev 6282) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-04-08 00:58:08 UTC (rev 6283) @@ -29,18 +29,17 @@ #include <OgrePrerequisites.h> #include <OgreTexture.h> +#include <OgreMaterial.h> #include "CameraSensor.hh" // Forward Declarations namespace Ogre { - class TexturePtr; class RenderTarget; class Camera; class Viewport; class SceneNode; - class Material; } namespace gazebo @@ -106,7 +105,7 @@ private: Ogre::TexturePtr renderTexture[4]; private: Ogre::RenderTarget *renderTarget[4]; - private: Ogre::Material *depthMaterial; + private: Ogre::MaterialPtr depthMaterial; private: std::string textureName[4]; private: std::string materialName[4]; Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-04-07 22:06:06 UTC (rev 6282) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-04-08 00:58:08 UTC (rev 6283) @@ -29,10 +29,10 @@ <rendering:ogre> <ambient>1.0 1.0 1.0 1.0</ambient> - <!--<sky> + <sky> <material>Gazebo/CloudySky</material> </sky> - --> + <shadowTechnique>stencilAdditive</shadowTechnique> </rendering:ogre> @@ -105,9 +105,10 @@ <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> - <controller:stereo_camera name="stereo_camera_controller"> + <!--<controller:stereo_camera name="stereo_camera_controller"> <interface:stereocamera name="stereo_camera_interface" /> </controller:stereo_camera> + --> </sensor:stereocamera> </body:empty> </model:physical> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-04-08 21:06:36
|
Revision: 6292 http://playerstage.svn.sourceforge.net/playerstage/?rev=6292&view=rev Author: natepak Date: 2008-04-08 21:06:40 -0700 (Tue, 08 Apr 2008) Log Message: ----------- More fixes to stereo vision Modified Paths: -------------- code/gazebo/trunk/build.py code/gazebo/trunk/examples/libgazebo/SConscript code/gazebo/trunk/examples/libgazebo/camera/SConstruct code/gazebo/trunk/examples/libgazebo/factory/.sconsign.dblite code/gazebo/trunk/examples/libgazebo/factory/SConstruct code/gazebo/trunk/examples/libgazebo/simiface/SConstruct code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/physics/HeightmapGeom.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/worlds/stereocamera.world Modified: code/gazebo/trunk/build.py =================================================================== --- code/gazebo/trunk/build.py 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/build.py 2008-04-09 04:06:40 UTC (rev 6292) @@ -54,8 +54,8 @@ f.write('Name: gazebo\n') f.write('Description: Simplified interface to Player\n') f.write('Version:' + version + '\n') - f.write('Requires:\n') - f.write('Libs: -L' + prefix + '/lib -lgazeboServer\n') + f.write('Requires: OGRE\n') + f.write('Libs: -L' + prefix + '/lib -lgazeboServer -lode\n') f.write('Cflags: -I' + prefix + '/include\n') f.close() Modified: code/gazebo/trunk/examples/libgazebo/SConscript =================================================================== --- code/gazebo/trunk/examples/libgazebo/SConscript 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/examples/libgazebo/SConscript 2008-04-09 04:06:40 UTC (rev 6292) @@ -1,4 +1,4 @@ -dirs = Split('audio camera position factory simiface') +dirs = Split('audio camera position factory simiface stereo') for subdir in dirs: SConscript('%s/SConscript' % subdir) Modified: code/gazebo/trunk/examples/libgazebo/camera/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/camera/SConstruct 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/examples/libgazebo/camera/SConstruct 2008-04-09 04:06:40 UTC (rev 6292) @@ -1,12 +1,6 @@ # 3rd party packages -parseConfigs=['pkg-config --cflags --libs OGRE', - 'xml2-config --cflags --libs', - 'ode-config --cflags --libs', - 'pkg-config --cflags --libs OIS', - 'fltk-config --cflags --libs --use-gl --use-images', - 'xft-config --cflags --libs', - 'pkg-config --cflags --libs libgazebo', +parseConfigs=['pkg-config --cflags --libs libgazebo', 'pkg-config --cflags --libs libgazeboServer'] Modified: code/gazebo/trunk/examples/libgazebo/factory/.sconsign.dblite =================================================================== (Binary files differ) Modified: code/gazebo/trunk/examples/libgazebo/factory/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/factory/SConstruct 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/examples/libgazebo/factory/SConstruct 2008-04-09 04:06:40 UTC (rev 6292) @@ -1,12 +1,6 @@ # 3rd party packages -parseConfigs=['pkg-config --cflags --libs OGRE', - 'xml2-config --cflags --libs', - 'ode-config --cflags --libs', - 'pkg-config --cflags --libs OIS', - 'fltk-config --cflags --libs --use-gl --use-images', - 'xft-config --cflags --libs', - 'pkg-config --cflags --libs libgazebo', +parseConfigs=['pkg-config --cflags --libs libgazebo', 'pkg-config --cflags --libs libgazeboServer'] env = Environment ( Modified: code/gazebo/trunk/examples/libgazebo/simiface/SConstruct =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/examples/libgazebo/simiface/SConstruct 2008-04-09 04:06:40 UTC (rev 6292) @@ -1,11 +1,6 @@ # 3rd party packages -parseConfigs=['pkg-config --cflags --libs OGRE', - 'xml2-config --cflags --libs', - 'ode-config --cflags --libs', - 'fltk-config --cflags --libs --use-gl --use-images', - 'xft-config --cflags --libs', - 'pkg-config --cflags --libs libgazebo', +parseConfigs=['pkg-config --cflags --libs libgazebo', 'pkg-config --cflags --libs libgazeboServer'] Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-04-09 04:06:40 UTC (rev 6292) @@ -174,6 +174,8 @@ << ((Iface*) this->mMap)->size << "\n"; std::cout.flags(origFlags); + + std::cout << "Create: Size[" << this->size << "] Size[" << ((Iface*) this->mMap)->size << "]\n"; } ////////////////////////////////////////////////////////////////////////////// @@ -257,6 +259,8 @@ throw(stream.str()); } + std::cout << "Open Size[" <<((Iface*) this->mMap)->size << "]\n"; + // Make sure everything is consistent if (((Iface*) this->mMap)->size < this->size) { Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-04-09 04:06:40 UTC (rev 6292) @@ -1324,8 +1324,8 @@ \{ */ -#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 3 -#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480 +#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 320 * 240 * 3 +#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 320 * 240 /// \brief Stereo data class StereoCameraData Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-04-09 04:06:40 UTC (rev 6292) @@ -93,9 +93,12 @@ void Stereo_Camera::PutCameraData() { StereoCameraData *data = this->cameraIface->data; - const unsigned char *rgb_src; - unsigned char *rgb_dst; + const unsigned char *rgb_src = NULL; + unsigned char *rgb_dst = NULL; + const unsigned char *rgb_src2 = NULL; + unsigned char *rgb_dst2 = NULL; + const float *disp_src; float *disp_dst; @@ -138,7 +141,7 @@ memcpy(disp_dst, disp_src, data->left_disparity_size); // Copy the right disparity data to the interface - disp_src = this->myParent->GetDisparityData(0); + disp_src = this->myParent->GetDisparityData(1); disp_dst = data->right_disparity; memcpy(disp_dst, disp_src, data->right_disparity_size); Modified: code/gazebo/trunk/server/physics/HeightmapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-04-09 04:06:40 UTC (rev 6292) @@ -148,8 +148,8 @@ this->terrainVertSize = tmpImage.getWidth(); - float nf = log(this->terrainVertSize-1)/log(2); - int ni = log(this->terrainVertSize-1)/log(2); + float nf = (float)(log(this->terrainVertSize-1)/log(2)); + int ni = (int)(log(this->terrainVertSize-1)/log(2)); // Make sure the heightmap image size is (2^n)+1 in size if ( nf - ni != 0) @@ -158,7 +158,7 @@ } // Calculate a good tile size - tileSize = pow( 2, ni/2 ); + tileSize = (int)(pow( 2, ni/2 )); if (tileSize <= 2) Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-09 04:06:40 UTC (rev 6292) @@ -42,7 +42,7 @@ #include "CameraManager.hh" #include "StereoCameraSensor.hh" -#define PF_FLOAT Ogre::PF_FLOAT16_R +#define PF_FLOAT Ogre::PF_FLOAT32_R using namespace gazebo; @@ -80,8 +80,6 @@ { CameraSensor::LoadChild(node); - std::cout << "Image Size[" << this->imageWidth << " " << this->imageHeight << "]\n"; - this->baseline = node->GetDouble("baseline",0,1); } @@ -105,17 +103,15 @@ this->materialName[D_LEFT] = this->GetName()+"_RttMat_Stereo_Left_Depth"; this->materialName[D_RIGHT] = this->GetName()+"_RttMat_Stereo_Right_Depth"; - i = D_LEFT; - // Create the render textures for the color textures - /*for (i = 0; i<2; i++) + for (i = 0; i<2; i++) { this->renderTexture[i] = this->CreateRTT(this->textureName[i], false); this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); - }*/ + } // Create the render texture for the depth textures - //for (i = 2; i<4; i++) + for (i = 2; i<4; i++) { this->renderTexture[i] = this->CreateRTT(this->textureName[i], true); this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); @@ -124,23 +120,23 @@ // Create the camera this->camera = OgreCreator::CreateCamera(this->GetName(), this->nearClip, this->farClip, this->hfov, - this->renderTarget[i]); + this->renderTarget[0]); //this->camera->setUseRenderingDistance(true); // Hack to make the camera use the right render target too. - //for (i=2; i<3; i++) + for (i=0; i<4; i++) { this->renderTarget[i]->setAutoUpdated(false); - /*if (i > 0) + if (i > 0) { // Setup the viewport to use the texture cviewport = this->renderTarget[i]->addViewport(camera); cviewport->setClearEveryFrame(true); cviewport->setOverlaysEnabled(false); cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); - }*/ + } // Create materials for all the render textures. matPtr = Ogre::MaterialManager::getSingleton().create( @@ -154,13 +150,11 @@ // Get pointer to the depth map material this->depthMaterial = Ogre::MaterialManager::getSingleton().load("Gazebo/DepthMap", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - // Get access to the buffer and make an image and write it to file - mBuffer = this->renderTexture[i]->getBuffer(0, 0); + mBuffer = this->renderTexture[0]->getBuffer(0,0); this->textureWidth = mBuffer->getWidth(); this->textureHeight = mBuffer->getHeight(); - std::cout << "Texture size[" << this->textureWidth << " " << this->textureHeight << "]\n"; //this->leftCameraListener.Init(this, this->renderTarget[0], true); //this->rightCameraListener.Init(this, this->renderTarget[1], false); @@ -172,14 +166,10 @@ this->depthBufferSize = this->imageWidth*this->imageHeight; // Allocate buffers - if (!this->depthBuffer[0]) - this->depthBuffer[0] = new float[this->depthBufferSize]; - if (!this->depthBuffer[1]) - this->depthBuffer[1] = new float[this->depthBufferSize]; - if (!this->rgbBuffer[0]) - this->rgbBuffer[0] = new unsigned char[this->rgbBufferSize]; - if (!this->rgbBuffer[1]) - this->rgbBuffer[1] = new unsigned char[this->rgbBufferSize]; + this->depthBuffer[0] = new float[this->depthBufferSize]; + this->depthBuffer[1] = new float[this->depthBufferSize]; + this->rgbBuffer[0] = new unsigned char[this->rgbBufferSize]; + this->rgbBuffer[1] = new unsigned char[this->rgbBufferSize]; CameraSensor::InitChild(); } @@ -203,14 +193,20 @@ CameraSensor::UpdateChild(params); + // Render the image texture + for (i=0; i<2; i++) + { + this->renderTarget[i]->update(); + } + sceneMgr->_suppressRenderStateChanges(true); // Get pointer to the material pass pass = this->depthMaterial->getBestTechnique()->getPass(0); - i = D_LEFT; - //for (i=2; i<3; i++) + // Render the depth texture + for (i=2; i<4; i++) { Ogre::AutoParamDataSource autoParamDataSource; @@ -245,9 +241,6 @@ pass->getFragmentProgramParameters()); } - // Render the texture - this->renderTarget[i]->update(); - // 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. @@ -263,7 +256,7 @@ // Return the material the camera renders to std::string StereoCameraSensor::GetMaterialName() const { - return this->materialName[D_LEFT]; + return this->materialName[LEFT]; } ////////////////////////////////////////////////////////////////////////////// @@ -283,6 +276,7 @@ { if (i > 1) gzthrow("Index must be 0 for left, or 1 for right disparity image\n"); + return this->depthBuffer[i]; } @@ -292,49 +286,37 @@ { Ogre::HardwarePixelBufferSharedPtr hardwareBuffer; int i; - Ogre::Box box; + int top,left,right,bottom; for (i=2; i<3; i++) { - printf("1\n"); // Get access to the buffer and make an image and write it to file hardwareBuffer = this->renderTexture[i]->getBuffer(0, 0); - hardwareBuffer->lock(Ogre::HardwarePixelBuffer::HBL_DISCARD); + hardwareBuffer->lock(Ogre::HardwarePixelBuffer::HBL_NORMAL); - box.top = (int)((hardwareBuffer->getHeight() - this->imageHeight) / 2.0); - box.left = (int)((hardwareBuffer->getWidth() - this->imageWidth) / 2.0); - box.right = box.left + this->imageWidth; - box.bottom = box.top + this->imageHeight; + top = (int)((hardwareBuffer->getHeight() - this->imageHeight) / 2.0); + left = (int)((hardwareBuffer->getWidth() - this->imageWidth) / 2.0); + right = left + this->imageWidth; + bottom = top + this->imageHeight; - printf("2\n"); if (i < 2) { - hardwareBuffer->blitToMemory ( box, + hardwareBuffer->blitToMemory ( Ogre::Box(left,top,right,bottom), Ogre::PixelBox( this->imageWidth, this->imageHeight, 1, Ogre::PF_R8G8B8, this->rgbBuffer[i]) ); } else { - hardwareBuffer->blitToMemory (box, + hardwareBuffer->blitToMemory (Ogre::Box(left,top,right,bottom), Ogre::PixelBox( this->imageWidth, this->imageHeight, 1, PF_FLOAT, this->depthBuffer[i-2]) ); } - printf("3\n"); hardwareBuffer->unlock(); } - - printf("4\n"); - for (i=0; i<this->imageHeight; i++) - { - for (int j = 0; j<this->imageWidth; j++) - { - printf("%4.2f\n",*(this->depthBuffer[D_LEFT] + i*this->imageWidth+j)); - } - } } //////////////////////////////////////////////////////////////////////////////// @@ -514,7 +496,7 @@ name, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME, Ogre::TEX_TYPE_2D, - 512, 512, 0, + this->imageWidth, this->imageHeight, 0, pf, Ogre::TU_RENDERTARGET); } Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-04-09 01:40:05 UTC (rev 6291) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-04-09 04:06:40 UTC (rev 6292) @@ -105,10 +105,9 @@ <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> - <!--<controller:stereo_camera name="stereo_camera_controller"> - <interface:stereocamera name="stereo_camera_interface" /> + <controller:stereo_camera name="stereo_camera_controller"> + <interface:stereocamera name="stereo_iface_0" /> </controller:stereo_camera> - --> </sensor:stereocamera> </body:empty> </model:physical> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-04-10 14:24:57
|
Revision: 6296 http://playerstage.svn.sourceforge.net/playerstage/?rev=6296&view=rev Author: natepak Date: 2008-04-10 14:24:57 -0700 (Thu, 10 Apr 2008) Log Message: ----------- Fix memory map problem with libgazebo. More fixes to stereo vision Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/examples/libgazebo/camera/camera.cc code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc code/gazebo/trunk/libgazebo/Iface.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/ActarrayInterface.cc code/gazebo/trunk/player/CameraInterface.cc code/gazebo/trunk/player/FiducialInterface.cc code/gazebo/trunk/player/LaserInterface.cc code/gazebo/trunk/player/PTZInterface.cc code/gazebo/trunk/player/Position2dInterface.cc code/gazebo/trunk/player/Position3dInterface.cc code/gazebo/trunk/player/PowerInterface.cc code/gazebo/trunk/player/SonarInterface.cc code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc code/gazebo/trunk/server/controllers/audio/Audio.cc code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc code/gazebo/trunk/server/sensors/camera/CameraSensor.cc code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/worlds/stereocamera.world Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/SConstruct 2008-04-10 21:24:57 UTC (rev 6296) @@ -80,7 +80,7 @@ rcconfig = env.RCConfig(target='gazeborc', source=Value(install_prefix)) # DEFAULT list of subdirectories to build -subdirs = ['server','libgazebo', 'player'] +subdirs = ['libgazebo','server', 'player'] # Set the compile mode if env['mode'] == 'debug': @@ -95,7 +95,7 @@ parseConfigs+=['pkg-config --cflags --libs OgreAL'] -optimize_for_cpu(env); +#optimize_for_cpu(env); # # Parse all the pacakge configurations @@ -170,6 +170,7 @@ # Process subdirectories # for subdir in subdirs: + print subdir SConscript('%s/SConscript' % subdir) # @@ -177,6 +178,7 @@ # gazebo = env.Program('gazebo',staticObjs) +Depends(gazebo, 'libgazebo/libgazebo.so') # # Create static and shared libraries for the server # Modified: code/gazebo/trunk/examples/libgazebo/camera/camera.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/camera/camera.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/examples/libgazebo/camera/camera.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -59,7 +59,7 @@ /// Open the Camera interface try { - camIface->Open(client, "camera_iface_1"); + camIface->Open(client, "camera_iface_0"); } catch (std::string e) { Modified: code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/examples/libgazebo/stereo/stereo.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -1,12 +1,50 @@ #include <gazebo/gazebo.h> #include <gazebo/GazeboError.hh> +gazebo::Client *client = NULL; +gazebo::SimulationIface *simIface = NULL; +gazebo::StereoCameraIface *stereoIface = NULL; +int saveCount = 0; + +void SaveFrame() +{ + char tmp[1024]; + FILE *fp; + + sprintf(tmp, "frame%04d.pgm", saveCount); + + fp = fopen( tmp, "wb" ); + + if (!fp) + { + printf( "unable to open file %s\n for writing", tmp ); + return; + } + + fprintf( fp, "P6\n# Gazebo\n%d %d\n255\n", stereoIface->data->width, stereoIface->data->height); + + for (unsigned int i = 0; i<stereoIface->data->height; i++) + { + for (unsigned int j =0; j<stereoIface->data->width; j++) + { + unsigned char value = stereoIface->data->left_disparity[i*stereoIface->data->width+j] * 255; + fwrite( &value, 1, 1, fp ); + fwrite( &value, 1, 1, fp ); + fwrite( &value, 1, 1, fp ); + } + } + + fclose( fp ); + saveCount++; +} + int main() { - gazebo::Client *client = new gazebo::Client(); - gazebo::SimulationIface *simIface = new gazebo::SimulationIface(); - gazebo::StereoCameraIface *stereoIface = new gazebo::StereoIface(); + client = new gazebo::Client(); + simIface = new gazebo::SimulationIface(); + stereoIface = new gazebo::StereoCameraIface(); + int serverId = 0; /// Connect to the libgazebo server @@ -34,7 +72,7 @@ /// Open the Stereo interface try { - posIface->Open(client, "stereo_iface_0"); + stereoIface->Open(client, "stereo_iface_0"); } catch (std::string e) { @@ -43,18 +81,16 @@ return -1; } - // Enable the motor - stereoIface->Lock(1); - stereoIface->Unlock(); - - /*while (true) + while (true) { - posIface->Lock(1); - posIface->data->cmdVelocity.pos.x += 10; - posIface->Unlock(); + stereoIface->Lock(1); + SaveFrame(); + stereoIface->Unlock(); usleep(100000); - }*/ + } + return 0; } + Modified: code/gazebo/trunk/libgazebo/Iface.cc =================================================================== --- code/gazebo/trunk/libgazebo/Iface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/libgazebo/Iface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -63,13 +63,12 @@ Iface::Iface(const std::string &type, size_t size) { this->type = type; + this->size = size; this->server = NULL; this->client = NULL; - this->openCount = 0; - this->creator = false; } @@ -140,6 +139,8 @@ throw(stream.str()); } + this->Lock(1); + // Set the file to the correct size if (ftruncate(this->mmapFd, this->size) < 0) { @@ -158,9 +159,9 @@ memset(this->mMap, 0, this->size); - ((Iface*) this->mMap)->version = LIBGAZEBO_VERSION; - ((Iface*) this->mMap)->size = this->size; - ((Iface*) this->mMap)->openCount = 0; + ((GazeboData*) this->mMap)->version = LIBGAZEBO_VERSION; + ((GazeboData*) this->mMap)->size = this->size; + ((GazeboData*) this->mMap)->openCount = 0; std::ios_base::fmtflags origFlags = std::cout.flags(); @@ -168,14 +169,14 @@ std::cout << "creating " << this->filename.c_str() << " " << setiosflags(std::ios::hex | std::ios::showbase) - << std::setw(3) << ((Iface*) this->mMap)->version << " " + << std::setw(3) << ((GazeboData*) this->mMap)->version << " " << std::setiosflags(std::ios::dec | ~std::ios::showbase) - << ((Iface*) this->mMap)->size << "\n"; + << ((GazeboData*) this->mMap)->size << "\n"; std::cout.flags(origFlags); - std::cout << "Create: Size[" << this->size << "] Size[" << ((Iface*) this->mMap)->size << "]\n"; + this->Unlock(); } ////////////////////////////////////////////////////////////////////////////// @@ -189,10 +190,10 @@ this->Create(server,id); - this->modelType = modelType; + ((GazeboData*)this->mMap)->modelType = modelType; - this->modelId = modelId; - this->parentModelId = parentModelId; + ((GazeboData*)this->mMap)->modelId = modelId; + ((GazeboData*)this->mMap)->parentModelId = parentModelId; } ////////////////////////////////////////////////////////////////////////////// @@ -259,12 +260,10 @@ throw(stream.str()); } - std::cout << "Open Size[" <<((Iface*) this->mMap)->size << "]\n"; - // Make sure everything is consistent - if (((Iface*) this->mMap)->size < this->size) + if (((GazeboData*) this->mMap)->size < this->size) { - stream << "expected file size: " << ((Iface*) this->mMap)->size + stream << "expected file size: " << ((GazeboData*) this->mMap)->size << " < " << this->size; throw(stream.str()); @@ -276,15 +275,19 @@ std::cout << "opening " << this->filename.c_str() << " " << std::setiosflags(std::ios::hex | std::ios::showbase) - << std::setw(3) << ((Iface*) this->mMap)->version << " " + << std::setw(3) << ((GazeboData*) this->mMap)->version << " " << std::setiosflags(std::ios::dec | ~std::ios::showbase) - << ((Iface*) this->mMap)->size << "\n"; + << ((GazeboData*) this->mMap)->size << "\n"; std::cout.setf(origFlags); - ((Iface*)this->mMap)->openCount++; - this->openCount++; + this->Lock(1); + printf("1 Iface Open[%d]\n",((GazeboData*)this->mMap)->openCount); + ((GazeboData*)this->mMap)->openCount++; + + printf("2 Iface Open[%d]\n",((GazeboData*)this->mMap)->openCount); + this->Unlock(); } @@ -292,10 +295,9 @@ // Close the interface (client) void Iface::Close() { - this->openCount--; - ((Iface*)this->mMap)->openCount--; + ((GazeboData*)this->mMap)->openCount--; - if (this->openCount <=0) + if (((GazeboData*)this->mMap)->openCount <= 0) { // Unmap the file munmap(this->mMap, this->size); @@ -368,8 +370,14 @@ /// Get the number of connections int Iface::GetOpenCount() { + int result = 0; + if (this->mMap) - return ((Iface*)this->mMap)->openCount; - else - return 0; + { + this->Lock(1); + result = ((GazeboData*)this->mMap)->openCount; + this->Unlock(); + } + + return result; } Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-04-10 21:24:57 UTC (rev 6296) @@ -265,8 +265,8 @@ /// \param modelId Id of the model /// \param parentModelId Id of the model's parent public: void Create(Server *server, std::string id, - const std::string &modelType, int modelId, - int parentModelId); + const std::string &modelType, int modelId, + int parentModelId); /// \brief Destroy the interface (server) public: void Destroy(); @@ -301,10 +301,10 @@ /// The server we are associated with public: Server *server; - + /// The client we are associated with public: Client *client; - + /// File descriptor for the mmap file public: int mmapFd; @@ -314,28 +314,35 @@ /// The name of the file we created/opened public: std::string filename; - /// Interface version number + /// type of interface + protected: std::string type; + + private: bool creator; + + private: size_t size; +}; + +class GazeboData +{ + /// Number of times the interface has been opened + public: int openCount; + + public: double time; + public: int version; - /// Allocation size public: size_t size; - /// Type of model that owns this interface - public: std::string modelType; - /// ID of the model that owns this interface public: int modelId; /// ID of the parent model public: int parentModelId; - /// type of interface - protected: std::string type; + /// Type of model that owns this interface + public: std::string modelType; - /// Number of times the interface has been opened - private: int openCount; - private: bool creator; }; /** \} */ @@ -358,6 +365,8 @@ /// \brief Simulation interface data class SimulationData { + public: GazeboData head; + /// Elapsed simulation time public: double simTime; @@ -449,6 +458,8 @@ /// \brief Audio interface data class AudioData { + public: GazeboData head; + /// Data timestamp public: double time; @@ -544,8 +555,7 @@ /// \brief Camera interface data class CameraData { - /// Data timestamp - public: double time; + public: GazeboData head; /// Width of image in pixels public: unsigned int width; @@ -621,8 +631,7 @@ /// \brief Position interface data class PositionData { - /// Data timestamp - public: double time; + public: GazeboData head; /// Pose (usually global cs) public: Pose pose; @@ -770,8 +779,7 @@ /// \brief Laser interface data class LaserData { - /// Data timestamp - public: double time; + public: GazeboData head; /// Range scan min angle public: double min_angle; @@ -869,6 +877,7 @@ /// \brief Fudicial interface data class FiducialFid { + /// Fiducial id public: int id; @@ -879,8 +888,7 @@ /// \brief Fiducial data class FiducialData { - /// Data timestamp - public: double time; + public: GazeboData head; /// Number of fiducials public: int count; @@ -935,8 +943,7 @@ /// \brief Fudicial interface data class FactoryData { - /// Data timestamp - public: double time; + public: GazeboData head; /// String describing the model to be initiated public: uint8_t newModel[4096]; @@ -1013,8 +1020,7 @@ /// \brief Fudicial interface data class GripperData { - /// Data timestamp - public: double time; + public: GazeboData head; /// \brief Current command for the gripper public: int cmd; @@ -1179,12 +1185,11 @@ /// \brief The actuator array data packet. class ActarrayData { + public: GazeboData head; + /// The number of actuators in the array. public: unsigned int actuators_count; - /// timestamp - public: double time; - /// The actuator data. public: ActarrayActuator actuators[GAZEBO_ACTARRAY_MAX_NUM_ACTUATORS]; @@ -1258,8 +1263,7 @@ /// \brief PTZ data class PTZData { - /// Data timestamp - public: double time; + public: GazeboData head; /// Measured pan angle (radians) public: double pan; @@ -1324,14 +1328,13 @@ \{ */ -#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 320 * 240 * 3 -#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 320 * 240 +#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 3 +#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480 /// \brief Stereo data class StereoCameraData { - /// Data timestamp - public: double time; + public: GazeboData head; /// Width of image in pixels public: unsigned int width; @@ -1339,18 +1342,18 @@ /// Height of image in pixels public: unsigned int height; + /// Left image size + public: unsigned int left_rgb_size; + + /// left image (R8G8B8) + public: unsigned char left_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; + /// Right image size public: unsigned int right_rgb_size; /// Right image (R8G8B8) public: unsigned char right_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; - /// Left image size - public: unsigned int left_rgb_size; - - /// left image (R8G8B8) - public: unsigned char left_rgb[GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE]; - /// Left disparity size public: unsigned int left_disparity_size; Modified: code/gazebo/trunk/player/ActarrayInterface.cc =================================================================== --- code/gazebo/trunk/player/ActarrayInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/ActarrayInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -207,11 +207,11 @@ this->iface->Lock(1); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { //Update the local time so we know when new info comes - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; unsigned int prevCount = this->actData.actuators_count; Modified: code/gazebo/trunk/player/CameraInterface.cc =================================================================== --- code/gazebo/trunk/player/CameraInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/CameraInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -80,15 +80,17 @@ struct timeval ts; + printf("Trying to lock\n"); this->iface->Lock(1); + printf("Player Update\n"); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + /*if (this->iface->data->head.time > this->datatime) { - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); // Set the image properties this->data.width = this->iface->data->width; @@ -127,7 +129,7 @@ this->SaveFrame(filename); } - } + }*/ this->iface->Unlock(); } Modified: code/gazebo/trunk/player/FiducialInterface.cc =================================================================== --- code/gazebo/trunk/player/FiducialInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/FiducialInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -158,15 +158,15 @@ this->iface->Lock(1); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { int i; FiducialFid *fid; - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); memset(&data, 0, sizeof(data)); data.fiducials_count = i; Modified: code/gazebo/trunk/player/LaserInterface.cc =================================================================== --- code/gazebo/trunk/player/LaserInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/LaserInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -168,16 +168,16 @@ { // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { int i; float rangeRes; float angleRes; - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); // Pick the rage resolution to use (1, 10, 100) Modified: code/gazebo/trunk/player/PTZInterface.cc =================================================================== --- code/gazebo/trunk/player/PTZInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/PTZInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -135,16 +135,16 @@ this->iface->Lock(1); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { int i; float rangeRes; float angleRes; - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); memset(&data, 0, sizeof(data)); Modified: code/gazebo/trunk/player/Position2dInterface.cc =================================================================== --- code/gazebo/trunk/player/Position2dInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/Position2dInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -218,12 +218,12 @@ if (this->iface->Lock(1)) { // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); data.pos.px = this->iface->data->pose.pos.x; data.pos.py = this->iface->data->pose.pos.y; Modified: code/gazebo/trunk/player/Position3dInterface.cc =================================================================== --- code/gazebo/trunk/player/Position3dInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/Position3dInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -201,12 +201,12 @@ this->iface->Lock(1); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); data.pos.px = this->iface->data->pose.pos.x; data.pos.py = this->iface->data->pose.pos.y; Modified: code/gazebo/trunk/player/PowerInterface.cc =================================================================== --- code/gazebo/trunk/player/PowerInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/PowerInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -79,12 +79,12 @@ gz_power_lock(this->iface, 1); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); data.percent = this->iface->data->levels[0]; Modified: code/gazebo/trunk/player/SonarInterface.cc =================================================================== --- code/gazebo/trunk/player/SonarInterface.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/player/SonarInterface.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -132,16 +132,16 @@ gz_sonar_lock(this->iface, 1); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { int i; float rangeRes; float angleRes; - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); memset(&data, 0, sizeof(data)); Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -101,7 +101,7 @@ float hiStop, loStop; this->myIface->Lock(1); - this->myIface->data->time = Simulator::Instance()->GetSimTime(); + this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); this->myIface->data->actuators_count = 16; Modified: code/gazebo/trunk/server/controllers/audio/Audio.cc =================================================================== --- code/gazebo/trunk/server/controllers/audio/Audio.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/audio/Audio.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -195,7 +195,7 @@ { if (this->myIface->Lock(1)) { - this->myIface->data->time = Simulator::Instance()->GetSimTime(); + this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); this->myIface->data->state = this->state; this->myIface->Unlock(); Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -101,7 +101,7 @@ this->cameraIface->Lock(1); // Data timestamp - data->time = Simulator::Instance()->GetSimTime(); + data->head.time = Simulator::Instance()->GetSimTime(); data->width = this->myParent->GetImageWidth(); data->height = this->myParent->GetImageHeight(); Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -63,9 +63,17 @@ // Load the controller void Stereo_Camera::LoadChild(XMLConfigNode *node) { - this->cameraIface = dynamic_cast<StereoCameraIface*>(this->ifaces[0]); + std::vector<Iface*>::iterator iter; - if (!this->cameraIface) + for (iter = this->ifaces.begin(); iter != this->ifaces.end(); iter++) + { + if ((*iter)->GetType() == "stereo") + this->stereoIface = dynamic_cast<StereoCameraIface*>(*iter); + else if ((*iter)->GetType() == "camera") + this->cameraIface = dynamic_cast<CameraIface*>(*iter); + } + + if (!this->stereoIface) gzthrow("Stereo_Camera controller requires a StereoCameraIface"); } @@ -79,7 +87,22 @@ // Update the controller void Stereo_Camera::UpdateChild(UpdateParams ¶ms) { - this->PutCameraData(); + + + if (this->cameraIface) + { + this->cameraIface->Lock(1); + if (this->cameraIface->data->head.openCount > 0) + this->PutCameraData(); + this->cameraIface->Unlock(); + + // New data is available + this->cameraIface->Post(); + } + + /*if (this->stereoIface && this->stereoIface->GetOpenCount() > 0) + this->PutStereoData(); + */ } //////////////////////////////////////////////////////////////////////////////// @@ -89,64 +112,93 @@ } //////////////////////////////////////////////////////////////////////////////// -// Put laser data to the interface -void Stereo_Camera::PutCameraData() +// Put stereo data to the interface +void Stereo_Camera::PutStereoData() { - StereoCameraData *data = this->cameraIface->data; - const unsigned char *rgb_src = NULL; - unsigned char *rgb_dst = NULL; + StereoCameraData *stereo_data = this->stereoIface->data; + //const unsigned char *rgb_src = NULL; + //unsigned char *rgb_dst = NULL; - const unsigned char *rgb_src2 = NULL; - unsigned char *rgb_dst2 = NULL; - const float *disp_src; float *disp_dst; - - int i, j, k; - this->cameraIface->Lock(1); + this->stereoIface->Lock(1); // Data timestamp - data->time = Simulator::Instance()->GetSimTime(); + stereo_data->head.time = Simulator::Instance()->GetSimTime(); - data->width = this->myParent->GetImageWidth(); - data->height = this->myParent->GetImageHeight(); + stereo_data->width = this->myParent->GetImageWidth(); + stereo_data->height = this->myParent->GetImageHeight(); - data->right_rgb_size = data->width * data->height * 3; - data->left_rgb_size = data->width * data->height * 3; + //stereo_data->right_rgb_size = stereo_data->width * stereo_data->height * 3; + //stereo_data->left_rgb_size = stereo_data->width * stereo_data->height * 3; - data->right_disparity_size = data->width * data->height; - data->left_disparity_size = data->width * data->height; + stereo_data->right_disparity_size = stereo_data->width * stereo_data->height * sizeof(float); + stereo_data->left_disparity_size = stereo_data->width * stereo_data->height * sizeof(float); // Make sure there is room to store the image - assert (data->right_rgb_size <= sizeof(data->right_rgb)); - assert (data->left_rgb_size <= sizeof(data->left_rgb)); + //assert (stereo_data->right_rgb_size <= sizeof(stereo_data->right_rgb)); + //assert (stereo_data->left_rgb_size <= sizeof(stereo_data->left_rgb)); - assert (data->right_disparity_size <= sizeof(data->right_disparity)); - assert (data->left_disparity_size <= sizeof(data->left_disparity)); + assert (stereo_data->right_disparity_size <= sizeof(stereo_data->right_disparity)); + assert (stereo_data->left_disparity_size <= sizeof(stereo_data->left_disparity)); // Copy the left pixel data to the interface - rgb_src = this->myParent->GetImageData(0); - rgb_dst = data->left_rgb; - memcpy(rgb_dst, rgb_src, data->left_rgb_size); + /*rgb_src = this->myParent->GetImageData(0); + rgb_dst = stereo_data->left_rgb; + memcpy(rgb_dst, rgb_src, stereo_data->left_rgb_size); // Copy the right pixel data to the interface rgb_src = this->myParent->GetImageData(1); - rgb_dst = data->right_rgb; - memcpy(rgb_dst, rgb_src, data->right_rgb_size); + rgb_dst = stereo_data->right_rgb; + memcpy(rgb_dst, rgb_src, stereo_data->right_rgb_size); + */ // Copy the left disparity data to the interface disp_src = this->myParent->GetDisparityData(0); - disp_dst = data->left_disparity; - memcpy(disp_dst, disp_src, data->left_disparity_size); + disp_dst = stereo_data->left_disparity; + memcpy(disp_dst, disp_src, stereo_data->left_disparity_size); // Copy the right disparity data to the interface disp_src = this->myParent->GetDisparityData(1); - disp_dst = data->right_disparity; - memcpy(disp_dst, disp_src, data->right_disparity_size); + disp_dst = stereo_data->right_disparity; + memcpy(disp_dst, disp_src, stereo_data->right_disparity_size); - this->cameraIface->Unlock(); + this->stereoIface->Unlock(); + this->stereoIface->Post(); +} - // New data is available - this->cameraIface->Post(); +//////////////////////////////////////////////////////////////////////////////// +// Put camera data to the interface +void Stereo_Camera::PutCameraData() +{ + CameraData *camera_data = this->cameraIface->data; + const unsigned char *rgb_src = NULL; + unsigned char *rgb_dst = NULL; + Pose3d cameraPose; + + camera_data->head.time = Simulator::Instance()->GetSimTime(); + + camera_data->width = this->myParent->GetImageWidth(); + camera_data->height = this->myParent->GetImageHeight(); + camera_data->image_size = camera_data->width * camera_data->height * 3; + assert (camera_data->image_size <= sizeof(camera_data->image)); + + camera_data->hfov = this->myParent->GetFOV(); + // Set the pose of the camera + cameraPose = this->myParent->GetWorldPose(); + camera_data->camera_pose.pos.x = cameraPose.pos.x; + camera_data->camera_pose.pos.y = cameraPose.pos.y; + camera_data->camera_pose.pos.z = cameraPose.pos.z; + camera_data->camera_pose.roll = cameraPose.rot.GetRoll(); + camera_data->camera_pose.pitch = cameraPose.rot.GetPitch(); + camera_data->camera_pose.yaw = cameraPose.rot.GetYaw(); + + // Copy the pixel data to the interface + rgb_src = this->myParent->GetImageData(0); + rgb_dst = camera_data->image; + + memcpy(rgb_dst, rgb_src, camera_data->image_size); + } + Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-04-10 21:24:57 UTC (rev 6296) @@ -33,6 +33,7 @@ namespace gazebo { class CameraIface; + class StereoCameraIface; class CameraSensor; /// @addtogroup gazebo_controller @@ -89,11 +90,15 @@ /// \return 0 on success protected: virtual void FiniChild(); + /// \brief Put stereo data to the iface + private: void PutStereoData(); + /// \brief Put camera data to the iface private: void PutCameraData(); /// The camera interface - private: StereoCameraIface *cameraIface; + private: StereoCameraIface *stereoIface; + private: CameraIface *cameraIface; /// The parent sensor private: StereoCameraSensor *myParent; Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -139,7 +139,7 @@ if (this->laserIface->Lock(1)) { // Data timestamp - this->laserIface->data->time = Simulator::Instance()->GetSimTime(); + this->laserIface->data->head.time = Simulator::Instance()->GetSimTime(); // Read out the laser range data this->laserIface->data->min_angle = minAngle; @@ -205,7 +205,7 @@ { // Data timestamp - this->fiducialIface->data->time = Simulator::Instance()->GetSimTime(); + this->fiducialIface->data->head.time = Simulator::Instance()->GetSimTime(); this->fiducialIface->data->count = 0; // TODO: clean this up Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -220,7 +220,7 @@ if (this->myIface->Lock(1)) { // TODO: Data timestamp - this->myIface->data->time = Simulator::Instance()->GetSimTime(); + this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); this->myIface->data->pose.pos.x = this->odomPose[0]; this->myIface->data->pose.pos.y = this->odomPose[1]; Modified: code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -227,7 +227,7 @@ if (this->myIface->Lock(1)) { // TODO: Data timestamp - this->myIface->data->time = Simulator::Instance()->GetSimTime(); + this->myIface->data->head.time = Simulator::Instance()->GetSimTime(); this->myIface->data->pose.pos.x = this->odomPose[0]; this->myIface->data->pose.pos.y = this->odomPose[1]; Modified: code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -170,7 +170,7 @@ this->ptzIface->Lock(1); // Data timestamp - data->time = Simulator::Instance()->GetSimTime(); + data->head.time = Simulator::Instance()->GetSimTime(); data->pan = this->panJoint->GetAngle(); data->tilt = this->tiltJoint->GetAngle(); Modified: code/gazebo/trunk/server/sensors/camera/CameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/CameraSensor.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/sensors/camera/CameraSensor.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -148,9 +148,6 @@ this->pose.rot.x = q.x; this->pose.rot.y = q.y; this->pose.rot.z = q.z; - - if (this->saveFrames) - this->SaveFrame(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -118,6 +118,9 @@ void MonoCameraSensor::UpdateChild(UpdateParams ¶ms) { CameraSensor::UpdateChild(params); + + if (this->saveFrames) + this->SaveFrame(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-04-10 21:24:57 UTC (rev 6296) @@ -42,7 +42,7 @@ #include "CameraManager.hh" #include "StereoCameraSensor.hh" -#define PF_FLOAT Ogre::PF_FLOAT32_R +#define PF_FLOAT Ogre::PF_FLOAT16_R using namespace gazebo; @@ -108,6 +108,7 @@ { this->renderTexture[i] = this->CreateRTT(this->textureName[i], false); this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); + this->renderTarget[i]->setAutoUpdated(false); } // Create the render texture for the depth textures @@ -115,24 +116,21 @@ { this->renderTexture[i] = this->CreateRTT(this->textureName[i], true); this->renderTarget[i] = this->renderTexture[i]->getBuffer()->getRenderTarget(); + this->renderTarget[i]->setAutoUpdated(false); } // Create the camera this->camera = OgreCreator::CreateCamera(this->GetName(), this->nearClip, this->farClip, this->hfov, - this->renderTarget[0]); + this->renderTarget[D_LEFT]); - //this->camera->setUseRenderingDistance(true); - // Hack to make the camera use the right render target too. for (i=0; i<4; i++) { - this->renderTarget[i]->setAutoUpdated(false); - - if (i > 0) + if (i != D_LEFT ) { // Setup the viewport to use the texture - cviewport = this->renderTarget[i]->addViewport(camera); + cviewport = this->renderTarget[i]->addViewport(this->camera); cviewport->setClearEveryFrame(true); cviewport->setOverlaysEnabled(false); cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); @@ -151,7 +149,7 @@ this->depthMaterial = Ogre::MaterialManager::getSingleton().load("Gazebo/DepthMap", Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); - mBuffer = this->renderTexture[0]->getBuffer(0,0); + mBuffer = this->renderTexture[D_LEFT]->getBuffer(0,0); this->textureWidth = mBuffer->getWidth(); this->textureHeight = mBuffer->getHeight(); @@ -189,25 +187,32 @@ Ogre::Viewport *vp = NULL; Ogre::SceneManager *sceneMgr = adapt->sceneMgr; Ogre::Pass *pass; + Ogre::Pass *prev_pass; + Ogre::SceneNode *gridNode = sceneMgr->getSceneNode("__OGRE_GRID_NODE__"); int i; CameraSensor::UpdateChild(params); - // Render the image texture - for (i=0; i<2; i++) - { - this->renderTarget[i]->update(); - } - sceneMgr->_suppressRenderStateChanges(true); + //prev_pass = sceneMgr->getPass(); + // Get pointer to the material pass pass = this->depthMaterial->getBestTechnique()->getPass(0); + if (gridNode) + gridNode->setVisible(false); // Render the depth texture for (i=2; i<4; i++) { + + // 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. + this->camera->setFarClipDistance( this->farClip ); + + Ogre::AutoParamDataSource autoParamDataSource; vp = this->renderTarget[i]->getViewport(0); @@ -240,16 +245,24 @@ renderSys->bindGpuProgramParameters(Ogre::GPT_FRAGMENT_PROGRAM, pass->getFragmentProgramParameters()); } - - // 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. - this->camera->setFarClipDistance( this->farClip ); + this->renderTarget[i]->update(); } sceneMgr->_suppressRenderStateChanges(false); + // Render the image texture + for (i=0; i<2; i++) + { + this->renderTarget[i]->update(); + } + + if (gridNode) + gridNode->setVisible(true); + this->FillBuffers(); + + if (this->saveFrames) + this->SaveFrame(); } //////////////////////////////////////////////////////////////////////////////// @@ -288,7 +301,7 @@ int i; int top,left,right,bottom; - for (i=2; i<3; i++) + for (i=0; i<4; i++) { // Get access to the buffer and make an image and write it to file hardwareBuffer = this->renderTexture[i]->getBuffer(0, 0); @@ -304,14 +317,14 @@ { hardwareBuffer->blitToMemory ( Ogre::Box(left,top,right,bottom), Ogre::PixelBox( this->imageWidth, this->imageHeight, - 1, Ogre::PF_R8G8B8, this->rgbBuffer[i]) + 1, Ogre::PF_B8G8R8, this->rgbBuffer[i]) ); } else { hardwareBuffer->blitToMemory (Ogre::Box(left,top,right,bottom), Ogre::PixelBox( this->imageWidth, this->imageHeight, - 1, PF_FLOAT, this->depthBuffer[i-2]) + 1, Ogre::PF_FLOAT32_R, this->depthBuffer[i-2]) ); } @@ -342,10 +355,9 @@ { for (int j =0; j<this->imageWidth; j++) { - int index = i*this->imageWidth + j; - //float ptr = (float)(*(this->depthBuffer + i * this->imageWidth + j)); + float f = this->depthBuffer[0][i*this->imageWidth+j]; - unsigned char value = this->saveFrameBuffer[i*this->imageWidth+j]; + unsigned char value = f * 255; fwrite( &value, 1, 1, fp ); fwrite( &value, 1, 1, fp ); fwrite( &value, 1, 1, fp ); @@ -487,7 +499,7 @@ Ogre::PixelFormat pf; if (depth) - pf = PF_FLOAT; + pf = Ogre::PF_FLOAT16_R; else pf = Ogre::PF_R8G8B8; Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh 2008-04-10 21:24:57 UTC (rev 6296) @@ -116,6 +116,8 @@ private: unsigned char *rgbBuffer[2]; private: double baseline; + private: Ogre::Camera *depthCamera; + /*private: class StereoCameraListener : public Ogre::RenderTargetListener { Modified: code/gazebo/trunk/worlds/stereocamera.world =================================================================== --- code/gazebo/trunk/worlds/stereocamera.world 2008-04-10 01:30:37 UTC (rev 6295) +++ code/gazebo/trunk/worlds/stereocamera.world 2008-04-10 21:24:57 UTC (rev 6296) @@ -67,7 +67,7 @@ <mass>1.0</mass> <visual> <mesh>unit_box</mesh> - <material>Gazebo/BumpyMetal</material> + <material>Gazebo/Red</material> </visual> </geom:box> </body:box> @@ -107,6 +107,7 @@ <controller:stereo_camera name="stereo_camera_controller"> <interface:stereocamera name="stereo_iface_0" /> + <interface:camera name="camera_iface_0" /> </controller:stereo_camera> </sensor:stereocamera> </body:empty> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-04-16 10:24:25
|
Revision: 6338 http://playerstage.svn.sourceforge.net/playerstage/?rev=6338&view=rev Author: natepak Date: 2008-04-16 10:24:32 -0700 (Wed, 16 Apr 2008) Log Message: ----------- Added masks for displaying laser data Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/Global.hh code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/physics/RayGeom.cc code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/sensors/camera/CameraSensor.cc code/gazebo/trunk/server/sensors/camera/CameraSensor.hh Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-04-16 17:24:18 UTC (rev 6337) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-04-16 17:24:32 UTC (rev 6338) @@ -1342,6 +1342,12 @@ /// Height of image in pixels public: unsigned int height; + /// Far clip distance in meters + public: float farClip; + + /// Near clip distance in meters + public: float nearClip; + /// Left image size public: unsigned int left_rgb_size; Modified: code/gazebo/trunk/server/Global.hh =================================================================== --- code/gazebo/trunk/server/Global.hh 2008-04-16 17:24:18 UTC (rev 6337) +++ code/gazebo/trunk/server/Global.hh 2008-04-16 17:24:32 UTC (rev 6338) @@ -62,7 +62,6 @@ #define GZ_ALL_CAMERA 0xFFFFFFFF #define GZ_LASER_CAMERA 0x00000001 -#define GZ_GUIDATA_CAMERA 0x00000002 #endif Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-04-16 17:24:18 UTC (rev 6337) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-04-16 17:24:32 UTC (rev 6338) @@ -100,9 +100,15 @@ this->cameraIface->Post(); } - /*if (this->stereoIface && this->stereoIface->GetOpenCount() > 0) - this->PutStereoData(); - */ + if (this->stereoIface) + { + this->stereoIface->Lock(1); + if (this->stereoIface->data->head.openCount > 0) + this->PutStereoData(); + this->stereoIface->Unlock(); + + this->stereoIface->Post(); + } } //////////////////////////////////////////////////////////////////////////////// @@ -122,13 +128,13 @@ const float *disp_src; float *disp_dst; - this->stereoIface->Lock(1); - // Data timestamp stereo_data->head.time = Simulator::Instance()->GetSimTime(); stereo_data->width = this->myParent->GetImageWidth(); stereo_data->height = this->myParent->GetImageHeight(); + stereo_data->farClip = this->myParent->GetFarClip(); + stereo_data->nearClip = this->myParent->GetNearClip(); //stereo_data->right_rgb_size = stereo_data->width * stereo_data->height * 3; //stereo_data->left_rgb_size = stereo_data->width * stereo_data->height * 3; @@ -164,8 +170,6 @@ disp_dst = stereo_data->right_disparity; memcpy(disp_dst, disp_src, stereo_data->right_disparity_size); - this->stereoIface->Unlock(); - this->stereoIface->Post(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/RayGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/RayGeom.cc 2008-04-16 17:24:18 UTC (rev 6337) +++ code/gazebo/trunk/server/physics/RayGeom.cc 2008-04-16 17:24:32 UTC (rev 6338) @@ -31,6 +31,7 @@ #include "OgreVisual.hh" #include "OgreDynamicLines.hh" #include "Body.hh" +#include "Global.hh" #include "RayGeom.hh" using namespace gazebo; @@ -57,6 +58,7 @@ this->visualNode->AttachObject(this->line); this->line->setMaterial("Gazebo/BlueEmissive"); + this->line->setVisibilityFlags(GZ_LASER_CAMERA); } this->contactLen = DBL_MAX; Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-04-16 17:24:18 UTC (rev 6337) +++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-04-16 17:24:32 UTC (rev 6338) @@ -24,6 +24,7 @@ * SVN: $Id:$ */ #include <Ogre.h> +#include "Global.hh" #include "GazeboMessage.hh" #include "GazeboError.hh" #include "XMLConfig.hh" @@ -99,6 +100,8 @@ if (obj) this->AttachObject(obj); + obj->setVisibilityFlags(GZ_ALL_CAMERA); + // Set the pose of the scene node this->SetPose(pose); Modified: code/gazebo/trunk/server/sensors/camera/CameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/CameraSensor.cc 2008-04-16 17:24:18 UTC (rev 6337) +++ code/gazebo/trunk/server/sensors/camera/CameraSensor.cc 2008-04-16 17:24:32 UTC (rev 6338) @@ -80,6 +80,13 @@ this->hfov = DTOR(node->GetDouble("hfov",60,0)); + this->visibilityMask = GZ_ALL_CAMERA; + + if (node->GetString("mask","none",0) == "laser") + { + this->visibilityMask ^= GZ_LASER_CAMERA; + } + // Create the directory to store frames if (this->saveFrames) { @@ -114,6 +121,8 @@ this->pitchNode->pitch(Ogre::Degree(0)); this->pitchNode->attachObject(this->camera); + this->camera->getViewport()->setVisibilityMask(this->visibilityMask); + this->saveCount = 0; } @@ -247,4 +256,16 @@ return this->camera; } +//////////////////////////////////////////////////////////////////////////////// +/// Get the near clip distance +double CameraSensor::GetNearClip() +{ + return this->nearClip; +} +//////////////////////////////////////////////////////////////////////////////// +/// Get the far clip distance +double CameraSensor::GetFarClip() +{ + return this->farClip; +} Modified: code/gazebo/trunk/server/sensors/camera/CameraSensor.hh =================================================================== --- code/gazebo/trunk/server/sensors/camera/CameraSensor.hh 2008-04-16 17:24:18 UTC (rev 6337) +++ code/gazebo/trunk/server/sensors/camera/CameraSensor.hh 2008-04-16 17:24:32 UTC (rev 6338) @@ -121,6 +121,12 @@ /// is @e not the same as the depth value. public: double GetZValue(int x, int y); + /// \brief Get the near clip distance + public: double GetNearClip(); + + /// \brief Get the far clip distance + public: double GetFarClip(); + /// \brief Enable or disable saving public: void EnableSaveFrame(bool enable); @@ -151,6 +157,8 @@ protected: unsigned int saveCount; protected: bool saveFrames; protected: std::string savePathname; + + protected: unsigned int visibilityMask; }; /// \} This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <ro...@us...> - 2008-05-11 18:22:38
|
Revision: 6426 http://playerstage.svn.sourceforge.net/playerstage/?rev=6426&view=rev Author: robotos Date: 2008-05-11 18:22:45 -0700 (Sun, 11 May 2008) Log Message: ----------- Negative numbers in update rate will make the rendering or physics update rate dependent one of each other. Change default behaviour of Gazebo from a 1:1 physics:rendering update to a 3:1 In most hardware this makes the simulation feel faster. Modified Paths: -------------- code/gazebo/trunk/doc/config_syntax.html code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/rendering/OgreAdaptor.cc Modified: code/gazebo/trunk/doc/config_syntax.html =================================================================== --- code/gazebo/trunk/doc/config_syntax.html 2008-05-12 00:09:57 UTC (rev 6425) +++ code/gazebo/trunk/doc/config_syntax.html 2008-05-12 01:22:45 UTC (rev 6426) @@ -183,6 +183,7 @@ \verbatim <physics:ode> + <updateRate>0</updateRate> <stepTime>0.03</stepTime> <gravity>0 0 -9.8</gravity> <cfm>0.008</cfm> @@ -193,7 +194,13 @@ - gravity : Vector that describes the direction and magnitude of gravity. - cfm : The global constraint force mixing. - erp : The global error reduction parameter. +- updateRate: number of times per second the physics simulation will try to update. This value is interpreted as: +- 0 : as fast as possible +- more than 0 : Maximum number of times the simulator will update physics each second (limit speed, save CPU, etc.) +- less than 0 : How many rendering update cycles we will wait till we update one physic cycle +The defaults are 0 physics updateRate (as fast as possible) and -3 render updateRate (update 3 times physics prior updating rendering) + \section worldfile_gui GUI Syntax This declaration indicates what GUI to use, and some basic parameters such as size and position of the GUI. @@ -265,6 +272,18 @@ - linearStart : Distance from the camera to begin rendering fog - linearEnd : Distance from the camera to stop rendering fog +The update rate (Frames Per Second) is specified using: +\verbatim + <updateRate>0</updateRate> +\endverbatim +updateRate: number of times per second the rendering will try to update. This value is interpreted as: +- 0 : as fast as possible +- more than 0 : Maximum number of times the simulator will render each second (maximum Framerate) +- less than 0 : How many physics update cycles we will wait till we update one render cycle +The defaults are 0 physics updateRate (as fast as possible) and -3 render updateRate (update 3 times physics updates prior updating rendering) + +Note that if the limits of physics and rendering updating rates are not related, no lineal behaviour can be seen even when the simulation is lineal. + OGRE resources are specified using the following (most people should only change the path information to match where they installed Gazebo): \verbatim <plugins path="/usr/local/lib/OGRE"> Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-05-12 00:09:57 UTC (rev 6425) +++ code/gazebo/trunk/server/Simulator.cc 2008-05-12 01:22:45 UTC (rev 6426) @@ -211,19 +211,23 @@ /// Main simulation loop, when this loop ends the simulation finish void Simulator::MainLoop() { - double maxPhysicsUpdateRate = World::Instance()->GetPhysicsEngine()->GetUpdateRate(); - double maxRenderUpdateRate = OgreAdaptor::Instance()->GetUpdateRate(); + double maxPhysicsUpdateTime = World::Instance()->GetPhysicsEngine()->GetUpdateRate(); + double maxRenderUpdateTime = OgreAdaptor::Instance()->GetUpdateRate(); double step = World::Instance()->GetPhysicsEngine()->GetStepTime(); - if (maxPhysicsUpdateRate == 0) + if (maxPhysicsUpdateTime == 0) gzmsg(2) << "updating the physics at full speed"; + else if (maxPhysicsUpdateTime > 0) + gzmsg(2) << "updating the physics " << 1/maxPhysicsUpdateTime << " times per second"; else - gzmsg(2) << "updating the physics " << 1/maxPhysicsUpdateRate << " times/seconds"; + gzmsg(2) << "updating the physics after " << -1/maxPhysicsUpdateTime << " visualization updates"; - if (maxRenderUpdateRate == 0) + if (maxRenderUpdateTime == 0) gzmsg(2) << "updating the visualization at full speed"; + else if (maxRenderUpdateTime > 0) + gzmsg(2) << "updating the visualization " << 1/maxRenderUpdateTime << " times per second"; else - gzmsg(2) << "updating the visualization " << 1/maxRenderUpdateRate << " times/seconds"; + gzmsg(2) << "updating the visualization " << -1/maxRenderUpdateTime << " times per each physics updates"; std::cout.flush(); while (!this->userQuit) @@ -231,7 +235,8 @@ bool updated=false; //During 3 seconds we want to keep balance between how time pass and update limits - //this is a time slot + //this is a time slot. We don't want to make this too big so we keep changing behaviour + // in new circunstances, nor too small so we have a good measure. if ((this->checkpoint + 3 )< this->GetRealTime()) { this->checkpoint = this->GetRealTime(); @@ -239,11 +244,15 @@ this->renderUpdates = 0; } - // Update the physics engine - // maxPhysicsUpdateRate * physicsUpdates means how much we have _advanced_ in this time slot so far. - // getRealTime - ckeckpoint means our +current point_ in the slot, we only update if our current point has + // Update the physics engine + // If maxPhysicsUpdateTime is positive, we will update when our time has come + // maxPhysicsUpdateTime * physicsUpdates means how much we have _advanced_ in this time slot so far. + // getRealTime - ckeckpoint means our _current point_ in the slot, we only update if our current point has // surpassed what we have already advanced. - if ((this->GetRealTime() - this->checkpoint) > (maxPhysicsUpdateRate * this->physicsUpdates)) + // If maxPhysicsUpdateTime is negative, we update if we are below the times rendering was updated * the multiplier of the userPause + // note that the multiplier defined in the file is inverted when read so for instance -2 will become -0.5 + if ((maxPhysicsUpdateTime >= 0) && ((this->GetRealTime() - this->checkpoint) > (maxPhysicsUpdateTime * this->physicsUpdates)) || \ + ((maxPhysicsUpdateTime < 0) && (this->physicsUpdates < (-maxPhysicsUpdateTime * this->renderUpdates)))) { if (!this->GetUserPause() && !this->GetUserStep() || @@ -265,9 +274,10 @@ this->physicsUpdates++; updated=true; } - + // Update the rendering and gui - if ((this->GetRealTime() - this->checkpoint) > (maxRenderUpdateRate * this->renderUpdates)) + if (((maxRenderUpdateTime >= 0) && ((this->GetRealTime() - this->checkpoint) > (maxRenderUpdateTime * this->renderUpdates))) || \ + ((maxRenderUpdateTime < 0) && (this->renderUpdates < (-maxRenderUpdateTime * this->physicsUpdates)))) { gazebo::OgreAdaptor::Instance()->Render(); this->gui->Update(); @@ -278,12 +288,15 @@ if (!updated) { double nextUpdate; - nextUpdate=MIN(this->renderUpdates+maxRenderUpdateRate, this->renderUpdates+maxPhysicsUpdateRate); - int realStep = static_cast<int>(nextUpdate - this->GetRealTime()); - struct timespec waiting; - waiting.tv_sec=0; - waiting.tv_nsec=realStep *1000000000; //nanoseconds to seconds - //nanosleep(&waiting,0); + nextUpdate=MIN(this->renderUpdates * maxRenderUpdateTime, this->renderUpdates * maxPhysicsUpdateTime); + double realStep = this->checkpoint + nextUpdate - this->GetRealTime(); + if (realStep > 0) + { + struct timespec waiting; + waiting.tv_sec=0; + waiting.tv_nsec=static_cast<long int>(realStep *1000000000); //nanoseconds to seconds + nanosleep(&waiting,0); + } } } } Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-05-12 00:09:57 UTC (rev 6425) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-05-12 01:22:45 UTC (rev 6426) @@ -125,7 +125,7 @@ gzthrow( "missing OGRE Rendering information" ); } - int maxFPS = node->GetInt("updateRate", 0); + int maxFPS = node->GetInt("updateRate", -3); if (maxFPS == 0) this->updateRate=0; else This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-05-18 16:51:46
|
Revision: 6447 http://playerstage.svn.sourceforge.net/playerstage/?rev=6447&view=rev Author: natepak Date: 2008-05-18 16:51:53 -0700 (Sun, 18 May 2008) Log Message: ----------- Updated PTZ stuff Modified Paths: -------------- code/gazebo/trunk/TODO code/gazebo/trunk/player/GazeboDriver.cc code/gazebo/trunk/player/PTZInterface.cc code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh Added Paths: ----------- code/gazebo/trunk/examples/player/ptz/ code/gazebo/trunk/examples/player/ptz/.player code/gazebo/trunk/examples/player/ptz/SConstruct code/gazebo/trunk/examples/player/ptz/camera.cc code/gazebo/trunk/examples/player/ptz/camera.world code/gazebo/trunk/examples/player/ptz/player.cfg code/gazebo/trunk/worlds/models/sonyvid30.model code/gazebo/trunk/worlds/pioneer2dx_camera.world Modified: code/gazebo/trunk/TODO =================================================================== --- code/gazebo/trunk/TODO 2008-05-18 00:50:10 UTC (rev 6446) +++ code/gazebo/trunk/TODO 2008-05-18 23:51:53 UTC (rev 6447) @@ -1,4 +1,6 @@ Open: +- Fix the updateRate tags for rendering and phyics to be more intuitive. +- Shadows draw on multiple surfaces(seem to pass through walls). - Removing the sky results in a black screen - Apply Linear and Angular Damping, see OGREODE sources. - A static geom which is offset does not actually move the geom, so collision Added: code/gazebo/trunk/examples/player/ptz/.player =================================================================== --- code/gazebo/trunk/examples/player/ptz/.player (rev 0) +++ code/gazebo/trunk/examples/player/ptz/.player 2008-05-18 23:51:53 UTC (rev 6447) @@ -0,0 +1,223079 @@ +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +configfile.cc:2156 error : Initialization failed for driver "gazebo" +server.cc:189 error : failed to parse config file player.cfg driver blocks +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +playertcp.cc:283 accepted TCP client 0 on port 6665, fd 7 +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +playertcp.cc:283 accepted TCP client 0 on port 6665, fd 7 +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/ptz/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +playertcp.cc:283 accepted TCP client 0 on port 6665, fd 7 +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning : Unhandled message for driver device=16777343:6665:ptz:0 type=command subtype=1 len=20 + +driver.cc:387 warning... [truncated message content] |
From: <ro...@us...> - 2008-05-23 07:32:10
|
Revision: 6455 http://playerstage.svn.sourceforge.net/playerstage/?rev=6455&view=rev Author: robotos Date: 2008-05-23 07:31:55 -0700 (Fri, 23 May 2008) Log Message: ----------- pedant warnings but avoid deprecation warnings (Ogre hits on it!) Modified Paths: -------------- code/gazebo/trunk/SConstruct code/gazebo/trunk/TODO Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-05-23 14:29:35 UTC (rev 6454) +++ code/gazebo/trunk/SConstruct 2008-05-23 14:31:55 UTC (rev 6455) @@ -84,9 +84,9 @@ # Set the compile mode if env['mode'] == 'debug': - env['CCFLAGS'] += Split('-ggdb -g3') + env['CCFLAGS'] += Split('-ggdb -g2 -Wall -Wno-deprecated') elif env['mode'] == 'profile': - env['CCFLAGS'] += Split('-pg') + env['CCFLAGS'] += Split('-ggdb -g2 -pg') env['LINKFLAGS'] += Split('-pg') elif env['mode'] == 'optimized': env['CCFLAGS'] += Split('-O3') Modified: code/gazebo/trunk/TODO =================================================================== --- code/gazebo/trunk/TODO 2008-05-23 14:29:35 UTC (rev 6454) +++ code/gazebo/trunk/TODO 2008-05-23 14:31:55 UTC (rev 6455) @@ -73,10 +73,6 @@ -Add "Angle" class, will handle ROTD and DTOR. -Templatize Vector, Quaternion, Pose - Replace all return int, with throws -- Find something shorter for: - std::ostringstreaom stream; - stream << "This is an error message of type[" << type << "]\n"; - gzthrow(stream.str()); Completed: @@ -119,4 +115,9 @@ - Add in Pioneer blender models - Add check for libxml2 - Add uninstall (scons -c automatically handles this!) -- Use the Contact information in the Collision Callback. See CVS version of Gazebo. +-- Find something shorter for: + std::ostringstreaom stream; + stream << "This is an error message of type[" << type << "]\n"; + gzthrow(stream.str()); + + Use the Contact information in the Collision Callback. See CVS version of Gazebo. This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-05-29 07:04:26
|
Revision: 6458 http://playerstage.svn.sourceforge.net/playerstage/?rev=6458&view=rev Author: natepak Date: 2008-05-29 07:04:32 -0700 (Thu, 29 May 2008) Log Message: ----------- Updates to the GUI Modified Paths: -------------- code/gazebo/trunk/TODO code/gazebo/trunk/examples/player/camera/camera.cc code/gazebo/trunk/examples/player/camera/player.cfg code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/GLWindow.hh code/gazebo/trunk/server/gui/Toolbar.cc code/gazebo/trunk/server/gui/Toolbar.hh code/gazebo/trunk/worlds/models/sonyvid30.model Modified: code/gazebo/trunk/TODO =================================================================== --- code/gazebo/trunk/TODO 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/TODO 2008-05-29 14:04:32 UTC (rev 6458) @@ -1,5 +1,6 @@ Open: -- Fix the updateRate tags for rendering and phyics to be more intuitive. +- Mouse picking + -Implement in CameraSensor class. - Shadows draw on multiple surfaces(seem to pass through walls). - Removing the sky results in a black screen - Apply Linear and Angular Damping, see OGREODE sources. @@ -15,7 +16,6 @@ - Implement Speech interface - Add in user adjustment of simple solids texture units - Fix sicklms200 PutFiducialData. Breaks when using large angles -- Mouse picking - Paging heightmaps. - Positioning the current heightmap is a big hack Modified: code/gazebo/trunk/examples/player/camera/camera.cc =================================================================== --- code/gazebo/trunk/examples/player/camera/camera.cc 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/examples/player/camera/camera.cc 2008-05-29 14:04:32 UTC (rev 6458) @@ -1,25 +1,36 @@ #include <libplayerc++/playerc++.h> #include <iostream> +using namespace PlayerCc; +using namespace std; + + int main() { // We throw exceptions on creation if we fail try { - using namespace PlayerCc; - using namespace std; - // Create a player client object, using the variables assigned by the // call to parse_args() PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); // Subscribe to the camera proxy CameraProxy cp(&robot, 0); + BlobfinderProxy bp (&robot, 0); + while (true) + { + robot.Read(); + + printf("Blobs[%d]\n",bp.GetCount()); + usleep(10000); + } + } catch (PlayerCc::PlayerError e) { std::cerr << "Error:" << e << std::endl; return -1; } + } Modified: code/gazebo/trunk/examples/player/camera/player.cfg =================================================================== --- code/gazebo/trunk/examples/player/camera/player.cfg 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/examples/player/camera/player.cfg 2008-05-29 14:04:32 UTC (rev 6458) @@ -10,7 +10,17 @@ ( name "gazebo" provides ["camera:0"] - gz_id "camera_iface_1" + gz_id "camera_iface_0" alwayson 1 save 1 ) + +driver +( + name "cmvision" + provides ["blobfinder:0"] + requires ["camera:0"] + + colorfile "colors.txt" +) + Modified: code/gazebo/trunk/server/gui/GLWindow.cc =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.cc 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/server/gui/GLWindow.cc 2008-05-29 14:04:32 UTC (rev 6458) @@ -37,6 +37,7 @@ #include "GazeboMessage.hh" #include "MainMenu.hh" #include "CameraManager.hh" +#include "World.hh" #include "OgreHUD.hh" @@ -86,6 +87,8 @@ this->colormap = fl_colormap; this->windowId = Fl_X::i(this)->xid; + this->mouseDrag = false; + Fl_Window::show(); } @@ -214,6 +217,13 @@ this->middleMousePressed = false; break; } + + if (!this->mouseDrag) + { + //Entity *ent = World::Instance()->GetEntityAt(this->mousePos); + } + + this->mouseDrag = false; } //////////////////////////////////////////////////////////////////////////////// @@ -231,6 +241,8 @@ camera->RotatePitch(d.y * this->rotateAmount); } } + + this->mouseDrag = true; } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/gui/GLWindow.hh =================================================================== --- code/gazebo/trunk/server/gui/GLWindow.hh 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/server/gui/GLWindow.hh 2008-05-29 14:04:32 UTC (rev 6458) @@ -105,6 +105,8 @@ private: std::map<int,int> keys; private: double lastUpdateTime; + + private: bool mouseDrag; }; } Modified: code/gazebo/trunk/server/gui/Toolbar.cc =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.cc 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/server/gui/Toolbar.cc 2008-05-29 14:04:32 UTC (rev 6458) @@ -26,6 +26,8 @@ #include <stdio.h> #include <FL/Fl_Value_Output.H> +#include <FL/Fl_Output.H> +#include <FL/Fl_Button.H> #include "CameraManager.hh" #include "CameraSensor.hh" @@ -53,14 +55,37 @@ sprintf(buffer,"Camera"); } - this->cameraInfoGrp = new Fl_Group(x+10,y+20,w-20,25*3, buffer); + this->cameraInfoGrp = new Fl_Group(x+10,y+20,w-20,25*5, "Camera"); // Camera Info Group this->cameraInfoGrp->box(FL_BORDER_BOX); - // Camera X output + // Camera name output x = this->cameraInfoGrp->x()+20; y = this->cameraInfoGrp->y()+2; + this->cameraName = new Fl_Output(x,y,this->cameraInfoGrp->w()-44,20); + + // Prev camera button + x = this->cameraInfoGrp->x()+2; + y = this->cameraInfoGrp->y()+2; + this->prevCameraButton = new Fl_Button(x,y,16,20,"<"); + this->prevCameraButton->callback( &gazebo::Toolbar::PrevCameraButtonCB, this ); + + // Next camera button + x = this->cameraInfoGrp->x() + this->cameraInfoGrp->w()-22; + y = this->cameraInfoGrp->y()+2; + this->nextCameraButton = new Fl_Button(x,y,16,20,">"); + this->nextCameraButton->callback( &gazebo::Toolbar::NextCameraButtonCB, this ); + + // Camera dimensions + x = this->cameraInfoGrp->x() + 40; + y = this->cameraName->y()+this->cameraName->h()+5; + this->cameraDimensions = new Fl_Output(x,y,this->cameraName->w()-20,20,"WxH"); + + + // Camera X output + x = this->cameraInfoGrp->x() + 20; + y = this->cameraDimensions->y() + this->cameraDimensions->h()+5; this->outputX = new Fl_Value_Output(x,y,60,20,"X"); this->outputX->precision(2); @@ -114,13 +139,19 @@ if (camera != NULL) { - sprintf(buffer,"%s [%d x %d]", camera->GetName().c_str(), camera->GetImageWidth(), camera->GetImageHeight()); + sprintf(buffer,"%s", camera->GetName().c_str()); + if (strcmp(buffer,this->cameraName->value()) != 0) + { + this->cameraName->value(buffer); + } - if (strcmp(buffer,this->cameraInfoGrp->label()) != 0) + sprintf(buffer,"%d x %d", camera->GetImageWidth(), camera->GetImageHeight()); + if (strcmp(buffer,this->cameraDimensions->value()) != 0) { - this->cameraInfoGrp->label(buffer); + this->cameraDimensions->value(buffer); } + Pose3d pose = camera->GetWorldPose(); this->outputX->value(pose.pos.x); @@ -132,3 +163,12 @@ } } +void Toolbar::PrevCameraButtonCB(Fl_Widget * /*w*/, void *data) +{ + CameraManager::Instance()->DecActiveCamera(); +} + +void Toolbar::NextCameraButtonCB(Fl_Widget * /*w*/, void *data) +{ + CameraManager::Instance()->IncActiveCamera(); +} Modified: code/gazebo/trunk/server/gui/Toolbar.hh =================================================================== --- code/gazebo/trunk/server/gui/Toolbar.hh 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/server/gui/Toolbar.hh 2008-05-29 14:04:32 UTC (rev 6458) @@ -29,6 +29,8 @@ #include <FL/Fl_Group.H> class Fl_Value_Output; +class Fl_Output; +class Fl_Button; namespace gazebo { @@ -45,7 +47,14 @@ /// \brief Update the toolbar data public: void Update(); + public: static void PrevCameraButtonCB(Fl_Widget * /*w*/, void *data); + public: static void NextCameraButtonCB(Fl_Widget * /*w*/, void *data); + private: Fl_Group *cameraInfoGrp; + private: Fl_Button *prevCameraButton; + private: Fl_Button *nextCameraButton; + private: Fl_Output *cameraName; + private: Fl_Output *cameraDimensions; private: Fl_Value_Output *outputX; private: Fl_Value_Output *outputY; private: Fl_Value_Output *outputZ; Modified: code/gazebo/trunk/worlds/models/sonyvid30.model =================================================================== --- code/gazebo/trunk/worlds/models/sonyvid30.model 2008-05-28 18:46:20 UTC (rev 6457) +++ code/gazebo/trunk/worlds/models/sonyvid30.model 2008-05-29 14:04:32 UTC (rev 6458) @@ -80,6 +80,13 @@ <material>Gazebo/White</material> </visual> </geom:cylinder> + + <sensor:camera name="sonyvid30_camera_sensor"> + <imageSize>320 240</imageSize> + <hfov>60</hfov> + <nearClip>0.1</nearClip> + <farClip>100</farClip> + </sensor:camera> </body:box> <joint:hinge name="pan_joint"> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-06 13:04:54
|
Revision: 6482 http://playerstage.svn.sourceforge.net/playerstage/?rev=6482&view=rev Author: natepak Date: 2008-06-06 13:04:53 -0700 (Fri, 06 Jun 2008) Log Message: ----------- Fixed the update rates, remove RenderEngine, added Gripper to the player interfaces Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/GazeboDriver.cc code/gazebo/trunk/player/GripperInterface.cc code/gazebo/trunk/player/GripperInterface.hh code/gazebo/trunk/player/Position2dInterface.hh code/gazebo/trunk/player/SConscript code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/gui/StatusBar.cc code/gazebo/trunk/server/physics/HeightmapGeom.cc code/gazebo/trunk/server/physics/TrimeshGeom.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreAdaptor.hh code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/rendering/SConscript code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/worlds/models/sonyvid30.model code/gazebo/trunk/worlds/pioneer2dx.world code/gazebo/trunk/worlds/pioneer2dx_camera.world Removed Paths: ------------- code/gazebo/trunk/server/rendering/RenderEngine.cc code/gazebo/trunk/server/rendering/RenderEngine.hh Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-06-06 20:04:53 UTC (rev 6482) @@ -1348,6 +1348,9 @@ /// Near clip distance in meters public: float nearClip; + /// Horizontal field of view of the camera in radians + public: double hfov; + /// Left image size public: unsigned int left_rgb_size; @@ -1371,6 +1374,7 @@ /// Right disparity (float) public: float right_disparity[GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE]; + }; Modified: code/gazebo/trunk/player/GazeboDriver.cc =================================================================== --- code/gazebo/trunk/player/GazeboDriver.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/player/GazeboDriver.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -38,6 +38,7 @@ #include "Position3dInterface.hh" #include "ActarrayInterface.hh" #include "PTZInterface.hh" +#include "GripperInterface.hh" /* #include "PowerInterface.hh" @@ -299,6 +300,11 @@ ifsrc = new PTZInterface( playerAddr, this, cf, section ); break; + case PLAYER_GRIPPER_CODE: + if (!player_quiet_startup) printf(" a gripper interface.\n"); + ifsrc = new GripperInterface( playerAddr, this, cf, section ); + break; + /* case PLAYER_POWER_CODE: if (!player_quiet_startup) printf(" a power interface.\n"); ifsrc = new PowerInterface( playerAddr, this, cf, section ); @@ -308,12 +314,6 @@ if (!player_quiet_startup) printf(" a sonar interface.\n"); ifsrc = new SonarInterface( playerAddr, this, cf, section ); break; - - case PLAYER_GRIPPER_CODE: - if (!player_quiet_startup) printf(" a gripper interface.\n"); - ifsrc = new GripperInterface( playerAddr, this, cf, section ); - break; - case PLAYER_TRUTH_CODE: if (!player_quiet_startup) printf(" a truth interface.\n"); ifsrc = new TruthInterface( playerAddr, this, cf, section ); Modified: code/gazebo/trunk/player/GripperInterface.cc =================================================================== --- code/gazebo/trunk/player/GripperInterface.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/player/GripperInterface.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -34,12 +34,15 @@ - PLAYER_GRIPPER_REQ_GET_GEOM */ +#include <iostream> #include <math.h> #include "gazebo.h" #include "GazeboDriver.hh" #include "GripperInterface.hh" +using namespace gazebo; + /////////////////////////////////////////////////////////////////////////////// // Constructor GripperInterface::GripperInterface(player_devaddr_t addr, @@ -53,7 +56,7 @@ strcat(this->gz_id, cf->ReadString(section, "gz_id", "")); // Allocate a Position Interface - this->iface = gz_gripper_alloc(); + this->iface = new GripperIface(); this->datatime = -1; } @@ -63,7 +66,7 @@ GripperInterface::~GripperInterface() { // Release this interface - gz_gripper_free(this->iface); + delete this->iface; } /////////////////////////////////////////////////////////////////////////////// @@ -72,87 +75,80 @@ player_msghdr_t *hdr, void *data) { -// This code works with Player CVS -#ifdef PLAYER_GRIPPER_CMD_OPEN - if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, - PLAYER_GRIPPER_CMD_OPEN, this->device_addr)) + if (this->iface->Lock(1)) { - gz_gripper_lock(this->iface, 1); - this->iface->data->cmd = GAZEBO_GRIPPER_CMD_OPEN; - gz_gripper_unlock(this->iface); - return 0; - } - else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, - PLAYER_GRIPPER_CMD_CLOSE, this->device_addr)) - { - gz_gripper_lock(this->iface, 1); - this->iface->data->cmd = GAZEBO_GRIPPER_CMD_CLOSE; - gz_gripper_unlock(this->iface); + // This code works with Player CVS +#ifdef PLAYER_GRIPPER_CMD_OPEN + if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_GRIPPER_CMD_OPEN, this->device_addr)) + { + this->iface->data->cmd = GAZEBO_GRIPPER_CMD_OPEN; + return 0; + } + else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_GRIPPER_CMD_CLOSE, this->device_addr)) + { + this->iface->data->cmd = GAZEBO_GRIPPER_CMD_CLOSE; + return 0; + } + else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_GRIPPER_CMD_STOP, this->device_addr)) + { + this->iface->data->cmd = GAZEBO_GRIPPER_CMD_STOP; + return 0; + } + else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_GRIPPER_CMD_STORE, this->device_addr)) + { + this->iface->data->cmd = GAZEBO_GRIPPER_CMD_STORE; + return 0; + } + else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, + PLAYER_GRIPPER_CMD_RETRIEVE, this->device_addr)) + { + this->iface->data->cmd = GAZEBO_GRIPPER_CMD_RETRIEVE; + return 0; + } + // is it a geometry request? + else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, + PLAYER_GRIPPER_REQ_GET_GEOM, + this->device_addr)) + { + // TODO: implement me - return 0; - } - else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, - PLAYER_GRIPPER_CMD_STOP, this->device_addr)) - { - gz_gripper_lock(this->iface, 1); - this->iface->data->cmd = GAZEBO_GRIPPER_CMD_STOP; - gz_gripper_unlock(this->iface); + player_gripper_geom_t pgeom; - return 0; - } - else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, - PLAYER_GRIPPER_CMD_STORE, this->device_addr)) - { - gz_gripper_lock(this->iface, 1); - this->iface->data->cmd = GAZEBO_GRIPPER_CMD_STORE; - gz_gripper_unlock(this->iface); + pgeom.pose.px = 0; + pgeom.pose.py = 0; + pgeom.pose.pz = 0; + pgeom.pose.proll = 0; + pgeom.pose.ppitch = 0; + pgeom.pose.pyaw = 0; - return 0; - } - else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, - PLAYER_GRIPPER_CMD_RETRIEVE, this->device_addr)) - { - gz_gripper_lock(this->iface, 1); - this->iface->data->cmd = GAZEBO_GRIPPER_CMD_RETRIEVE; - gz_gripper_unlock(this->iface); + pgeom.outer_size.sw = 0; + pgeom.outer_size.sl = 0; + pgeom.outer_size.sh = 0; - return 0; - } - // is it a geometry request? - else if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_REQ, - PLAYER_GRIPPER_REQ_GET_GEOM, - this->device_addr)) - { - // TODO: implement me + pgeom.inner_size.sw = 0; + pgeom.inner_size.sl = 0; + pgeom.inner_size.sh = 0; - player_gripper_geom_t pgeom; + pgeom.num_beams = 2; - pgeom.pose.px = 0; - pgeom.pose.py = 0; - pgeom.pose.pz = 0; - pgeom.pose.proll = 0; - pgeom.pose.ppitch = 0; - pgeom.pose.pyaw = 0; + this->driver->Publish(this->device_addr, respQueue, + PLAYER_MSGTYPE_RESP_ACK, + PLAYER_GRIPPER_REQ_GET_GEOM, + (void*)&pgeom, sizeof(pgeom), NULL); - pgeom.outer_size.sw = 0; - pgeom.outer_size.sl = 0; - pgeom.outer_size.sh = 0; + return 0; + } +#endif - pgeom.inner_size.sw = 0; - pgeom.inner_size.sl = 0; - pgeom.inner_size.sh = 0; - - pgeom.num_beams = 2; - - this->driver->Publish(this->device_addr, respQueue, - PLAYER_MSGTYPE_RESP_ACK, - PLAYER_GRIPPER_REQ_GET_GEOM, - (void*)&pgeom, sizeof(pgeom), NULL); - - return 0; + this->iface->Unlock(); } -#endif + else + this->Unsubscribe(); return -1; } @@ -165,15 +161,15 @@ player_gripper_data_t data; struct timeval ts; - gz_gripper_lock(this->iface, 1); + this->iface->Lock(1); // Only Update when new data is present - if (this->iface->data->time > this->datatime) + if (this->iface->data->head.time > this->datatime) { - this->datatime = this->iface->data->time; + this->datatime = this->iface->data->head.time; - ts.tv_sec = (int) (this->iface->data->time); - ts.tv_usec = (int) (fmod(this->iface->data->time, 1) * 1e6); + ts.tv_sec = (int) (this->iface->data->head.time); + ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); memset(&data, 0, sizeof(data)); @@ -200,13 +196,13 @@ data.state = PLAYER_GRIPPER_STATE_ERROR; #endif - this->driver->Publish( this->device_addr, NULL, + this->driver->Publish( this->device_addr, PLAYER_MSGTYPE_DATA, PLAYER_GRIPPER_DATA_STATE, (void*)&data, sizeof(data), &this->datatime ); } - gz_gripper_unlock(this->iface); + this->iface->Unlock(); } @@ -215,16 +211,21 @@ // GazeboDriver::Subscribe void GripperInterface::Subscribe() { - // Open the interface - if (gz_gripper_open(this->iface, GazeboClient::client, this->gz_id) != 0) + try { - printf("Error Subscribing to Gazebo Position Interface\n"); + this->iface->Open( GazeboClient::client, this->gz_id); } + catch (std::string e) + { + std::cerr << "Error subscribing to Gazebo Gripper Interface\n" + << e << "\n"; + exit(0); + } } /////////////////////////////////////////////////////////////////////////////// // Close a SHM interface. This is called from GazeboDriver::Unsubscribe void GripperInterface::Unsubscribe() { - gz_gripper_close(this->iface); + this->iface->Close(); } Modified: code/gazebo/trunk/player/GripperInterface.hh =================================================================== --- code/gazebo/trunk/player/GripperInterface.hh 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/player/GripperInterface.hh 2008-06-06 20:04:53 UTC (rev 6482) @@ -29,41 +29,54 @@ #include "GazeboInterface.hh" -// Forward declarations -typedef struct gz_gripper gz_gripper_t; - -/// \brief Gripper interface -class GripperInterface : public GazeboInterface +namespace gazebo { - /// @brief Constructor - public: GripperInterface(player_devaddr_t addr, GazeboDriver *driver, - ConfigFile *cf, int section); + /// \addtogroup player_iface + /// \{ + /// \defgroup gripper_player Gripper Interface + /// \brief Gripper Player interface + /// \{ + + class GripperIface; + + /// \brief Gripper interface + class GripperInterface : public GazeboInterface + { + /// \brief Constructor + public: GripperInterface(player_devaddr_t addr, GazeboDriver *driver, + ConfigFile *cf, int section); + + /// \brief Destructor + public: virtual ~GripperInterface(); + + /// \brief Handle all messages. This is called from GazeboDriver + public: virtual int ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data); + + /// \brief Update this interface, publish new info. + public: virtual void Update(); + + /// \brief Open a SHM interface when a subscription is received. \ + /// This is called fromGazeboDriver::Subscribe + public: virtual void Subscribe(); + + /// \brief Close a SHM interface. This is called from \ + /// GazeboDriver::Unsubscribe + public: virtual void Unsubscribe(); + + private: GripperIface *iface; + + /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile + private: char *gz_id; + + /// \brief Timestamp on last data update + private: double datatime; + + }; + + /// \} + /// \} + +} - /// @brief Destructor - public: virtual ~GripperInterface(); - - /// @brief Handle all messages. This is called from GazeboDriver - public: virtual int ProcessMessage(QueuePointer &respQueue, - player_msghdr_t *hdr, void *data); - - /// @brief Update this interface, publish new info. - public: virtual void Update(); - - /// @brief Open a SHM interface when a subscription is received. \ - /// This is called fromGazeboDriver::Subscribe - public: virtual void Subscribe(); - - /// @brief Close a SHM interface. This is called from \ - /// GazeboDriver::Unsubscribe - public: virtual void Unsubscribe(); - - private: gz_gripper_t *iface; - - /// @brief Gazebo id. This needs to match and ID in a Gazebo WorldFile - private: char *gz_id; - - /// @brief Timestamp on last data update - private: double datatime; - -}; #endif Modified: code/gazebo/trunk/player/Position2dInterface.hh =================================================================== --- code/gazebo/trunk/player/Position2dInterface.hh 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/player/Position2dInterface.hh 2008-06-06 20:04:53 UTC (rev 6482) @@ -31,54 +31,54 @@ namespace gazebo { - -/// \addtogroup player_iface -/// \{ -/// \defgroup position2d_player Position2d Interface -/// \brief Position2d Player interface -/// \{ - -// Forward declarations -class PositionIface; - -/// \brief Position2d Player interface -class Position2dInterface : public GazeboInterface -{ - /// \brief Constructor - public: Position2dInterface(player_devaddr_t addr, GazeboDriver *driver, - ConfigFile *cf, int section); - - /// \brief Destructor - public: virtual ~Position2dInterface(); - - /// \brief Handle all messages. This is called from GazeboDriver - public: virtual int ProcessMessage(QueuePointer &respQueue, - player_msghdr_t *hdr, void *data); - - /// \brief Update this interface, publish new info. - public: virtual void Update(); - - /// \brief Open a SHM interface when a subscription is received. - /// This is called fromGazeboDriver::Subscribe - public: virtual void Subscribe(); - - /// \brief Close a SHM interface. This is called from - /// GazeboDriver::Unsubscribe - public: virtual void Unsubscribe(); - - private: PositionIface *iface; - - /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile - private: char *gz_id; - - /// \brief Timestamp on last data update - private: double datatime; -}; - -/// \} -/// \} - - + + /// \addtogroup player_iface + /// \{ + /// \defgroup position2d_player Position2d Interface + /// \brief Position2d Player interface + /// \{ + + // Forward declarations + class PositionIface; + + /// \brief Position2d Player interface + class Position2dInterface : public GazeboInterface + { + /// \brief Constructor + public: Position2dInterface(player_devaddr_t addr, GazeboDriver *driver, + ConfigFile *cf, int section); + + /// \brief Destructor + public: virtual ~Position2dInterface(); + + /// \brief Handle all messages. This is called from GazeboDriver + public: virtual int ProcessMessage(QueuePointer &respQueue, + player_msghdr_t *hdr, void *data); + + /// \brief Update this interface, publish new info. + public: virtual void Update(); + + /// \brief Open a SHM interface when a subscription is received. + /// This is called fromGazeboDriver::Subscribe + public: virtual void Subscribe(); + + /// \brief Close a SHM interface. This is called from + /// GazeboDriver::Unsubscribe + public: virtual void Unsubscribe(); + + private: PositionIface *iface; + + /// \brief Gazebo id. This needs to match and ID in a Gazebo WorldFile + private: char *gz_id; + + /// \brief Timestamp on last data update + private: double datatime; + }; + + /// \} + /// \} + + } #endif Modified: code/gazebo/trunk/player/SConscript =================================================================== --- code/gazebo/trunk/player/SConscript 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/player/SConscript 2008-06-06 20:04:53 UTC (rev 6482) @@ -14,12 +14,13 @@ 'CameraInterface.cc', 'FiducialInterface.cc', 'PTZInterface.cc', - 'ActarrayInterface.cc' + 'ActarrayInterface.cc', + 'GripperInterface.cc' ] -# Position3dInterface.cc PowerInterface.cc SonarInterface.cc GripperInterface.cc GpsInterface.cc ') +# Position3dInterface.cc PowerInterface.cc SonarInterface.ccGpsInterface.cc ') -playerEnv = env.Copy() +playerEnv = env.Clone() for cfg in parseConfigs: print "Checking for ["+cfg+"]" Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/Simulator.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -39,12 +39,13 @@ #include "gazebo.h" #include "PhysicsEngine.hh" #include "OgreAdaptor.hh" -#include "RenderEngine.hh" #include "GazeboMessage.hh" #include "Global.hh" #include "Simulator.hh" +#define MAX_FRAME_RATE 35 + using namespace gazebo; //////////////////////////////////////////////////////////////////////////////// @@ -146,7 +147,7 @@ try { OgreAdaptor::Instance()->Init(rootNode); - this->renderEngine = (RenderEngine *) OgreAdaptor::Instance(); + this->renderEngine = OgreAdaptor::Instance(); //this->renderEngine->Init(); } catch (gazebo::GazeboError e) @@ -204,23 +205,29 @@ { gazebo::World::Instance()->Fini(); } -/* + //////////////////////////////////////////////////////////////////////////////// /// Main simulation loop, when this loop ends the simulation finish void Simulator::MainLoop() { - double step= World::Instance()->GetPhysicsEngine()->GetStepTime(); + double step = World::Instance()->GetPhysicsEngine()->GetStepTime(); + double physicsUpdateRate = World::Instance()->GetPhysicsEngine()->GetUpdateRate(); + double renderUpdateRate = OgreAdaptor::Instance()->GetUpdateRate(); + double currTime; double elapsedTime; this->prevPhysicsTime = this->GetRealTime(); this->prevRenderTime = this->GetRealTime(); + + printf("Period Physics[%f] Render[%f]\n", physicsUpdateRate, renderUpdateRate); while (!this->userQuit) { currTime = this->GetRealTime(); - if ((currTime - this->prevPhysicsTime) >= step) + if (physicsUpdateRate == 0 || + currTime - this->prevPhysicsTime >= 1.0/physicsUpdateRate) { this->simTime += step; @@ -238,13 +245,14 @@ this->pause=true; } - World::Instance()->Update(); //physics + World::Instance()->Update(); this->prevPhysicsTime = this->GetRealTime(); } // Update the rendering - if (currTime - this->prevRenderTime > 0.02) + if (renderUpdateRate == 0 || + currTime - this->prevRenderTime >= 1.0/renderUpdateRate) { this->GetRenderEngine()->Render(); this->prevRenderTime = this->GetRealTime(); @@ -253,110 +261,17 @@ // Update the gui this->gui->Update(); - elapsedTime = (this->GetRealTime()-currTime)*2.0; + elapsedTime = (this->GetRealTime() - currTime); // Wait if we're going too fast - if ( elapsedTime < 0.02 ) + if ( elapsedTime < 1.0/MAX_FRAME_RATE ) { - usleep( (0.02 - elapsedTime) * 1e6 ); + //printf("Too fast Elapsed Time[%f] [%f]\n",elapsedTime, (int)((1.0/MAX_FRAME_RATE - elapsedTime) * 1e6)); + usleep( (int)((1.0/MAX_FRAME_RATE - elapsedTime) * 1e6) ); } } } -*/ -//////////////////////////////////////////////////////////////////////////////// -/// Main simulation loop, when this loop ends the simulation finish -void Simulator::MainLoop() -{ - double maxPhysicsUpdateTime = World::Instance()->GetPhysicsEngine()->GetUpdateRate(); - double maxRenderUpdateTime = this->GetRenderEngine()->GetUpdateRate(); - double step = World::Instance()->GetPhysicsEngine()->GetStepTime(); - - if (maxPhysicsUpdateTime == 0) - gzmsg(2) << "updating the physics at full speed"; - else if (maxPhysicsUpdateTime > 0) - gzmsg(2) << "updating the physics " << 1/maxPhysicsUpdateTime << " times per second"; - else - gzmsg(2) << "updating the physics after " << -1/maxPhysicsUpdateTime << " visualization updates"; - if (maxRenderUpdateTime == 0) - gzmsg(2) << "updating the visualization at full speed"; - else if (maxRenderUpdateTime > 0) - gzmsg(2) << "updating the visualization " << 1/maxRenderUpdateTime << " times per second"; - else - gzmsg(2) << "updating the visualization after " << -1/maxRenderUpdateTime << " physics updates" << std::endl; - std::cout.flush(); - - while (!this->userQuit) - { - bool updated=false; - - //During 3 seconds we want to keep balance between how time pass and update limits - //this is a time slot. We don't want to make this too big so we keep changing behaviour - // in new circunstances, nor too small so we have a good measure. - if ((this->checkpoint + 3 )< this->GetRealTime()) - { - this->checkpoint = this->GetRealTime(); - this->physicsUpdates = 0; - this->renderUpdates = 0; - } - - // Update the physics engine - // If maxPhysicsUpdateTime is positive, we will update when our time has come - // maxPhysicsUpdateTime * physicsUpdates means how much we have _advanced_ in this time slot so far. - // getRealTime - ckeckpoint means our _current point_ in the slot, we only update if our current point has - // surpassed what we have already advanced. - // If maxPhysicsUpdateTime is negative, we update if we are below the times rendering was updated * the multiplier of the userPause - // note that the multiplier defined in the file is inverted when read so for instance -2 will become -0.5 - if (((maxPhysicsUpdateTime >= 0) && ((this->GetRealTime() - this->checkpoint) > (maxPhysicsUpdateTime * this->physicsUpdates))) || \ - ((maxPhysicsUpdateTime < 0) && (this->physicsUpdates < (-maxPhysicsUpdateTime * this->renderUpdates)))) - { - - if ((!this->GetUserPause() && !this->GetUserStep()) || - (this->GetUserStep() && this->GetUserStepInc())) - { - this->simTime += step; - ++this->iterations; - this->pause=false; - this->SetUserStepInc(!this->GetUserStepInc()); - } - else - { - this->pauseTime += step; - this->pause=true; - } - - World::Instance()->Update(); //physics - - ++this->physicsUpdates; - updated=true; - } - - // Update the rendering and gui - if (((maxRenderUpdateTime >= 0) && ((this->GetRealTime() - this->checkpoint) > (maxRenderUpdateTime * this->renderUpdates))) || \ - ((maxRenderUpdateTime < 0) && (this->renderUpdates < (-maxRenderUpdateTime * this->physicsUpdates)))) - { - this->GetRenderEngine()->Render(); - this->gui->Update(); - ++this->renderUpdates; - updated=true; - } - - if (!updated) - { - double nextUpdate; - nextUpdate=MIN(this->renderUpdates * maxRenderUpdateTime, this->renderUpdates * maxPhysicsUpdateTime); - double realStep = this->checkpoint + nextUpdate - this->GetRealTime(); - if (realStep > 0) - { - struct timespec waiting; - waiting.tv_sec=0; - waiting.tv_nsec=static_cast<long int>(realStep *1000000000); //nanoseconds to seconds - nanosleep(&waiting,0); - } - } - } -} - //////////////////////////////////////////////////////////////////////////////// /// Gets our current GUI interface Gui *Simulator::GetUI() const @@ -371,7 +286,7 @@ return this->gazeboConfig; } -RenderEngine *Simulator::GetRenderEngine() const +OgreAdaptor *Simulator::GetRenderEngine() const { return this->renderEngine; } @@ -391,22 +306,7 @@ return this->iterations; } -/* - These methods are needeD? //////////////////////////////////////////////////////////////////////////////// -void Global::SetIterations(unsigned long count) -{ -iterations = count; -} - -//////////////////////////////////////////////////////////////////////////////// -void Global::IncIterations() -{ -iterations++; -} -*/ - -//////////////////////////////////////////////////////////////////////////////// // Get the simulation time double Simulator::GetSimTime() const { Modified: code/gazebo/trunk/server/Simulator.hh =================================================================== --- code/gazebo/trunk/server/Simulator.hh 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/Simulator.hh 2008-06-06 20:04:53 UTC (rev 6482) @@ -43,7 +43,7 @@ class XMLConfig; class XMLConfigNode; class GazeboConfig; - class RenderEngine; + class OgreAdaptor; /// \brief The World /* @@ -83,7 +83,7 @@ public: GazeboConfig *GetGazeboConfig() const; /// \brief Gets our current GUI interface - public: RenderEngine *GetRenderEngine() const; + public: OgreAdaptor *GetRenderEngine() const; /// \brief Returns the state of the simulation true if paused public: bool IsPaused() const; @@ -150,7 +150,7 @@ /// Pointer to the selected Gui private: Gui *gui; - private: RenderEngine *renderEngine; + private: OgreAdaptor *renderEngine; /// Pointer to the selected Gui private: GazeboConfig *gazeboConfig; Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -107,25 +107,36 @@ for (unsigned int i=0; i<16; i++) { double cmdAngle = this->myIface->data->cmd_pos[i]; + double cmdSpeed = this->myIface->data->cmd_speed[i]; + joint = dynamic_cast<HingeJoint*>(this->joints[i]); - if (cmdAngle > joint->GetHighStop()) + if (this->myIface->data->joint_mode[i] == GAZEBO_ACTARRAY_JOINT_POSITION_MODE) { - cmdAngle = joint->GetHighStop(); - } - else if (cmdAngle < joint->GetLowStop()) - { - cmdAngle = joint->GetLowStop(); - } + if (cmdAngle > joint->GetHighStop()) + { + cmdAngle = joint->GetHighStop(); + } + else if (cmdAngle < joint->GetLowStop()) + { + cmdAngle = joint->GetLowStop(); + } - angle = cmdAngle - joint->GetAngle(); + angle = cmdAngle - joint->GetAngle(); - if (fabs(angle) > 0.01) + if (fabs(angle) > 0.01) + { + joint->SetParam( dParamVel, this->gains[i] * angle); + joint->SetParam( dParamFMax, this->forces[i] ); + } + } + else if (this->myIface->data->joint_mode[i] == GAZEBO_ACTARRAY_JOINT_SPEED_MODE) { - joint->SetParam( dParamVel, this->gains[i] * angle); + joint->SetParam( dParamVel, cmdSpeed ); joint->SetParam( dParamFMax, this->forces[i] ); } + this->myIface->data->actuators[i].position = joint->GetAngle(); this->myIface->data->actuators[i].speed = joint->GetAngleRate(); } Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -136,6 +136,8 @@ stereo_data->farClip = this->myParent->GetFarClip(); stereo_data->nearClip = this->myParent->GetNearClip(); + stereo_data->hfov = this->myParent->GetFOV(); + //stereo_data->right_rgb_size = stereo_data->width * stereo_data->height * 3; //stereo_data->left_rgb_size = stereo_data->width * stereo_data->height * 3; Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -160,7 +160,7 @@ this->odomVel[1] = 0.0; this->odomVel[2] = da / params.stepTime; - if (this->enableMotors) + //if (this->enableMotors) { this->joints[LEFT]->SetParam( dParamVel, this->wheelSpeed[LEFT] / this->wheelDiam * 2.0 ); @@ -171,14 +171,14 @@ this->joints[RIGHT]->SetParam( dParamFMax, torque ); } - else + /*else { this->joints[LEFT]->SetParam( dParamVel, 0 ); this->joints[RIGHT]->SetParam( dParamVel, 0 ); this->joints[LEFT]->SetParam( dParamFMax, 0 ); this->joints[RIGHT]->SetParam( dParamFMax, 0 ); - } + }*/ this->PutPositionData(); } @@ -204,8 +204,8 @@ this->enableMotors = this->myIface->data->cmdEnableMotors > 0; - this->wheelSpeed[LEFT] = vr + va * this->wheelSep / 2; - this->wheelSpeed[RIGHT] = vr - va * this->wheelSep / 2; + this->wheelSpeed[LEFT] = vr - va * this->wheelSep / 2; + this->wheelSpeed[RIGHT] = vr + va * this->wheelSep / 2; this->myIface->Unlock(); } Modified: code/gazebo/trunk/server/gui/StatusBar.cc =================================================================== --- code/gazebo/trunk/server/gui/StatusBar.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/gui/StatusBar.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -29,7 +29,7 @@ #include <FL/Fl_Output.H> #include "Simulator.hh" -#include "RenderEngine.hh" +#include "OgreAdaptor.hh" #include "StatusBar.hh" using namespace gazebo; Modified: code/gazebo/trunk/server/physics/HeightmapGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/physics/HeightmapGeom.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -33,7 +33,7 @@ #include "GazeboError.hh" #include "OgreAdaptor.hh" #include "Simulator.hh" -#include "RenderEngine.hh" +#include "OgreAdaptor.hh" #include "Global.hh" #include "Body.hh" #include "HeightmapGeom.hh" @@ -52,7 +52,7 @@ // Destructor HeightmapGeom::~HeightmapGeom() { - OgreAdaptor *ogreAdaptor = (OgreAdaptor *) Simulator::Instance()->GetRenderEngine(); + OgreAdaptor *ogreAdaptor = Simulator::Instance()->GetRenderEngine(); ogreAdaptor->sceneMgr->destroyQuery(this->rayQuery); } @@ -133,12 +133,7 @@ int tileSize; OgreAdaptor *ogreAdaptor; - if (Simulator::Instance()->GetRenderEngine()->GetType() != "ogre") - { - gzthrow("Heighmaps are only supported by the Ogre renderer"); - } - else - ogreAdaptor = (OgreAdaptor *)(Simulator::Instance()->GetRenderEngine()); + ogreAdaptor = Simulator::Instance()->GetRenderEngine(); std::string imageFilename = node->GetString("image","",1); std::string worldTexture = node->GetString("worldTexture","",0); Modified: code/gazebo/trunk/server/physics/TrimeshGeom.cc =================================================================== --- code/gazebo/trunk/server/physics/TrimeshGeom.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/physics/TrimeshGeom.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -32,7 +32,7 @@ #include "TrimeshGeom.hh" #include "GazeboError.hh" #include "Simulator.hh" -#include "RenderEngine.hh" +#include "OgreAdaptor.hh" using namespace gazebo; @@ -109,11 +109,6 @@ int vindex = 0; int iindex = 0; - if (Simulator::Instance()->GetRenderEngine()->GetType() != "ogre") - { - gzthrow("Heighmaps are only supported by the Ogre renderer"); - } - this->meshName = node->GetString("mesh","",1); mesh = Ogre::MeshManager::getSingleton().load(this->meshName,Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME); Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -99,11 +99,7 @@ this->gravity = cnode->GetVector3("gravity",this->gravity); this->stepTime = cnode->GetDouble("stepTime",this->stepTime); - int maxUpdatesSecond = cnode->GetInt("updateRate",0); - if (maxUpdatesSecond == 0) - this->updateRate = 0.0; - else - this->updateRate = 1.0/maxUpdatesSecond; + this->updateRate = cnode->GetDouble("maxUpdateRate", 0, 0); this->globalCFM = cnode->GetDouble("cfm",1e-5,0); this->globalERP = cnode->GetDouble("erp",0.2,0); } Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -68,8 +68,8 @@ this->camera=NULL; this->viewport=NULL; this->root=NULL; - this->type = "ogre"; + this->updateRate = 0; } //////////////////////////////////////////////////////////////////////////////// @@ -124,13 +124,9 @@ { gzthrow( "missing OGRE Rendering information" ); } - - int maxFPS = node->GetInt("updateRate", -3); - if (maxFPS == 0) - this->updateRate=0; - else - this->updateRate = 1.0/maxFPS; + this->updateRate = node->GetDouble("maxUpdateRate",0,0); + ambient.r = node->GetTupleDouble("ambient",0,1.0); ambient.g = node->GetTupleDouble("ambient",1,1.0); ambient.b = node->GetTupleDouble("ambient",2,1.0); @@ -500,6 +496,7 @@ this->root->renderOneFrame(); } +//////////////////////////////////////////////////////////////////////////////// float OgreAdaptor::GetAverageFPS() const { float lastFPS, avgFPS, bestFPS, worstFPS; @@ -509,3 +506,9 @@ } +//////////////////////////////////////////////////////////////////////////////// +/// Get the desired update rate +double OgreAdaptor::GetUpdateRate() +{ + return this->updateRate; +} Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.hh 2008-06-06 20:04:53 UTC (rev 6482) @@ -31,7 +31,6 @@ #include <X11/Xutil.h> #include "SingletonT.hh" -#include "RenderEngine.hh" namespace Ogre { @@ -64,7 +63,7 @@ class Entity; /// \brief Adptor to Ogre3d -class OgreAdaptor : RenderEngine, public SingletonT<OgreAdaptor> +class OgreAdaptor : public SingletonT<OgreAdaptor> { /// \brief Constructor @@ -91,8 +90,12 @@ /// \brief Resize the rendering window public: void ResizeWindow(unsigned int w, unsigned int h); + /// \brief Get the average frame rate public: float GetAverageFPS() const; + /// \brief Get the desired update rate + public: double GetUpdateRate(); + private: void LoadPlugins(); private: void SetupResources(); private: void SetupRenderSystem(bool create); @@ -137,7 +140,9 @@ //private: Vector3 terrainSize; //private: unsigned int terrainVertSize; //private: std::string terrainImage; - + + private: double updateRate; + private: friend class DestroyerT<OgreAdaptor>; private: friend class SingletonT<OgreAdaptor>; Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -140,7 +140,7 @@ // Set the material of the mesh - this->SetMaterial(node->GetString("material",std::string(),1)); + this->SetMaterial(node->GetString("material",std::string(),0)); // Allow the mesh to cast shadows this->SetCastShadows(node->GetBool("castShadows",true,0)); Deleted: code/gazebo/trunk/server/rendering/RenderEngine.cc =================================================================== --- code/gazebo/trunk/server/rendering/RenderEngine.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/rendering/RenderEngine.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -1,51 +0,0 @@ -/* - * 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: Middleman between Rendering engine and Gazebo - * Author: Nate Koenig, Jordi Polo - * Date: 02 Jun 2008 - */ - -#include "RenderEngine.hh" - -using namespace gazebo; - -//////////////////////////////////////////////////////////////////////////////// -/// Constructor -RenderEngine::RenderEngine() -{ -} - -//////////////////////////////////////////////////////////////////////////////// -/// Destructor -RenderEngine::~RenderEngine() -{ -} - -double RenderEngine::GetUpdateRate() const -{ - return this->updateRate; -} - -std::string RenderEngine::GetType() const -{ - return this->type; -} - Deleted: code/gazebo/trunk/server/rendering/RenderEngine.hh =================================================================== --- code/gazebo/trunk/server/rendering/RenderEngine.hh 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/rendering/RenderEngine.hh 2008-06-06 20:04:53 UTC (rev 6482) @@ -1,81 +0,0 @@ -/* - * 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: Middleman between actual rendering engine and Gazebo - * Author: Nate Koenig, Jordi Polo - * Date: 02 Jun 2008 - */ - -#ifndef RENDERENGINE -#define RENDERENGINE - -#include <string> - -namespace gazebo -{ -/// \addtogroup gazebo_rendering -/// \{ - - -class XMLConfigNode; - -/// \brief Rendering engine for the simulation -class RenderEngine -{ - - /// \brief Constructor - public: RenderEngine(); - - /// \brief Destructor - public: virtual ~RenderEngine(); - - /// \brief Closes the present rendering engine, frees the resources - public: virtual void Close() = 0; - - /// \brief Default initialization. - /// Let the rendering engine create the window and rendering context - public: virtual void Init(XMLConfigNode *rootNode) = 0; - - /// \brief Save rendering engine settings - public: virtual void Save(XMLConfigNode *node) = 0; - - /// \brief Render a single frame - public: virtual void Render() = 0; - - /// \brief Resize the rendering window - public: virtual void ResizeWindow(unsigned int w, unsigned int h) = 0; - - /// \brief Gets the average frame per second rate achieved - public: virtual float GetAverageFPS() const = 0; - - /// \brief Gets the minimum time between renders, set by in the file to limit Framerate - public: double GetUpdateRate() const; - - /// \brief Gets the type (name) of this Rendering Engine - public: std::string GetType() const; - - protected: double updateRate; - protected: std::string type; - - -}; - -} -#endif Modified: code/gazebo/trunk/server/rendering/SConscript =================================================================== --- code/gazebo/trunk/server/rendering/SConscript 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/rendering/SConscript 2008-06-06 20:04:53 UTC (rev 6482) @@ -1,11 +1,19 @@ #Import variable Import('env staticObjs sharedObjs headers') -sources = Split('RenderEngine.cc OgreCreator.cc OgreAdaptor.cc OgreFrameListener.cc OgreDynamicRenderable.cc OgreDynamicLines.cc OgreSimpleShape.cc OgreHUD.cc MovableText.cc OgreVisual.cc') +sources = ['OgreCreator.cc', + 'OgreAdaptor.cc', + 'OgreFrameListener.cc', + 'OgreDynamicRenderable.cc', + 'OgreDynamicLines.cc', + 'OgreSimpleShape.cc', + 'OgreHUD.cc', + 'MovableText.cc', + 'OgreVisual.cc' + ] headers.append( ['#/server/rendering/MovableText.hh', - '#/server/rendering/RenderEngine.hh', '#/server/rendering/OgreAdaptor.hh', '#/server/rendering/OgreCreator.hh', '#/server/rendering/OgreDynamicLines.hh', Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-06-06 20:04:53 UTC (rev 6482) @@ -186,9 +186,17 @@ Ogre::Viewport *vp = NULL; Ogre::SceneManager *sceneMgr = adapt->sceneMgr; Ogre::Pass *pass; - Ogre::SceneNode *gridNode = sceneMgr->getSceneNode("__OGRE_GRID_NODE__"); + Ogre::SceneNode *gridNode = NULL; int i; + try + { + gridNode = sceneMgr->getSceneNode("__OGRE_GRID_NODE__"); + } + catch (...) + { + } + CameraSensor::UpdateChild(params); sceneMgr->_suppressRenderStateChanges(true); Modified: code/gazebo/trunk/worlds/models/sonyvid30.model =================================================================== --- code/gazebo/trunk/worlds/models/sonyvid30.model 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/worlds/models/sonyvid30.model 2008-06-06 20:04:53 UTC (rev 6482) @@ -19,14 +19,15 @@ <rpy>0.0 0.0 0.0</rpy> <geom:box name="camera_bottom_geom"> - <xyz>0.0 0.0 0.00</xyz> + <xyz>0.0 0.0 0.02</xyz> <rpy>0 0 90</rpy> <size>0.15 0.14 0.04</size> <mass>0.01</mass> <visual> + <rpy>0 0 -90</rpy> <size>0.15 0.14 0.04</size> - <mesh>unit_box</mesh> + <mesh>sonyvid30_bottom.mesh</mesh> <material>Gazebo/Grey</material> </visual> </geom:box> @@ -37,15 +38,16 @@ <rpy>0.0 0.0 0.0</rpy> <geom:box name="camera_column_geom"> - <xyz>0.0 0.0 0.00</xyz> + <xyz>0.02 0.00 0.0</xyz> <rpy>0 0 90</rpy> - <size>0.04 0.04</size> + <size>0.11 0.11</size> <mass>0.01</mass> <visual> - <size>0.04 0.04 0.04</size> - <mesh>unit_cylinder</mesh> - <material>Gazebo/White</material> + <rpy>0 0 -90</rpy> + <size>0.11 0.11 0.03</size> + <mesh>sonyvid30_mid.mesh</mesh> + <material>Gazebo/Black</material> </visual> </geom:box> </body:box> @@ -56,26 +58,28 @@ <rpy>0.0 0.0 0.0</rpy> <geom:box name="camera_top_geom"> - <xyz>0.0 0.0 0.02</xyz> <rpy>0 0 90</rpy> - <size>0.12 0.12 0.05</size> + <size>0.12 0.119 0.03</size> <mass>0.01</mass> <visual> - <size>0.12 0.12 0.05</size> - <mesh>unit_box</mesh> + <xyz>0.0 0.06 -0.015</xyz> + <rpy>0 0 90</rpy> + <size>0.12 0.119 0.03</size> + <mesh>sonyvid30_top.mesh</mesh> + <!--<material>Gazebo/Grey</material>--> <material>Gazebo/Grey</material> </visual> </geom:box> <geom:cylinder name="camera_lens_geom"> - <xyz>0.042 0.00 0.02</xyz> + <xyz>0.045 -0.002 -0.002</xyz> <rpy>0 90 0</rpy> - <size>0.04 0.04</size> + <size>0.01 0.04</size> <mass>0.01</mass> <visual> - <size>0.04 0.04 0.04</size> + <size>0.025 0.025 0.04</size> <mesh>unit_cylinder</mesh> <material>Gazebo/White</material> </visual> Modified: code/gazebo/trunk/worlds/pioneer2dx.world =================================================================== --- code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-06 20:04:53 UTC (rev 6482) @@ -23,6 +23,7 @@ <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> <erp>0.8</erp> + <maxUpdateRate>0</maxUpdateRate> </physics:ode> <rendering:gui> @@ -36,6 +37,7 @@ <sky> <material>Gazebo/CloudySky</material> </sky> + <maxUpdateRate>0</maxUpdateRate> </rendering:ogre> <!-- Ground Plane --> Modified: code/gazebo/trunk/worlds/pioneer2dx_camera.world =================================================================== --- code/gazebo/trunk/worlds/pioneer2dx_camera.world 2008-06-06 19:16:27 UTC (rev 6481) +++ code/gazebo/trunk/worlds/pioneer2dx_camera.world 2008-06-06 20:04:53 UTC (rev 6482) @@ -140,14 +140,17 @@ <!-- White Directional light --> <model:renderable name="directional_white"> + <xyz>2.0 0 2.0</xyz> <light> - <type>directional</type> - <direction>0 -0.6 -0.4</direction> - <diffuseColor>1.0 1.0 1.0</diffuseColor> - <specularColor>0.2 0.2 0.2</specularColor> - <attenuation>1000 1.0 0.0 0</attenuation> + <type>spot</type> + <range>1000 1000 100</range> + <direction>-0.1 0 -0.9</direction> + <diffuseColor>0.8 0.8 0.8</diffuseColor> + <specularColor>0.1 0.1 0.1</specularColor> + <attenuation>10 1.0 0.0 0</attenuation> </light> </model:renderable> + </gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-13 13:55:00
|
Revision: 6556 http://playerstage.svn.sourceforge.net/playerstage/?rev=6556&view=rev Author: natepak Date: 2008-06-13 13:55:07 -0700 (Fri, 13 Jun 2008) Log Message: ----------- Updates to the grid, removed bad gui classes Modified Paths: -------------- code/gazebo/trunk/TODO code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/rendering/OgreAdaptor.cc code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/worlds/pioneer2dx.world code/gazebo/trunk/worlds/simpleshapes.world Modified: code/gazebo/trunk/TODO =================================================================== --- code/gazebo/trunk/TODO 2008-06-13 20:54:30 UTC (rev 6555) +++ code/gazebo/trunk/TODO 2008-06-13 20:55:07 UTC (rev 6556) @@ -1,4 +1,5 @@ Open: +- Make headless - Mouse picking -Implement in CameraSensor class. - Shadows draw on multiple surfaces(seem to pass through walls). Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-06-13 20:54:30 UTC (rev 6555) +++ code/gazebo/trunk/server/Simulator.cc 2008-06-13 20:55:07 UTC (rev 6556) @@ -31,10 +31,9 @@ //#include <boost/bind.hpp> #include "World.hh" -#include "GuiFactory.hh" #include "Gui.hh" -#include "DummyGui.hh" #include "XMLConfig.hh" +#include "Gui.hh" #include "GazeboConfig.hh" #include "gazebo.h" #include "PhysicsEngine.hh" @@ -134,8 +133,24 @@ //Create and initialize the Gui try { - this->gui=GuiFactory::NewGui(rootNode); - this->gui->Init(); + XMLConfigNode *childNode = rootNode->GetChild("gui"); + + if (childNode) + { + int width = childNode->GetTupleInt("size",0,640); + int height = childNode->GetTupleInt("size",1,480); + int x = childNode->GetTupleInt("pos",0,0); + int y = childNode->GetTupleInt("pos",1,0); + std::string type = childNode->GetString("type","fltk",1); + + gzmsg(1) << "Creating GUI:\n\tType[" << type + << "] Pos[" << x << " " << y + << "] Size[" << width << " " << height << "]\n"; + + // Create the GUI + this->gui = new Gui(x, y, width, height, type+"::Gazebo"); + this->gui->Init(); + } } catch (GazeboError e) { @@ -259,7 +274,8 @@ } // Update the gui - this->gui->Update(); + if (this->gui) + this->gui->Update(); elapsedTime = (this->GetRealTime() - currTime); @@ -386,13 +402,12 @@ } -// Move to GuiFactory? void Simulator::SaveGui(XMLConfigNode *node) { Vector2<int> size; XMLConfigNode* childNode = node->GetChild("gui"); - if (childNode) + if (childNode && this->gui) { size.x = this->gui->GetWidth(); size.y = this->gui->GetHeight(); Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-06-13 20:54:30 UTC (rev 6555) +++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2008-06-13 20:55:07 UTC (rev 6556) @@ -167,6 +167,10 @@ // Create a window for Ogre this->CreateWindow(); + // No window...then exit + if (!this->window) + return; + // Get the SceneManager, in this case a generic one if (node->GetChild("bsp")) { @@ -459,20 +463,23 @@ { Ogre::StringVector paramsVector; Ogre::NameValuePairList params; - Gui *gui=Simulator::Instance()->GetUI(); + Gui *gui = Simulator::Instance()->GetUI(); - paramsVector.push_back( Ogre::StringConverter::toString( (size_t)(gui->GetDisplay()) ) ); - paramsVector.push_back( Ogre::StringConverter::toString((int)gui->GetVisualInfo()->screen)); + if (gui) + { + paramsVector.push_back( Ogre::StringConverter::toString( (size_t)(gui->GetDisplay()) ) ); + paramsVector.push_back( Ogre::StringConverter::toString((int)gui->GetVisualInfo()->screen)); - paramsVector.push_back( Ogre::StringConverter::toString((int)gui->GetWindowId())); - paramsVector.push_back( Ogre::StringConverter::toString((size_t)(gui->GetVisualInfo()))); + paramsVector.push_back( Ogre::StringConverter::toString((int)gui->GetWindowId())); + paramsVector.push_back( Ogre::StringConverter::toString((size_t)(gui->GetVisualInfo()))); - params["parentWindowHandle"] = Ogre::StringConverter::toString(paramsVector); + params["parentWindowHandle"] = Ogre::StringConverter::toString(paramsVector); - this->window = this->root->createRenderWindow( "WindowName", gui->GetWidth(), gui->GetHeight(), false, ¶ms); + this->window = this->root->createRenderWindow( "WindowName", gui->GetWidth(), gui->GetHeight(), false, ¶ms); - this->window->setActive(true); - this->window->setAutoUpdated(true); + this->window->setActive(true); + this->window->setAutoUpdated(true); + } } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-06-13 20:54:30 UTC (rev 6555) +++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-06-13 20:55:07 UTC (rev 6556) @@ -360,7 +360,7 @@ Ogre::Plane plane; plane.d = 49; plane.normal = Ogre::Vector3::NEGATIVE_UNIT_Z; - OgreAdaptor::Instance()->sceneMgr->setSkyPlane(true, plane, material, 50, 8, true, 0.5, 150, 150); + OgreAdaptor::Instance()->sceneMgr->setSkyPlane(true, plane, material, 500, 100, true, 0.5, 150, 150); } else { @@ -386,43 +386,86 @@ { Ogre::ManualObject* gridObject = OgreAdaptor::Instance()->sceneMgr->createManualObject("__OGRE_GRID__"); + gridObject->setCastShadows(false); + Ogre::SceneNode* gridObjectNode = OgreAdaptor::Instance()->sceneMgr->getRootSceneNode()->createChildSceneNode("__OGRE_GRID_NODE__"); Ogre::MaterialPtr gridObjectMaterialX = Ogre::MaterialManager::getSingleton().create("__OGRE_GRID_MATERIAL_X__","debugger1"); gridObjectMaterialX->setReceiveShadows(true); gridObjectMaterialX->getTechnique(0)->setLightingEnabled(true); - gridObjectMaterialX->getTechnique(0)->getPass(0)->setDiffuse(0.4,0.0,0.0,0); - gridObjectMaterialX->getTechnique(0)->getPass(0)->setAmbient(0.4,0.0,0.0); - gridObjectMaterialX->getTechnique(0)->getPass(0)->setSelfIllumination(0.1,0.0,0.0); + gridObjectMaterialX->getTechnique(0)->getPass(0)->setDiffuse(0.2,0.2,0.2,0); + gridObjectMaterialX->getTechnique(0)->getPass(0)->setAmbient(0.2,0.2,0.2); + gridObjectMaterialX->getTechnique(0)->getPass(0)->setSelfIllumination(0.0,0.0,0.0); + gridObjectMaterialX->setReceiveShadows(false); Ogre::MaterialPtr gridObjectMaterialY = Ogre::MaterialManager::getSingleton().create("__OGRE_GRID_MATERIAL_Y__","debugger2"); gridObjectMaterialY->setReceiveShadows(true); gridObjectMaterialY->getTechnique(0)->setLightingEnabled(true); - gridObjectMaterialY->getTechnique(0)->getPass(0)->setDiffuse(0.0,0.0,0.4,0); - gridObjectMaterialY->getTechnique(0)->getPass(0)->setAmbient(0.0,0.0,0.4); - gridObjectMaterialY->getTechnique(0)->getPass(0)->setSelfIllumination(0.0,0.0,0.1); + gridObjectMaterialY->getTechnique(0)->getPass(0)->setDiffuse(0.2,0.2,0.2,0); + gridObjectMaterialY->getTechnique(0)->getPass(0)->setAmbient(0.2,0.2,0.2); + gridObjectMaterialY->getTechnique(0)->getPass(0)->setSelfIllumination(0.0,0.0,0.0); + gridObjectMaterialY->setReceiveShadows(false); float d = 0.01; - float z = 0.01; + float z_bottom = .02; + float height = 1.0; + int dim = 50; + + // Vertex Values for a square box + float v[8][3] = + { + {-1, -1, -1}, {+1, -1, -1}, {+1, +1, -1}, {-1, +1, -1}, + {-1, -1, +1}, {+1, -1, +1}, {+1, +1, +1}, {-1, +1, +1} + }; + + // Indices + int ind[36] = + { + // Bottom Face + 0, 1, 2, + 2, 3, 0, + + // Top Face + 4, 5, 7, + 7, 5, 6, + + // Front Face + 0, 4, 7, + 7, 3, 0, + + // Back face + 5, 1, 6, + 6, 1, 2, + + // Left face + 0, 5, 4, + 0, 1, 5, + + // Right face + 3, 7, 6, + 6, 2, 3 + + + }; + gridObject->begin("__OGRE_GRID_MATERIAL_Y__", Ogre::RenderOperation::OT_TRIANGLE_LIST); - for (int y=-100; y<100; y++) + for (int y=-dim; y<dim; y++) { if (y%10 == 0) d = 0.04; else d = 0.01; - gridObject->position(-100, y-d, z); - gridObject->position(100, y-d, z); - gridObject->position(100, y+d, z); - - gridObject->position(-100, y-d, z); - gridObject->position(100, y+d, z); - gridObject->position(-100, y+d, z); - + // For each face + for (int i = 0; i < 36; i++) + { + gridObject->position( v[ind[i]][0] * dim, + y+v[ind[i]][1] * 0.02, + v[ind[i]][2] * 0.01 ); + } char *name=new char[20]; char *text=new char[10]; @@ -453,25 +496,24 @@ } gridObject->end(); + gridObject->begin("__OGRE_GRID_MATERIAL_X__", Ogre::RenderOperation::OT_TRIANGLE_LIST); - z -= 0.001; - - for (int x=-100; x<100; x++) + for (int x=-dim; x<dim; x++) { if (x%10 == 0) d = 0.04; else d = 0.01; - gridObject->position(x+d, 100, z); - gridObject->position(x-d, 100, z); - gridObject->position(x-d, -100, z); + // For each face + for (int i = 0; i < 36; i++) + { + gridObject->position(x+v[ind[i]][0] * 0.02, + v[ind[i]][1] * dim, + v[ind[i]][2] * 0.01 ); + } - gridObject->position(x+d, -100, z); - gridObject->position(x+d, 100, z); - gridObject->position(x-d, -100, z); - char *name=new char[20]; char *text=new char[10]; Modified: code/gazebo/trunk/worlds/pioneer2dx.world =================================================================== --- code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-13 20:54:30 UTC (rev 6555) +++ code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-13 20:55:07 UTC (rev 6556) @@ -26,17 +26,25 @@ <maxUpdateRate>0</maxUpdateRate> </physics:ode> - <rendering:gui> + <!--<rendering:gui> <type>fltk</type> <size>640 480</size> <pos>0 0</pos> </rendering:gui> + --> <rendering:ogre> <ambient>0.5 0.5 0.5 1.0</ambient> <sky> <material>Gazebo/CloudySky</material> </sky> + + <fog> + <color>1.0 1.0 1.0</color> + <linearStart>10</linearStart> + <linearEnd>100</linearEnd> + </fog> + <maxUpdateRate>0</maxUpdateRate> </rendering:ogre> @@ -52,7 +60,7 @@ <size>2000 2000</size> <segments>10 10</segments> <uvTile>100 100</uvTile> - <material>Gazebo/GrassFloor</material> + <material>Gazebo/Grey</material> </geom:plane> </body:plane> </model:physical> Modified: code/gazebo/trunk/worlds/simpleshapes.world =================================================================== --- code/gazebo/trunk/worlds/simpleshapes.world 2008-06-13 20:54:30 UTC (rev 6555) +++ code/gazebo/trunk/worlds/simpleshapes.world 2008-06-13 20:55:07 UTC (rev 6556) @@ -32,7 +32,6 @@ <sky> <material>Gazebo/CloudySky</material> </sky> - </rendering:ogre> <model:physical name="sphere1_model"> @@ -100,7 +99,7 @@ <size>2000 2000</size> <segments>10 10</segments> <uvTile>100 100</uvTile> - <material>Gazebo/GrassFloor</material> + <material>Gazebo/Grey</material> </geom:plane> </body:plane> </model:physical> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-16 13:04:17
|
Revision: 6589 http://playerstage.svn.sourceforge.net/playerstage/?rev=6589&view=rev Author: natepak Date: 2008-06-16 13:04:26 -0700 (Mon, 16 Jun 2008) Log Message: ----------- Updated the gripper driver and interface Modified Paths: -------------- code/gazebo/trunk/player/GripperInterface.cc code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc code/gazebo/trunk/server/physics/Geom.cc code/gazebo/trunk/server/physics/SliderJoint.cc code/gazebo/trunk/server/rendering/OgreCreator.cc code/gazebo/trunk/server/rendering/OgreCreator.hh code/gazebo/trunk/server/rendering/OgreVisual.cc code/gazebo/trunk/server/rendering/OgreVisual.hh code/gazebo/trunk/worlds/test.world Modified: code/gazebo/trunk/player/GripperInterface.cc =================================================================== --- code/gazebo/trunk/player/GripperInterface.cc 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/player/GripperInterface.cc 2008-06-16 20:04:26 UTC (rev 6589) @@ -77,9 +77,6 @@ if (this->iface->Lock(1)) { - - // This code works with Player CVS -#ifdef PLAYER_GRIPPER_CMD_OPEN if (Message::MatchMessage(hdr, PLAYER_MSGTYPE_CMD, PLAYER_GRIPPER_CMD_OPEN, this->device_addr)) { @@ -143,7 +140,6 @@ return 0; } -#endif this->iface->Unlock(); } Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-06-16 20:04:26 UTC (rev 6589) @@ -91,6 +91,34 @@ // Update the controller void Pioneer2_Gripper::UpdateChild(UpdateParams ¶ms) { + /*double leftPaddleHiStop = this->joints[LEFT]->GetParam(dParamHiStop); + double leftPaddleLoStop = this->joints[LEFT]->GetParam(dParamLoStop); + double rightPaddleHiStop = this->joints[RIGHT]->GetParam(dParamHiStop); + double rightPaddleLoStop = this->joints[RIGHT]->GetParam(dParamLoStop); + + double leftPaddlePos = this->joints[LEFT]->GetPosition(); + double rightPaddlePos = this->joints[RIGHT]->GetPosition(); + */ + + this->myIface->Lock(1); + + switch( this->myIface->data->cmd) + { + case GAZEBO_GRIPPER_CMD_OPEN: + this->joints[RIGHT]->SetParam(dParamVel,0.1); + this->joints[LEFT]->SetParam(dParamVel, -0.1); + break; + + case GAZEBO_GRIPPER_CMD_CLOSE: + this->joints[RIGHT]->SetParam(dParamVel,-0.1); + this->joints[LEFT]->SetParam(dParamVel,0.1); + break; + } + + this->joints[LEFT]->SetParam(dParamFMax,.01); + this->joints[RIGHT]->SetParam(dParamFMax,.01); + + this->myIface->Unlock(); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/Geom.cc =================================================================== --- code/gazebo/trunk/server/physics/Geom.cc 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/server/physics/Geom.cc 2008-06-16 20:04:26 UTC (rev 6589) @@ -27,7 +27,6 @@ #include <sstream> #include "OgreVisual.hh" -#include "OgreCreator.hh" #include "Global.hh" #include "GazeboMessage.hh" #include "ContactParams.hh" @@ -124,7 +123,7 @@ while (childNode) { OgreVisual *visual = new OgreVisual(this->visualNode); - OgreCreator::CreateVisual(childNode,visual); + visual->Load(childNode); this->visuals.push_back(visual); childNode = childNode->GetNext("visual"); } @@ -461,10 +460,22 @@ /// Set the visibility of the joints of this geometry void Geom::ShowJoints(bool show) { + std::vector<OgreVisual*>::iterator iter; + if (show) - this->visualNode->SetTransparency(0.6); + { + for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++) + { + (*iter)->SetTransparency(0.6); + } + } else - this->visualNode->SetTransparency(0); + { + for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++) + { + (*iter)->SetTransparency(0.0); + } + } } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/physics/SliderJoint.cc =================================================================== --- code/gazebo/trunk/server/physics/SliderJoint.cc 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/server/physics/SliderJoint.cc 2008-06-16 20:04:26 UTC (rev 6589) @@ -51,7 +51,7 @@ this->SetAxis(node->GetVector3("axis",Vector3(0,0,1))); double lowStop = node->GetDouble("lowStop",-DBL_MAX,0); - double hiStop = node->GetDouble("hiStop",DBL_MAX,0); + double hiStop = node->GetDouble("highStop",DBL_MAX,0); // Perform this three step ordering to ensure the parameters are set // properly. This is taken from the ODE wiki. Modified: code/gazebo/trunk/server/rendering/OgreCreator.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/server/rendering/OgreCreator.cc 2008-06-16 20:04:26 UTC (rev 6589) @@ -98,71 +98,7 @@ //return visual; } -void OgreCreator::CreateVisual(XMLConfigNode *node, OgreVisual *parent) -{ - std::ostringstream stream; - Pose3d pose; - Vector3 size; - Ogre::Vector3 meshSize; - Ogre::MovableObject *obj = NULL; - std::string meshName = node->GetString("mesh","",1); - - // Read the desired position and rotation of the mesh - pose.pos = node->GetVector3("xyz", Vector3(0,0,0)); - pose.rot = node->GetRotation("rpy", Quatern()); - - try - { - // Create the entity - stream << parent->GetName() << "_ENTITY"; - obj = (Ogre::MovableObject*)parent->GetSceneNode()->getCreator()->createEntity(stream.str(), meshName); - } - catch (Ogre::Exception e) - { - std::cerr << "Ogre Error:" << e.getFullDescription() << "\n"; - gzthrow("Unable to create a mesh from " + meshName); - } - - // Set the pose of the scene node - parent->SetPose(pose); - - // Attach the entity to the node - if (obj) - { - parent->AttachObject(obj); - obj->setVisibilityFlags(GZ_ALL_CAMERA); - meshSize = obj->getBoundingBox().getSize(); - } - - // Get the desired size of the mesh - if (node->GetChild("size") != NULL) - size = node->GetVector3("size",Vector3(1,1,1)); - else - size = Vector3(meshSize.x, meshSize.y, meshSize.z); - - // Get and set teh desired scale of the mesh - if (node->GetChild("scale") != NULL) - { - Vector3 scale = node->GetVector3("scale",Vector3(1,1,1)); - parent->SetScale(scale); - } - else - { - parent->SetScale(Vector3(size.x/meshSize.x, size.y/meshSize.y, size.z/meshSize.z)); - } - - - // Set the material of the mesh - parent->SetMaterial(node->GetString("material",std::string(),1)); - - // Allow the mesh to cast shadows - parent->SetCastShadows(node->GetBool("castShadows",true,0)); - - parent->SetXML(node); -} - - //////////////////////////////////////////////////////////////////////////////// /// Create a light source and attach it to the visual node /// Note that the properties here are not modified afterwards and thus, Modified: code/gazebo/trunk/server/rendering/OgreCreator.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreCreator.hh 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/server/rendering/OgreCreator.hh 2008-06-16 20:04:26 UTC (rev 6589) @@ -63,9 +63,6 @@ ///properties if needed, to avoid this create a child visual node for the plane public: static void CreatePlane(XMLConfigNode *node, OgreVisual *parent); - ///\brief Create a visual entity - public: static void CreateVisual(XMLConfigNode *node, OgreVisual *parent); - /// \brief Create a light source public: static void CreateLight(XMLConfigNode *node, OgreVisual *parent); Modified: code/gazebo/trunk/server/rendering/OgreVisual.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/server/rendering/OgreVisual.cc 2008-06-16 20:04:26 UTC (rev 6589) @@ -76,10 +76,8 @@ } - //////////////////////////////////////////////////////////////////////////////// // Load the visual -/* void OgreVisual::Load(XMLConfigNode *node) { std::ostringstream stream; @@ -130,7 +128,6 @@ if (node->GetChild("scale") != NULL) { Vector3 scale = node->GetVector3("scale",Vector3(1,1,1)); - this->sceneNode->setScale(scale.x, scale.y, scale.z); } else @@ -144,8 +141,9 @@ // Allow the mesh to cast shadows this->SetCastShadows(node->GetBool("castShadows",true,0)); + + this->SetXML(node); } -*/ void OgreVisual::Save() { @@ -285,6 +283,7 @@ if (this->myMaterial.isNull()) { gzmsg(0) << "The visual " << this->sceneNode->getName() << " can't set transparency for this geom without a material\n"; + return; } @@ -300,7 +299,7 @@ while (passIt.hasMoreElements ()) { - sc = this->origMaterial->getTechnique (i)->getPass (j)->getDiffuse (); + sc = this->origMaterial->getTechnique(i)->getPass(j)->getDiffuse(); if (this->transparency >0.0) passIt.peekNext ()->setDepthWriteEnabled (false); @@ -315,14 +314,14 @@ dc.r -= sc.r * this->transparency; dc.g -= sc.g * this->transparency; dc.b -= sc.b * this->transparency; - passIt.peekNext ()->setAmbient (Ogre::ColourValue::Black); + passIt.peekNext()->setAmbient(Ogre::ColourValue::Black); break; case Ogre::SBT_TRANSPARENT_ALPHA: default: dc = sc; dc.a = sc.a * (1.0f - this->transparency); - passIt.peekNext()->setAmbient(this->origMaterial->getTechnique (i)->getPass (j)->getAmbient ()); + passIt.peekNext()->setAmbient(this->origMaterial->getTechnique(i)->getPass(j)->getAmbient()); break; } passIt.peekNext ()->setDiffuse (dc); @@ -351,7 +350,7 @@ //FIXME: Modifying selfIllumination is invasive to the material definition of the user // Choose other effect. - +/* Ogre::Technique *t; Ogre::Material::TechniqueIterator techniqueIt = this->myMaterial->getTechniqueIterator(); while ( techniqueIt.hasMoreElements() ) @@ -372,6 +371,7 @@ passIt.moveNext (); } } + */ } Modified: code/gazebo/trunk/server/rendering/OgreVisual.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreVisual.hh 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/server/rendering/OgreVisual.hh 2008-06-16 20:04:26 UTC (rev 6589) @@ -48,6 +48,9 @@ /// \brief Destructor public: virtual ~OgreVisual(); + /// \brief Load the visual + public: void Load(XMLConfigNode *node); + /// \brief Attach a renerable object to the visual public: void AttachObject( Ogre::MovableObject *obj); Modified: code/gazebo/trunk/worlds/test.world =================================================================== --- code/gazebo/trunk/worlds/test.world 2008-06-16 19:59:31 UTC (rev 6588) +++ code/gazebo/trunk/worlds/test.world 2008-06-16 20:04:26 UTC (rev 6589) @@ -38,7 +38,7 @@ </rendering:ogre> <model:physical name="sphere1_model"> - <xyz>0 0 9.0</xyz> + <xyz>0 0 0.1</xyz> <static>false</static> <body:sphere name="sphere1_body"> <geom:sphere name="sphere1_geom"> @@ -54,17 +54,6 @@ </body:sphere> </model:physical> - <model:physical name="terrain_model"> - <body:heightmap name ="terrain_body"> - <geom:heightmap name="terrain_geom"> - <image>test.jpg</image> - <worldTexture>test.jpg</worldTexture> - <detailTexture>test.jpg</detailTexture> - <size>17 17 1.0</size> - </geom:heightmap> - </body:heightmap> - </model:physical> - <model:physical name="plane1_model"> <xyz>0 0 0</xyz> <static>true</static> @@ -81,8 +70,8 @@ </model:physical> <model:physical name="cam2_model"> - <xyz>-1.62 10.92 0.77</xyz> - <rpy>0 16 -79</rpy> + <xyz>-3.0 0 2.59</xyz> + <rpy>0 28 -2</rpy> <static>true</static> <body:empty name="cam2_body"> @@ -98,120 +87,4 @@ </body:empty> </model:physical> - <model:physical name="pioneer2dx_model1"> - <xyz>-12 0 0.145</xyz> - <rpy>0.0 0.0 0.0</rpy> - - <controller:differential_position2d name="controller1"> - <leftJoint>left_wheel_hinge</leftJoint> - <rightJoint>right_wheel_hinge</rightJoint> - <wheelSeparation>0.34</wheelSeparation> - <wheelDiameter>0.15</wheelDiameter> - <torque>5</torque> - <interface:position name="position_iface_0"/> - </controller:differential_position2d> - - <model:physical name="laser"> - <xyz>0.15 0 0.18</xyz> - - <attach> - <parentBody>chassis_body</parentBody> - <myBody>laser_body</myBody> - </attach> - - <include embedded="true"> - <xi:include href="models/sicklms200.model" /> - </include> - </model:physical> - - <!-- - The include should be last within a model. All previous statements - will override those in the included file - --> - <include embedded="true"> - <xi:include href="models/pioneer2dx.model" /> - </include> - </model:physical> - - <model:physical name="pioneer2dx_model2"> - <xyz>12 0 0.145</xyz> - <rpy>0.0 0.0 180.0</rpy> - - <model:physical name="laser2"> - <xyz>0.15 0 0.18</xyz> - - <attach> - <parentBody>chassis_body</parentBody> - <myBody>laser_body</myBody> - </attach> - - <include embedded="true"> - <xi:include href="models/sicklms200.model" /> - </include> - </model:physical> - - <!-- - The include should be last within a model. All previous statements - will override those in the included file - --> - <include embedded="true"> - <xi:include href="models/pioneer2dx.model" /> - </include> - </model:physical> - - - <model:physical name="pioneer2dx_model3"> - <xyz>0 5 0.145</xyz> - <rpy>0.0 0.0 -90.0</rpy> - - <model:physical name="laser3"> - <xyz>0.15 0 0.18</xyz> - - <attach> - <parentBody>chassis_body</parentBody> - <myBody>laser_body</myBody> - </attach> - - <include embedded="true"> - <xi:include href="models/sicklms200.model" /> - </include> - </model:physical> - - <!-- - The include should be last within a model. All previous statements - will override those in the included file - --> - <include embedded="true"> - <xi:include href="models/pioneer2dx.model" /> - </include> - </model:physical> - - <model:physical name="pioneer2dx_model4"> - <xyz>0 -5 0.145</xyz> - <rpy>0.0 0.0 90.0</rpy> - - <model:physical name="laser4"> - <xyz>0.15 0 0.18</xyz> - - <attach> - <parentBody>chassis_body</parentBody> - <myBody>laser_body</myBody> - </attach> - - <include embedded="true"> - <xi:include href="models/sicklms200.model" /> - </include> - </model:physical> - - <!-- - The include should be last within a model. All previous statements - will override those in the included file - --> - <include embedded="true"> - <xi:include href="models/pioneer2dx.model" /> - </include> - </model:physical> - - - </gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-21 03:54:55
|
Revision: 6656 http://playerstage.svn.sourceforge.net/playerstage/?rev=6656&view=rev Author: natepak Date: 2008-06-20 15:52:19 -0700 (Fri, 20 Jun 2008) Log Message: ----------- GUI now handles multiple windows. Modified Paths: -------------- code/gazebo/trunk/Media/materials/scripts/Gazebo.material code/gazebo/trunk/SConstruct code/gazebo/trunk/server/Model.cc code/gazebo/trunk/server/Model.hh code/gazebo/trunk/server/SConscript code/gazebo/trunk/server/Simulator.cc code/gazebo/trunk/server/Simulator.hh code/gazebo/trunk/server/World.cc code/gazebo/trunk/server/World.hh code/gazebo/trunk/server/controllers/Controller.cc code/gazebo/trunk/server/controllers/Controller.hh code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh code/gazebo/trunk/server/controllers/audio/Audio.cc code/gazebo/trunk/server/controllers/audio/Audio.hh code/gazebo/trunk/server/controllers/camera/SConscript code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh code/gazebo/trunk/server/controllers/factory/Factory.cc code/gazebo/trunk/server/controllers/factory/Factory.hh code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.hh code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.hh code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh code/gazebo/trunk/server/gui/GLWindow.cc code/gazebo/trunk/server/gui/GLWindow.hh code/gazebo/trunk/server/gui/Gui.cc code/gazebo/trunk/server/gui/Gui.hh code/gazebo/trunk/server/gui/MainMenu.cc code/gazebo/trunk/server/gui/MainMenu.hh code/gazebo/trunk/server/gui/SConscript code/gazebo/trunk/server/gui/StatusBar.cc code/gazebo/trunk/server/gui/StatusBar.hh code/gazebo/trunk/server/gui/Toolbar.cc code/gazebo/trunk/server/physics/Body.cc code/gazebo/trunk/server/physics/Body.hh 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/OgreCreator.hh code/gazebo/trunk/server/rendering/OgreFrameListener.cc code/gazebo/trunk/server/rendering/OgreHUD.cc code/gazebo/trunk/server/rendering/OgreHUD.hh code/gazebo/trunk/server/rendering/SConscript code/gazebo/trunk/server/sensors/Sensor.cc code/gazebo/trunk/server/sensors/Sensor.hh code/gazebo/trunk/server/sensors/SensorStub.hh code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.hh code/gazebo/trunk/server/sensors/camera/SConscript code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.hh code/gazebo/trunk/server/sensors/ray/RaySensor.cc code/gazebo/trunk/server/sensors/ray/RaySensor.hh code/gazebo/trunk/worlds/bandit.world code/gazebo/trunk/worlds/factory.world code/gazebo/trunk/worlds/laser.world code/gazebo/trunk/worlds/lights.world code/gazebo/trunk/worlds/models/bandit.model code/gazebo/trunk/worlds/models/sonyvid30.model code/gazebo/trunk/worlds/pioneer2at.world code/gazebo/trunk/worlds/pioneer2dx.world code/gazebo/trunk/worlds/pioneer2dx_camera.world code/gazebo/trunk/worlds/simpleshapes.world code/gazebo/trunk/worlds/terrain.world code/gazebo/trunk/worlds/trimesh.world Added Paths: ----------- code/gazebo/trunk/Media/materials/programs/DepthMap.frag code/gazebo/trunk/Media/materials/programs/DepthMap.vert code/gazebo/trunk/examples/player/blobfinder/ code/gazebo/trunk/examples/player/blobfinder/.player code/gazebo/trunk/examples/player/blobfinder/.sconsign.dblite code/gazebo/trunk/examples/player/blobfinder/SConstruct code/gazebo/trunk/examples/player/blobfinder/camera.cc code/gazebo/trunk/examples/player/blobfinder/camera.world code/gazebo/trunk/examples/player/blobfinder/colors.txt code/gazebo/trunk/examples/player/blobfinder/player.cfg code/gazebo/trunk/server/gui/GLFrame.cc code/gazebo/trunk/server/gui/GLFrame.hh code/gazebo/trunk/server/gui/GLFrameManager.cc code/gazebo/trunk/server/gui/GLFrameManager.hh code/gazebo/trunk/server/rendering/CameraManager.cc code/gazebo/trunk/server/rendering/CameraManager.hh code/gazebo/trunk/server/rendering/OgreCamera.cc code/gazebo/trunk/server/rendering/OgreCamera.hh code/gazebo/trunk/server/rendering/UserCamera.cc code/gazebo/trunk/server/rendering/UserCamera.hh Removed Paths: ------------- code/gazebo/trunk/server/UpdateParams.cc code/gazebo/trunk/server/UpdateParams.hh code/gazebo/trunk/server/sensors/camera/CameraManager.cc code/gazebo/trunk/server/sensors/camera/CameraManager.hh code/gazebo/trunk/server/sensors/camera/CameraSensor.cc code/gazebo/trunk/server/sensors/camera/CameraSensor.hh Added: code/gazebo/trunk/Media/materials/programs/DepthMap.frag =================================================================== --- code/gazebo/trunk/Media/materials/programs/DepthMap.frag (rev 0) +++ code/gazebo/trunk/Media/materials/programs/DepthMap.frag 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,8 @@ +varying float depth; + +void main() +{ + float f = depth; + + gl_FragColor = vec4(f, f, f, f); +} Added: code/gazebo/trunk/Media/materials/programs/DepthMap.vert =================================================================== --- code/gazebo/trunk/Media/materials/programs/DepthMap.vert (rev 0) +++ code/gazebo/trunk/Media/materials/programs/DepthMap.vert 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,11 @@ +uniform float pNear; +uniform float pFar; + +varying float depth; + +void main() +{ + gl_Position = ftransform(); + + depth = gl_Position.z / (pFar - pNear); +} Modified: code/gazebo/trunk/Media/materials/scripts/Gazebo.material =================================================================== --- code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/Media/materials/scripts/Gazebo.material 2008-06-20 22:52:19 UTC (rev 6656) @@ -1,143 +1,26 @@ //////////////////////////////////////////////////////////////////////////////// // Shader Programs -vertex_program Gazebo/AmbientOneTextureVS glsl +vertex_program Gazebo/DepthMapVS glsl { - source AmbientOneTexture.vert -} + source DepthMap.vert -fragment_program Gazebo/AmbientOneTextureFS glsl -{ - source AmbientOneTexture.frag -} - -vertex_program Gazebo/DirectionalLightVS glsl -{ - source DirectionalLight.vert -} - -fragment_program Gazebo/DirectionalLightFS glsl -{ - source DirectionalLight.frag -} - -vertex_program Gazebo/PointLightVS glsl -{ - source PointLight.vert default_params { - param_named_auto lightDiffuse light_diffuse_colour 0 + param_named_auto pNear near_clip_distance + param_named_auto pFar far_clip_distance } } -fragment_program Gazebo/PointLightFS glsl +fragment_program Gazebo/DepthMapFS glsl { - source PointLight.frag - default_params - { - param_named_auto lightAttenuation light_attenuation 0 - param_named_auto lightPositionOS light_position_object_space 0 - param_named_auto cameraPositionOS camera_position_object_space - } -} + source DepthMap.frag -vertex_program Gazebo/SpotLightVS glsl -{ - source SpotLight.vert default_params { - param_named_auto lightDiffuse light_diffuse_colour 0 - param_named_auto lightSpecular light_specular_colour 0 } } -fragment_program Gazebo/SpotLightFS glsl -{ - source SpotLight.frag - default_params - { - param_named_auto lightAttenuation light_attenuation 0 - param_named_auto lightPositionOS light_position_object_space 0 - param_named_auto cameraPositionOS camera_position_object_space - param_name_auto spotExponent light_exponent 0 - } -} - -//////////////////////////////////////////////////////////////////////////////// -// Base material for per-pixel shading -material PerPixelShaded -{ - technique - { - pass Ambient - { - - vertex_program_ref Gazebo/AmbientOneTextureVS - { - } - - fragment_program_ref Gazebo/AmbientOneTextureFS - { - } - } - - pass DirectionalLight - { - iteration once_per_light directional - - ambient 0 0 0 - diffuse 1 1 1 1 - specular 0.5 0.5 0.5 0.5 16 - - scene_blend add - - //vertex_program_ref Gazebo/DirectionalLightVS - //{} - - //fragment_program_ref Gazebo/DirectionalLightFS - //{ } - } - pass PointLight - { - iteration once_per_light point - - ambient 0 0 0 - diffuse 1 1 1 1 - specular 0.5 0.5 0.5 0.5 16 - - scene_blend add - - vertex_program_ref Gazebo/PointLightVS - { - } - - fragment_program_ref Gazebo/PointLightFS - { - } - } - - pass SpotLight - { - iteration once_per_light spot - - ambient 0 0 0 - diffuse 1 1 1 1 - specular 0.5 0.5 0.5 0.5 16 - - scene_blend add - - vertex_program_ref Gazebo/SpotLightVS - { - } - - fragment_program_ref Gazebo/SpotLightFS - { - } - } - } -} - - //: PerPixelShaded material Gazebo/test { technique @@ -241,6 +124,23 @@ } } +material Gazebo/Red +{ + receive_shadows on + + technique + { + pass + { + ambient 1.000000 0.000000 0.000000 1.000000 + diffuse 1.000000 0.000000 0.000000 1.000000 + specular 0.000000 0.000000 0.000000 1.000000 + emissive 0.000000 0.000000 0.000000 0.000000 + lighting on + } + } +} + material Gazebo/RedEmissive { receive_shadows off @@ -467,7 +367,6 @@ } } -//: PerPixelShaded material Gazebo/GrassFloor { //receive_shadows on @@ -679,3 +578,23 @@ } } } + +material Gazebo/DepthMap +{ + technique + { + pass Z-write + { + //cull_hardware none + //cull_software none + //depth_check off + + vertex_program_ref Gazebo/DepthMapVS + { + } + fragment_program_ref Gazebo/DepthMapFS + { + } + } + } +} Modified: code/gazebo/trunk/SConstruct =================================================================== --- code/gazebo/trunk/SConstruct 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/SConstruct 2008-06-20 22:52:19 UTC (rev 6656) @@ -55,7 +55,7 @@ LIBPATH=Split('#libgazebo'), #LIBS=Split('gazebo boost_python') - LIBS=Split('gazebo'), + LIBS=Split('gazebo boost_signals'), LINKFLAGS=Split('-export-dynamic'), TARFLAGS = '-c -z', Added: code/gazebo/trunk/examples/player/blobfinder/.player =================================================================== --- code/gazebo/trunk/examples/player/blobfinder/.player (rev 0) +++ code/gazebo/trunk/examples/player/blobfinder/.player 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,98 @@ +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success +plugins.cc:160 trying to load /home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin... +plugins.cc:164 failed (/home/nate/work/psg/new_gazebo/trunk/examples/player/blobfinder/./libgazeboplugin.so: cannot open shared object file: No such file or directory) + +plugins.cc:175 trying to load /home/nate/local//lib/libgazeboplugin... +plugins.cc:177 success +plugins.cc:217 invoking player_driver_init()... +plugins.cc:233 success Added: code/gazebo/trunk/examples/player/blobfinder/.sconsign.dblite =================================================================== (Binary files differ) Property changes on: code/gazebo/trunk/examples/player/blobfinder/.sconsign.dblite ___________________________________________________________________ Name: svn:mime-type + application/octet-stream Added: code/gazebo/trunk/examples/player/blobfinder/SConstruct =================================================================== --- code/gazebo/trunk/examples/player/blobfinder/SConstruct (rev 0) +++ code/gazebo/trunk/examples/player/blobfinder/SConstruct 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,16 @@ + +# 3rd party packages +parseConfigs=['pkg-config --cflags --libs playerc++'] + + +env = Environment ( + CC = 'g++', + CCFLAGS = Split ('-pthread -pipe -W -Wall -O2'), +) + + +# Parse all the pacakge configurations +for cfg in parseConfigs: + env.ParseConfig(cfg) + +env.Program('camera','camera.cc') Added: code/gazebo/trunk/examples/player/blobfinder/camera.cc =================================================================== --- code/gazebo/trunk/examples/player/blobfinder/camera.cc (rev 0) +++ code/gazebo/trunk/examples/player/blobfinder/camera.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,35 @@ +#include <libplayerc++/playerc++.h> +#include <iostream> + +int main() +{ + // We throw exceptions on creation if we fail + try + { + using namespace PlayerCc; + using namespace std; + + // Create a player client object, using the variables assigned by the + // call to parse_args() + PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); + + // Subscribe to the camera proxy + CameraProxy cp(&robot, 0); + BlobFinderProxy bp(&robot, 0); + + } + catch (PlayerCc::PlayerError e) + { + std::cerr << "Error:" << e << std::endl; + return -1; + } + + while (true) + { + robot.Read(); + + printf("Count[%d]\n", bp.GetCount()); + + usleep(10000); + } +} Property changes on: code/gazebo/trunk/examples/player/blobfinder/camera.cc ___________________________________________________________________ Name: svn:executable + * Added: code/gazebo/trunk/examples/player/blobfinder/camera.world =================================================================== --- code/gazebo/trunk/examples/player/blobfinder/camera.world (rev 0) +++ code/gazebo/trunk/examples/player/blobfinder/camera.world 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,77 @@ +<?xml version="1.0"?> + +<gazebo:world + xmlns:xi="http://www.w3.org/2001/XInclude" + xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" + xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" + xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" + xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" + xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" + xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" + xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" + xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" + xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" + xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" + xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > + + <verbosity>5</verbosity> + + <physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.8</gravity> + <cfm>10e-5</cfm> + <erp>0.8</erp> + </physics:ode> + + <rendering:gui> + <type>fltk</type> + <size>640 480</size> + <pos>0 0</pos> + </rendering:gui> + + <rendering:ogre> + <ambient>0.8 0.8 0.8 1.0</ambient> + <sky> + <material>Gazebo/CloudySky</material> + </sky> + </rendering:ogre> + + <!-- Ground Plane --> + <model:physical name="plane1_model"> + <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> + <static>true</static> + + <body:plane name="plane1_body"> + <geom:plane name="plane1_geom"> + <normal>0 0 1</normal> + <size>2000 2000</size> + <segments>10 10</segments> + <uvTile>100 100</uvTile> + <material>Gazebo/GrassFloor</material> + </geom:plane> + </body:plane> + </model:physical> + + <!-- The camera --> + <model:physical name="cam1_model"> + <xyz>0.07 -1.10 0.5</xyz> + <rpy>0 12 52.5</rpy> + <static>true</static> + + <body:empty name="cam1_body"> + <sensor:camera name="cam1_sensor"> + <imageSize>640 480</imageSize> + <hfov>60</hfov> + <nearClip>0.1</nearClip> + <farClip>100</farClip> + <controller:generic_camera name="cam1_controller"> + <interface:camera name="camera_iface_1"/> + </controller:generic_camera> + </sensor:camera> + </body:empty> + </model:physical> + +</gazebo:world> Added: code/gazebo/trunk/examples/player/blobfinder/colors.txt =================================================================== --- code/gazebo/trunk/examples/player/blobfinder/colors.txt (rev 0) +++ code/gazebo/trunk/examples/player/blobfinder/colors.txt 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,5 @@ +[Colors] +(255, 0, 0) 0.000000 10 Red + +[Thresholds] +( 25:164, 80:120,150:240) Added: code/gazebo/trunk/examples/player/blobfinder/player.cfg =================================================================== --- code/gazebo/trunk/examples/player/blobfinder/player.cfg (rev 0) +++ code/gazebo/trunk/examples/player/blobfinder/player.cfg 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,25 @@ +driver +( + name "gazebo" + provides ["simulation:0"] + plugin "libgazeboplugin" + server_id "default" +) + +driver +( + name "gazebo" + provides ["camera:0"] + gz_id "camera_iface_0" + alwayson 1 + save 1 +) + +driver +( + name "cmvision" + provides ["blobfinder:0"] + requires ["camera:0"] + + colorfile "colors.txt" +) Modified: code/gazebo/trunk/server/Model.cc =================================================================== --- code/gazebo/trunk/server/Model.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/Model.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -254,7 +254,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the model -int Model::Update(UpdateParams ¶ms) +int Model::Update() { std::map<std::string, Body* >::iterator bodyIter; std::map<std::string, Controller* >::iterator contIter; @@ -266,7 +266,7 @@ { if (bodyIter->second) { - bodyIter->second->Update(params); + bodyIter->second->Update(); } } @@ -275,7 +275,7 @@ { if (contIter->second) - contIter->second->Update(params); + contIter->second->Update(); } for (jointIter = this->joints.begin(); jointIter != this->joints.end(); jointIter++) Modified: code/gazebo/trunk/server/Model.hh =================================================================== --- code/gazebo/trunk/server/Model.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/Model.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -33,7 +33,6 @@ #include <vector> #include "MovableText.hh" -#include "UpdateParams.hh" #include "Pose3d.hh" #include "Joint.hh" #include "Entity.hh" @@ -74,7 +73,7 @@ /// \brief Update the model /// \param params Update parameters - public: int Update(UpdateParams ¶ms); + public: int Update(); /// \brief Finalize the model public: int Fini(); Modified: code/gazebo/trunk/server/SConscript =================================================================== --- code/gazebo/trunk/server/SConscript 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/SConscript 2008-06-20 22:52:19 UTC (rev 6656) @@ -16,7 +16,6 @@ 'Time.cc', 'Entity.cc', 'GazeboError.cc', - 'UpdateParams.cc', 'GazeboMessage.cc', 'Model.cc', 'Simulator.cc', @@ -36,7 +35,6 @@ '#/server/StaticPluginRegister.hh', '#/server/StringValue.hh', '#/server/Time.hh', - '#/server/UpdateParams.hh', '#/server/Vector2.hh', '#/server/Vector3.hh', '#/server/World.hh', Modified: code/gazebo/trunk/server/Simulator.cc =================================================================== --- code/gazebo/trunk/server/Simulator.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/Simulator.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -27,8 +27,6 @@ #include <iostream> #include <fstream> #include <sys/time.h> -//#include <boost/signals.hpp> -//#include <boost/bind.hpp> #include "World.hh" #include "Gui.hh" @@ -104,7 +102,7 @@ loaded=false; } - // Load the world file + // Load the world file this->xmlFile=new gazebo::XMLConfig(); try { @@ -114,12 +112,13 @@ { gzthrow("The XML config file can not be loaded, please make sure is a correct file\n" << e); } + XMLConfigNode *rootNode(xmlFile->GetRootNode()); - // Load the messaging system + // Load the messaging system gazebo::GazeboMessage::Instance()->Load(rootNode); - // load the configuration options + // load the configuration options this->gazeboConfig=new gazebo::GazeboConfig(); try { @@ -130,7 +129,10 @@ gzthrow("Error loading the Gazebo configuration file, check the .gazeborc file on your HOME directory \n" << e); } - //Create and initialize the Gui + // Load the Ogre rendering system + OgreAdaptor::Instance()->Load(rootNode); + + // Create and initialize the Gui try { XMLConfigNode *childNode = rootNode->GetChild("gui"); @@ -149,7 +151,6 @@ // Create the GUI this->gui = new Gui(x, y, width, height, type+"::Gazebo"); - this->gui->Init(); } } catch (GazeboError e) @@ -157,20 +158,24 @@ gzthrow( "Error loading the GUI\n" << e); } - //Initialize RenderEngine //create factory - //this->renderEngine = new OgreAdaptor(); + //Initialize RenderEngine try { OgreAdaptor::Instance()->Init(rootNode); this->renderEngine = OgreAdaptor::Instance(); - //this->renderEngine->Init(); } catch (gazebo::GazeboError e) { gzthrow("Failed to Initialize the Rendering engine subsystem\n" << e ); } - //Create the world + // Initialize the GUI + if (this->gui) + { + this->gui->Init(); + } + + //Create the world try { gazebo::World::Instance()->Load(rootNode, serverId); @@ -184,6 +189,19 @@ } //////////////////////////////////////////////////////////////////////////////// +/// Initialize the simulation +int Simulator::Init() +{ + + this->startTime = this->GetWallTime(); + + //Initialize the world + if (gazebo::World::Instance()->Init() != 0) + return -1; + return 0; +} + +//////////////////////////////////////////////////////////////////////////////// /// Save the world configuration file void Simulator::Save(const std::string& filename) { @@ -201,20 +219,7 @@ } } - //////////////////////////////////////////////////////////////////////////////// -/// Initialize the simulation -int Simulator::Init() -{ - this->startTime = this->GetWallTime(); - - //Initialize the world - if (gazebo::World::Instance()->Init() != 0) - return -1; - return 0; -} - -//////////////////////////////////////////////////////////////////////////////// /// Finalize the simulation void Simulator::Fini( ) { @@ -269,13 +274,15 @@ if (renderUpdateRate == 0 || currTime - this->prevRenderTime >= renderUpdatePeriod) { - this->GetRenderEngine()->Render(); - this->prevRenderTime = this->GetRealTime(); + //this->GetRenderEngine()->Render(); + //this->prevRenderTime = this->GetRealTime(); } // Update the gui if (this->gui) + { this->gui->Update(); + } elapsedTime = (this->GetRealTime() - currTime); Modified: code/gazebo/trunk/server/Simulator.hh =================================================================== --- code/gazebo/trunk/server/Simulator.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/Simulator.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -120,7 +120,6 @@ public: void LoadGui(XMLConfigNode *rootNode); public: void SaveGui(XMLConfigNode *node); - //User Iteractions /// \brief Simulator finished by the user public: void SetUserQuit(); Deleted: code/gazebo/trunk/server/UpdateParams.cc =================================================================== --- code/gazebo/trunk/server/UpdateParams.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/UpdateParams.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -1,42 +0,0 @@ -/* - * 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: Parameters for each update cycle - * Author: Nate Koenig - * Date: 8 May 2007 - * SVN: $Id$ - */ - -#include "UpdateParams.hh" - -using namespace gazebo; - -//////////////////////////////////////////////////////////////////////////////// -/// Constructor -UpdateParams::UpdateParams() -{ - this->stepTime = 0.02; -} - -//////////////////////////////////////////////////////////////////////////////// -/// Destructor -UpdateParams::~UpdateParams() -{ -} Deleted: code/gazebo/trunk/server/UpdateParams.hh =================================================================== --- code/gazebo/trunk/server/UpdateParams.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/UpdateParams.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -1,52 +0,0 @@ -/* - * 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: Parameters for each update cycle - * Author: Nate Koenig - * Date: 8 May 2007 - * SVN: $Id$ - */ - -#ifndef UPDATEPARAMS_HH -#define UPDATEPARAMS_HH - -namespace gazebo -{ -/// \addtogroup gazebo_server -/// \brief Parameters used during update cycle -/// \{ - -/// \brief Parameters used during update cycle -class UpdateParams -{ - /// \brief Constructor - public: UpdateParams(); - - /// \brief Destructor - public: virtual ~UpdateParams(); - - /// \brief Elapsed time since last update - public: double stepTime; -}; - -/// \} -} - -#endif Modified: code/gazebo/trunk/server/World.cc =================================================================== --- code/gazebo/trunk/server/World.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/World.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -37,7 +37,6 @@ #include "Model.hh" #include "Simulator.hh" #include "gazebo.h" -#include "UpdateParams.hh" #include "World.hh" #include "Geom.hh" @@ -168,18 +167,15 @@ // Update the world int World::Update() { - UpdateParams params; std::vector< Model* >::iterator miter; std::vector< Model* >::iterator miter2; - params.stepTime = this->physicsEngine->GetStepTime(); - // Update all the models for (miter=this->models.begin(); miter!=this->models.end(); miter++) { if (*miter) { - (*miter)->Update(params); + (*miter)->Update(); } } @@ -476,6 +472,7 @@ return this->showPhysics; } + //////////////////////////////////////////////////////////////////////////////// /// Set whether to show the joints void World::SetShowPhysics(bool show) Modified: code/gazebo/trunk/server/World.hh =================================================================== --- code/gazebo/trunk/server/World.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/World.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -113,41 +113,41 @@ /// \brief register a geom This method is no more than a manually-done signal system public: void RegisterGeom(Geom *geom); - // User control of how the world is viewed - // If this section grows it may become a model-view structure ... - /// \brief Return true if the bounding boxes should be shown - public: bool GetShowBoundingBoxes(); + // User control of how the world is viewed + // If this section grows it may become a model-view structure ... + /// \brief Return true if the bounding boxes should be shown + public: bool GetShowBoundingBoxes(); - /// \brief Set if the bounding boxes should be shown - public: void SetShowBoundingBoxes(bool show); + /// \brief Set if the bounding boxes should be shown + public: void SetShowBoundingBoxes(bool show); - /// \brief Get wheter to show the joints - public: bool GetShowJoints(); + /// \brief Get wheter to show the joints + public: bool GetShowJoints(); - /// \brief Set whether to show the joints - public: void SetShowJoints(bool show); + /// \brief Set whether to show the joints + public: void SetShowJoints(bool show); - /// \brief Set to view as wireframe - public: void SetWireframe( bool wire ); + /// \brief Set to view as wireframe + public: void SetWireframe( bool wire ); - /// \brief Get whether to view as wireframe - public: bool GetWireframe(); + /// \brief Get whether to view as wireframe + public: bool GetWireframe(); - /// \brief Set to view as wireframe - public: void SetShowPhysics( bool show ); + /// \brief Set to view as wireframe + public: void SetShowPhysics( bool show ); - /// \brief Get whether to view as wireframe - public: bool GetShowPhysics(); + /// \brief Get whether to view as wireframe + public: bool GetShowPhysics(); - /// Set to true to show bounding boxes - private: bool showBoundingBoxes; + /// Set to true to show bounding boxes + private: bool showBoundingBoxes; - /// Set to true to show joints - private: bool showJoints; + /// Set to true to show joints + private: bool showJoints; - private: bool showPhysics; + private: bool showPhysics; - private: bool wireframe; + private: bool wireframe; /// \brief Load a model Modified: code/gazebo/trunk/server/controllers/Controller.cc =================================================================== --- code/gazebo/trunk/server/controllers/Controller.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/Controller.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -114,7 +114,15 @@ this->xmlNode=node; } + //////////////////////////////////////////////////////////////////////////////// +/// Initialize the controller. Called once on startup. +void Controller::Init() +{ + this->InitChild(); +} + +//////////////////////////////////////////////////////////////////////////////// /// Save the controller. void Controller::Save() { @@ -122,15 +130,7 @@ this->SaveChild(this->xmlNode); } - //////////////////////////////////////////////////////////////////////////////// -/// Initialize the controller. Called once on startup. -void Controller::Init() -{ - this->InitChild(); -} - -//////////////////////////////////////////////////////////////////////////////// // Reset the controller void Controller::Reset() { @@ -139,11 +139,11 @@ //////////////////////////////////////////////////////////////////////////////// /// Update the controller. Called every cycle. -void Controller::Update(UpdateParams ¶ms) +void Controller::Update() { if (lastUpdate + updatePeriod <= Simulator::Instance()->GetSimTime()) { - this->UpdateChild(params); + this->UpdateChild(); lastUpdate = Simulator::Instance()->GetSimTime(); } } Modified: code/gazebo/trunk/server/controllers/Controller.hh =================================================================== --- code/gazebo/trunk/server/controllers/Controller.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/Controller.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -30,7 +30,6 @@ #include <string> #include <vector> -#include "UpdateParams.hh" namespace gazebo { @@ -69,7 +68,7 @@ /// \brief Update the controller. Called every cycle. /// \param params Parameters to the update cycle - public: void Update(UpdateParams ¶ms); + public: void Update(); /// \brief Finialize the controller. Called once on completion. public: void Fini(); @@ -87,7 +86,7 @@ protected: virtual void ResetChild() {return;} /// \brief Update function for the child class - protected: virtual void UpdateChild(UpdateParams &/*params*/) {return;} + protected: virtual void UpdateChild() {return;} /// \brief Fini function for the child class protected: virtual void FiniChild() {return;} Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -94,7 +94,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Bandit_Actarray::UpdateChild(UpdateParams ¶ms) +void Bandit_Actarray::UpdateChild() { HingeJoint *joint = NULL; float angle; Modified: code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh =================================================================== --- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -73,7 +73,7 @@ /// Update the controller /// \return 0 on success - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// Finalize the controller /// \return 0 on success Modified: code/gazebo/trunk/server/controllers/audio/Audio.cc =================================================================== --- code/gazebo/trunk/server/controllers/audio/Audio.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/audio/Audio.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -100,7 +100,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void AudioController::UpdateChild(UpdateParams ¶ms) +void AudioController::UpdateChild() { OgreAL::Sound *sound; Modified: code/gazebo/trunk/server/controllers/audio/Audio.hh =================================================================== --- code/gazebo/trunk/server/controllers/audio/Audio.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/audio/Audio.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -74,7 +74,7 @@ protected: virtual void ResetChild(); /// Update the controller - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// Finalize the controller protected: virtual void FiniChild(); Modified: code/gazebo/trunk/server/controllers/camera/SConscript =================================================================== --- code/gazebo/trunk/server/controllers/camera/SConscript 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/camera/SConscript 2008-06-20 22:52:19 UTC (rev 6656) @@ -1,7 +1,10 @@ #Import variable Import('env staticObjs sharedObjs') -dirs = Split('generic stereo') +dirs = [ + 'generic', + 'stereo' + ] for subdir in dirs : SConscript('%s/SConscript' % subdir) Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -35,7 +35,7 @@ #include "gazebo.h" #include "GazeboError.hh" #include "ControllerFactory.hh" -#include "CameraSensor.hh" +#include "OgreCamera.hh" #include "Generic_Camera.hh" using namespace gazebo; @@ -47,7 +47,7 @@ Generic_Camera::Generic_Camera(Entity *parent) : Controller(parent) { - this->myParent = dynamic_cast<CameraSensor*>(this->parent); + this->myParent = dynamic_cast<OgreCamera*>(this->parent); if (!this->myParent) gzthrow("Generic_Camera controller requires a Camera Sensor as its parent"); @@ -77,7 +77,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Generic_Camera::UpdateChild(UpdateParams ¶ms) +void Generic_Camera::UpdateChild() { this->PutCameraData(); } @@ -122,7 +122,7 @@ assert (data->image_size <= sizeof(data->image)); // Copy the pixel data to the interface - src = this->myParent->GetImageData(); + //src = this->myParent->GetImageData(); dst = data->image; memcpy(dst, src, data->image_size); Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -34,7 +34,7 @@ namespace gazebo { class CameraIface; - class CameraSensor; + class OgreCamera; /// @addtogroup gazebo_controller /// @{ @@ -84,7 +84,7 @@ /// \brief Update the controller /// \return 0 on success - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// \brief Finalize the controller /// \return 0 on success @@ -97,7 +97,7 @@ private: CameraIface *cameraIface; /// The parent sensor - private: CameraSensor *myParent; + private: OgreCamera *myParent; }; Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -40,7 +40,7 @@ using namespace gazebo; -GZ_REGISTER_STATIC_CONTROLLER("stereo_camera", Stereo_Camera); +GZ_REGISTER_STATIC_CONTROLLER("stereocamera", Stereo_Camera); //////////////////////////////////////////////////////////////////////////////// // Constructor @@ -85,10 +85,8 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Stereo_Camera::UpdateChild(UpdateParams ¶ms) +void Stereo_Camera::UpdateChild() { - - if (this->cameraIface) { this->cameraIface->Lock(1); Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -34,7 +34,6 @@ { class CameraIface; class StereoCameraIface; - class CameraSensor; /// @addtogroup gazebo_controller /// @{ @@ -84,7 +83,7 @@ /// \brief Update the controller /// \return 0 on success - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// \brief Finalize the controller /// \return 0 on success Modified: code/gazebo/trunk/server/controllers/factory/Factory.cc =================================================================== --- code/gazebo/trunk/server/controllers/factory/Factory.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/factory/Factory.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -80,7 +80,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Factory::UpdateChild(UpdateParams ¶ms) +void Factory::UpdateChild() { // If there is a string, then add the contents to the world if (strcmp((const char*)this->factoryIface->data->newModel,"")!=0) Modified: code/gazebo/trunk/server/controllers/factory/Factory.hh =================================================================== --- code/gazebo/trunk/server/controllers/factory/Factory.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/factory/Factory.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -73,7 +73,7 @@ /// Update the controller /// \return 0 on success - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// Finalize the controller /// \return 0 on success Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -89,7 +89,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Pioneer2_Gripper::UpdateChild(UpdateParams ¶ms) +void Pioneer2_Gripper::UpdateChild() { /*double leftPaddleHiStop = this->joints[LEFT]->GetParam(dParamHiStop); double leftPaddleLoStop = this->joints[LEFT]->GetParam(dParamLoStop); Modified: code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh =================================================================== --- code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/gripper/pioneer2/Pioneer2_Gripper.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -76,7 +76,7 @@ /// Update the controller /// \return 0 on success - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// Finalize the controller /// \return 0 on success Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -90,7 +90,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void SickLMS200_Laser::UpdateChild(UpdateParams ¶ms) +void SickLMS200_Laser::UpdateChild() { bool opened = false; Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.hh =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -84,7 +84,7 @@ /// \brief Update the controller /// \return 0 on success - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// \brief Finalize the controller /// \return 0 on success Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -33,6 +33,7 @@ #include "Simulator.hh" #include "gazebo.h" #include "GazeboError.hh" +#include "PhysicsEngine.hh" #include "ControllerFactory.hh" #include "Differential_Position2d.hh" @@ -93,17 +94,6 @@ } //////////////////////////////////////////////////////////////////////////////// -// Load the controller -void Differential_Position2d::SaveChild(XMLConfigNode *node) -{ - node->SetValue("wheelSeparation",this->wheelSep); - node->SetValue("wheelDiameter",this->wheelDiam); - node->SetValue("torque",this->torque); -// node->SetValue("leftJoint",this->XMLData["leftJointName"]); -// node->SetValue("rightJoint",this->XMLData["rightJointName"]); -} - -//////////////////////////////////////////////////////////////////////////////// // Initialize the controller void Differential_Position2d::InitChild() { @@ -115,8 +105,21 @@ this->odomVel[0] = 0.0; this->odomVel[1] = 0.0; this->odomVel[2] = 0.0; + } +//////////////////////////////////////////////////////////////////////////////// +// Load the controller +void Differential_Position2d::SaveChild(XMLConfigNode *node) +{ + node->SetValue("wheelSeparation",this->wheelSep); + node->SetValue("wheelDiameter",this->wheelDiam); + node->SetValue("torque",this->torque); +// node->SetValue("leftJoint",this->XMLData["leftJointName"]); +// node->SetValue("rightJoint",this->XMLData["rightJointName"]); +} + + void Differential_Position2d::ResetChild() { // Reset odometric pose @@ -131,21 +134,25 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Differential_Position2d::UpdateChild(UpdateParams ¶ms) +void Differential_Position2d::UpdateChild() { // TODO: Step should be in a parameter of this function double wd, ws; double d1, d2; double dr, da; + double stepTime; this->GetPositionCmd(); wd = this->wheelDiam; ws = this->wheelSep; + + stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime(); + // Distance travelled by front wheels - d1 = params.stepTime * wd / 2 * this->joints[LEFT]->GetAngleRate(); - d2 = params.stepTime * wd / 2 * this->joints[RIGHT]->GetAngleRate(); + d1 = stepTime * wd / 2 * this->joints[LEFT]->GetAngleRate(); + d2 = stepTime * wd / 2 * this->joints[RIGHT]->GetAngleRate(); dr = (d1 + d2) / 2; da = (d2 - d1) / ws; @@ -156,9 +163,9 @@ this->odomPose[2] += da; // Compute odometric instantaneous velocity - this->odomVel[0] = dr / params.stepTime; + this->odomVel[0] = dr / stepTime; this->odomVel[1] = 0.0; - this->odomVel[2] = da / params.stepTime; + this->odomVel[2] = da / stepTime; //if (this->enableMotors) { Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -84,7 +84,7 @@ protected: void ResetChild(); /// Update the controller - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// Finalize the controller protected: virtual void FiniChild(); @@ -121,6 +121,7 @@ private: HingeJoint *joints[2]; + private: PhysicsEngine *physicsEngine; }; /** \} */ Modified: code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -172,7 +172,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Steering_Position2d::UpdateChild(UpdateParams ¶ms) +void Steering_Position2d::UpdateChild() { this->GetPositionCmd(); Modified: code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.hh =================================================================== --- code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/position2d/steering/Steering_Position2d.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -102,7 +102,7 @@ protected: virtual void ResetChild(); /// Update the controller - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// Finalize the controller protected: virtual void FiniChild(); Modified: code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -57,6 +57,9 @@ this->motionGain = 2; this->force = 10; + + this->panJoint = NULL; + this->tiltJoint = NULL; } //////////////////////////////////////////////////////////////////////////////// @@ -112,7 +115,7 @@ //////////////////////////////////////////////////////////////////////////////// // Update the controller -void Generic_PTZ::UpdateChild(UpdateParams ¶ms) +void Generic_PTZ::UpdateChild() { this->ptzIface->Lock(1); Modified: code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh =================================================================== --- code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh 2008-06-20 19:20:15 UTC (rev 6655) +++ code/gazebo/trunk/server/controllers/ptz/generic/Generic_PTZ.hh 2008-06-20 22:52:19 UTC (rev 6656) @@ -76,7 +76,7 @@ protected: virtual void InitChild(); /// \brief Update the controller - protected: virtual void UpdateChild(UpdateParams ¶ms); + protected: virtual void UpdateChild(); /// \brief Finalize the controller protected: virtual void FiniChild(); Added: code/gazebo/trunk/server/gui/GLFrame.cc =================================================================== --- code/gazebo/trunk/server/gui/GLFrame.cc (rev 0) +++ code/gazebo/trunk/server/gui/GLFrame.cc 2008-06-20 22:52:19 UTC (rev 6656) @@ -0,0 +1,194 @@ +/* + * 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: FLTK GL Frame + * Author: Nate Koenig + * Date: 18 Jun 2008 + * SVN: $Id:$ + */ + +#include <boost/bind.hpp> + +#include "CameraManager.hh" +#include "Global.hh" +#include "Pose3d.hh" +#include "GLWindow.hh" +#include "GLFrameManager.hh" +#include "UserCamera.hh" +#include "GLFrame.hh" + +using namespace gazebo; + +//////////////////////////////////////////////////////////////////////////////// +// Constructor +GLFrame::GLFrame(int x, int y, int w, int h, const std::string &name) + : Fl_Group(x,y,w,h, "") +{ + + this->box(FL_DOWN_BOX); + + this->headerBar = new Fl_Group(x,y,w,30); + this->headerBar->box(FL_UP_BOX); + + this->viewChoice = new Fl_Choice(x+2, y+2, 150,26); + this->viewChoice->add("View"); + this->viewChoice->mode(0, FL_MENU_DIVIDER); + this->viewChoice->add("Top", "", &gazebo::GLFrame::ViewCB, this); + this->viewChoice->add("Front", "", &gazebo::GLFrame::ViewCB, this); + this->viewChoice->add("Left", "", &gazebo::GLFrame::ViewCB, this); + this->viewChoice->add("User", "", &gazebo::GLFrame::ViewCB, this); + this->viewChoice->value(0); + + this->splitChoice = new Fl_Choice(this->viewChoice->x()+this->viewChoice->w()+2, this->viewChoice->y(), 150, 26); + + this->splitChoice->add("Split Window"); + this->splitChoice->mode(0,FL_MENU_DIVIDER); + this->splitChoice->add("Horizontal","", &gazebo::GLFrame::SplitCB, this); + this->splitChoice->add("Vertical","", &gazebo::GLFrame::SplitCB, this); + this->splitChoice->value(0); + + this->headerBar->end(); + this->headerBar->resizable(NULL); + + this->glWindow = new GLWindow(x+1,y+30, w-2, h-60)... [truncated message content] |
From: <na...@us...> - 2008-06-23 07:25:33
|
Revision: 6665 http://playerstage.svn.sourceforge.net/playerstage/?rev=6665&view=rev Author: natepak Date: 2008-06-23 07:25:42 -0700 (Mon, 23 Jun 2008) Log Message: ----------- Updates to the bandit model and the stereo camera Modified Paths: -------------- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/worlds/bandit.world code/gazebo/trunk/worlds/models/bandit.model Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-22 05:48:02 UTC (rev 6664) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-23 14:25:42 UTC (rev 6665) @@ -119,6 +119,8 @@ // Put stereo data to the interface void Stereo_Camera::PutStereoData() { + + printf("put stereo data\n"); StereoCameraData *stereo_data = this->stereoIface->data; //const unsigned char *rgb_src = NULL; //unsigned char *rgb_dst = NULL; Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-06-22 05:48:02 UTC (rev 6664) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-06-23 14:25:42 UTC (rev 6665) @@ -43,6 +43,7 @@ #include "StereoCameraSensor.hh" #define PF_FLOAT Ogre::PF_FLOAT32_R +#define PF_RGB Ogre::PF_B8G8R8 using namespace gazebo; @@ -328,7 +329,7 @@ { hardwareBuffer->blitToMemory ( Ogre::Box(left,top,right,bottom), Ogre::PixelBox( this->imageWidth, this->imageHeight, - 1, Ogre::PF_B8G8R8, this->rgbBuffer[i]) + 1, PF_RGB, this->rgbBuffer[i]) ); } else @@ -512,7 +513,7 @@ if (depth) pf = PF_FLOAT; else - pf = Ogre::PF_R8G8B8; + pf = PF_RGB; // Create the left render texture return Ogre::TextureManager::getSingleton().createManual( Modified: code/gazebo/trunk/worlds/bandit.world =================================================================== --- code/gazebo/trunk/worlds/bandit.world 2008-06-22 05:48:02 UTC (rev 6664) +++ code/gazebo/trunk/worlds/bandit.world 2008-06-23 14:25:42 UTC (rev 6665) @@ -34,6 +34,11 @@ <material>Gazebo/CloudySky</material> </sky> <shadowTechnique>stencilAdditive</shadowTechnique> + <fog> + <color>1.0 1.0 1.0</color> + <linearStart>10</linearStart> + <linearEnd>100</linearEnd> + </fog> </rendering:ogre> <model:physical name="pioneer2dx_model1"> @@ -137,14 +142,4 @@ </light> </model:renderable> - <model:renderable name="directional_white2"> - <light> - <type>directional</type> - <direction>0 -0.5 0.5</direction> - <diffuseColor>0.4 0.4 0.4</diffuseColor> - <specularColor>0.0 0.0 0.0</specularColor> - <attenuation>100 0.0 1.0 0.4</attenuation> - </light> - </model:renderable> - </gazebo:world> Modified: code/gazebo/trunk/worlds/models/bandit.model =================================================================== --- code/gazebo/trunk/worlds/models/bandit.model 2008-06-22 05:48:02 UTC (rev 6664) +++ code/gazebo/trunk/worlds/models/bandit.model 2008-06-23 14:25:42 UTC (rev 6665) @@ -61,7 +61,7 @@ <hfov>60</hfov> <nearClip>0.1</nearClip> <farClip>100</farClip> - <saveFrames>true</saveFrames> + <saveFrames>false</saveFrames> <saveFramePath>frames</saveFramePath> <baseline>0.2</baseline> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-23 15:51:21
|
Revision: 6670 http://playerstage.svn.sourceforge.net/playerstage/?rev=6670&view=rev Author: natepak Date: 2008-06-23 15:51:30 -0700 (Mon, 23 Jun 2008) Log Message: ----------- Added Vertical and Horizontal field of view attributes to the cameras Modified Paths: -------------- code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/rendering/OgreCamera.cc code/gazebo/trunk/server/rendering/OgreCamera.hh code/gazebo/trunk/server/rendering/UserCamera.cc Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-06-23 22:19:19 UTC (rev 6669) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-06-23 22:51:30 UTC (rev 6670) @@ -572,6 +572,9 @@ /// Horizontal field of view of the camera in radians public: double hfov; + /// Vertical field of view of the camera in radians + public: double vfov; + /// Pose of the camera public: Pose camera_pose; @@ -1351,7 +1354,10 @@ /// Horizontal field of view of the camera in radians public: double hfov; - /// Left image size + /// Vertical field of view of the camera in radians + public: double vfov; + + // Left image size public: unsigned int left_rgb_size; /// left image (R8G8B8) Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-06-23 22:19:19 UTC (rev 6669) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-06-23 22:51:30 UTC (rev 6670) @@ -107,7 +107,8 @@ data->image_size = data->width * data->height * 3; // GetFOV() returns radians - data->hfov = this->myParent->GetFOV(); + data->hfov = this->myParent->GetHFOV(); + data->vfov = this->myParent->GetVFOV(); // Set the pose of the camera cameraPose = this->myParent->GetWorldPose(); Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-23 22:19:19 UTC (rev 6669) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-23 22:51:30 UTC (rev 6670) @@ -136,7 +136,8 @@ stereo_data->farClip = this->myParent->GetFarClip(); stereo_data->nearClip = this->myParent->GetNearClip(); - stereo_data->hfov = this->myParent->GetFOV(); + stereo_data->hfov = this->myParent->GetHFOV(); + stereo_data->vfov = this->myParent->GetVFOV(); //stereo_data->right_rgb_size = stereo_data->width * stereo_data->height * 3; //stereo_data->left_rgb_size = stereo_data->width * stereo_data->height * 3; @@ -190,7 +191,9 @@ camera_data->image_size = camera_data->width * camera_data->height * 3; assert (camera_data->image_size <= sizeof(camera_data->image)); - camera_data->hfov = this->myParent->GetFOV(); + camera_data->hfov = this->myParent->GetHFOV(); + camera_data->vfov = this->myParent->GetVFOV(); + // Set the pose of the camera cameraPose = this->myParent->GetWorldPose(); camera_data->camera_pose.pos.x = cameraPose.pos.x; Modified: code/gazebo/trunk/server/rendering/OgreCamera.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCamera.cc 2008-06-23 22:19:19 UTC (rev 6669) +++ code/gazebo/trunk/server/rendering/OgreCamera.cc 2008-06-23 22:51:30 UTC (rev 6670) @@ -237,12 +237,19 @@ ////////////////////////////////////////////////////////////////////////////// /// Get the horizontal field of view of the camera -double OgreCamera::GetFOV() const +double OgreCamera::GetHFOV() const { return this->hfov; } ////////////////////////////////////////////////////////////////////////////// +/// Get the vertical field of view of the camera +double OgreCamera::GetVFOV() const +{ + return this->camera->getFOVy().valueRadians(); +} + +////////////////////////////////////////////////////////////////////////////// /// Get the width of the image unsigned int OgreCamera::GetImageWidth() const { Modified: code/gazebo/trunk/server/rendering/OgreCamera.hh =================================================================== --- code/gazebo/trunk/server/rendering/OgreCamera.hh 2008-06-23 22:19:19 UTC (rev 6669) +++ code/gazebo/trunk/server/rendering/OgreCamera.hh 2008-06-23 22:51:30 UTC (rev 6670) @@ -100,7 +100,10 @@ public: void SetFOV( float radians ); /// \brief Get the camera FOV (horizontal) - public: double GetFOV() const; + public: double GetHFOV() const; + + /// \brief Get the camera FOV (vertical) + public: double GetVFOV() const; /// \brief Get the width of the image public: unsigned int GetImageWidth() const; Modified: code/gazebo/trunk/server/rendering/UserCamera.cc =================================================================== --- code/gazebo/trunk/server/rendering/UserCamera.cc 2008-06-23 22:19:19 UTC (rev 6669) +++ code/gazebo/trunk/server/rendering/UserCamera.cc 2008-06-23 22:51:30 UTC (rev 6670) @@ -57,7 +57,7 @@ { OgreCamera::LoadCam(node); - this->SetClipDist(0.1, 1000); + this->SetClipDist(0.1, 100); this->SetFOV( DTOR(60) ); } This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-24 09:02:42
|
Revision: 6677 http://playerstage.svn.sourceforge.net/playerstage/?rev=6677&view=rev Author: natepak Date: 2008-06-24 09:02:24 -0700 (Tue, 24 Jun 2008) Log Message: ----------- Fixed the fiducial-laser interface. Modified Paths: -------------- code/gazebo/trunk/player/FiducialInterface.cc code/gazebo/trunk/player/gazebo.cfg code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/server/sensors/ray/RaySensor.cc code/gazebo/trunk/worlds/test.world Modified: code/gazebo/trunk/player/FiducialInterface.cc =================================================================== --- code/gazebo/trunk/player/FiducialInterface.cc 2008-06-24 15:09:10 UTC (rev 6676) +++ code/gazebo/trunk/player/FiducialInterface.cc 2008-06-24 16:02:24 UTC (rev 6677) @@ -174,9 +174,9 @@ ts.tv_sec = (int) (this->iface->data->head.time); ts.tv_usec = (int) (fmod(this->iface->data->head.time, 1) * 1e6); - unsigned int oldCount = data.fiducials_count; + unsigned int oldCount = this->data.fiducials_count; - data.fiducials_count = i; + this->data.fiducials_count = this->iface->data->count; if (oldCount != this->data.fiducials_count) { Modified: code/gazebo/trunk/player/gazebo.cfg =================================================================== --- code/gazebo/trunk/player/gazebo.cfg 2008-06-24 15:09:10 UTC (rev 6676) +++ code/gazebo/trunk/player/gazebo.cfg 2008-06-24 16:02:24 UTC (rev 6677) @@ -25,12 +25,12 @@ gz_id "laser_iface_0" ) -#driver -#( -# name "gazebo" -# provides ["fiducial:0"] -# gz_id "sicklms_fid_iface_1" -#) +driver +( + name "gazebo" + provides ["fiducial:0"] + gz_id "fiducial_iface_0" +) #driver #( Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-24 15:09:10 UTC (rev 6676) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-24 16:02:24 UTC (rev 6677) @@ -92,23 +92,30 @@ // Update the controller void SickLMS200_Laser::UpdateChild() { - bool opened = false; + bool laserOpened = false; + bool fidOpened = false; if (this->laserIface->Lock(1)) { - opened = this->laserIface->GetOpenCount() > 0; + laserOpened = this->laserIface->GetOpenCount() > 0; + fidOpened = this->fiducialIface->GetOpenCount() > 0; this->laserIface->Unlock(); } - if (opened) + if (laserOpened) { this->myParent->SetActive(true); - this->PutLaserData(); - //this->PutFiducialData(); } - else + + if (fidOpened) { + this->myParent->SetActive(true); + this->PutFiducialData(); + } + + if (!laserOpened && !fidOpened) + { this->myParent->SetActive(false); } } @@ -173,7 +180,7 @@ // Intensity is either-or v = (int) this->myParent->GetRetro(ja) || (int) this->myParent->GetRetro(jb); - this->laserIface->data->ranges[rangeCount-i-1] = r + minRange; + this->laserIface->data->ranges[i] = r + minRange; this->laserIface->data->intensity[i] = v; } @@ -196,15 +203,13 @@ double maxAngle = this->myParent->GetMaxAngle(); double minAngle = this->myParent->GetMinAngle(); -//TODO: implement max range and rangeCount -// double maxRange = this->myParent->GetMaxRange(); + double maxRange = this->myParent->GetMaxRange(); double minRange = this->myParent->GetMinRange(); int rayCount = this->myParent->GetRayCount(); -// int rangeCount = this->myParent->GetRangeCount(); + int rangeCount = this->myParent->GetRangeCount(); if (this->fiducialIface->Lock(1)) { - // Data timestamp this->fiducialIface->data->head.time = Simulator::Instance()->GetSimTime(); this->fiducialIface->data->count = 0; Modified: code/gazebo/trunk/server/sensors/ray/RaySensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2008-06-24 15:09:10 UTC (rev 6676) +++ code/gazebo/trunk/server/sensors/ray/RaySensor.cc 2008-06-24 16:02:24 UTC (rev 6677) @@ -109,8 +109,8 @@ this->prevPose = bodyPose; // Create and array of ray geoms - //for (int i = 0; i < this->rayCount; i++) - for (int i = this->rayCount-1; i >= 0; i--) + for (int i = 0; i < this->rayCount; i++) + //for (int i = this->rayCount-1; i >= 0; i--) { angle = i * (this->maxAngle - this->minAngle) / (rayCount - 1) + this->minAngle; Modified: code/gazebo/trunk/worlds/test.world =================================================================== --- code/gazebo/trunk/worlds/test.world 2008-06-24 15:09:10 UTC (rev 6676) +++ code/gazebo/trunk/worlds/test.world 2008-06-24 16:02:24 UTC (rev 6677) @@ -5,57 +5,50 @@ xmlns:gazebo="http://playerstage.sourceforge.net/gazebo/xmlschema/#gz" xmlns:model="http://playerstage.sourceforge.net/gazebo/xmlschema/#model" xmlns:sensor="http://playerstage.sourceforge.net/gazebo/xmlschema/#sensor" + xmlns:window="http://playerstage.sourceforge.net/gazebo/xmlschema/#window" + xmlns:param="http://playerstage.sourceforge.net/gazebo/xmlschema/#param" xmlns:body="http://playerstage.sourceforge.net/gazebo/xmlschema/#body" xmlns:geom="http://playerstage.sourceforge.net/gazebo/xmlschema/#geom" xmlns:joint="http://playerstage.sourceforge.net/gazebo/xmlschema/#joint" xmlns:interface="http://playerstage.sourceforge.net/gazebo/xmlschema/#interface" + xmlns:ui="http://playerstage.sourceforge.net/gazebo/xmlschema/#ui" xmlns:rendering="http://playerstage.sourceforge.net/gazebo/xmlschema/#rendering" - xmlns:renderable="http://playerstage.sourceforge.net/gazebo/xmlschema/#renderable" xmlns:controller="http://playerstage.sourceforge.net/gazebo/xmlschema/#controller" xmlns:physics="http://playerstage.sourceforge.net/gazebo/xmlschema/#physics" > <verbosity>5</verbosity> <physics:ode> - <stepTime>0.02</stepTime> - <gravity>0 0 -9.80665</gravity> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.8</gravity> <cfm>10e-5</cfm> - <erp>0.3</erp> + <erp>0.8</erp> + <maxUpdateRate>0</maxUpdateRate> </physics:ode> <rendering:gui> <type>fltk</type> - <size>640 480</size> + <size>800 600</size> <pos>0 0</pos> </rendering:gui> <rendering:ogre> - <ambient>1.0 1.0 1.0 1.0</ambient> + <ambient>0.5 0.5 0.5 1.0</ambient> <sky> <material>Gazebo/CloudySky</material> </sky> + <fog> + <color>1.0 1.0 1.0</color> + <linearStart>10</linearStart> + <linearEnd>100</linearEnd> + </fog> </rendering:ogre> - <model:physical name="sphere1_model"> - <xyz>0 0 0.1</xyz> - <static>false</static> - <body:sphere name="sphere1_body"> - <geom:sphere name="sphere1_geom"> - <size>0.2</size> - <mass>1.0</mass> - - <visual> - <scale>0.2 0.2 0.2</scale> - <material>Gazebo/Rocky</material> - <mesh>unit_sphere</mesh> - </visual> - </geom:sphere> - </body:sphere> - </model:physical> - + <!-- Ground Plane --> <model:physical name="plane1_model"> <xyz>0 0 0</xyz> + <rpy>0 0 0</rpy> <static>true</static> <body:plane name="plane1_body"> @@ -64,27 +57,84 @@ <size>2000 2000</size> <segments>10 10</segments> <uvTile>100 100</uvTile> - <material>Gazebo/GrassFloor</material> + <material>Gazebo/Grey</material> </geom:plane> </body:plane> </model:physical> - <model:physical name="cam2_model"> - <xyz>-3.0 0 2.59</xyz> - <rpy>0 28 -2</rpy> - <static>true</static> + <model:physical name="cylinder1_model"> + <xyz>1 -1.5 0.5</xyz> + <rpy>0.0 0.0 0.0</rpy> - <body:empty name="cam2_body"> - <sensor:camera name="cam2_sensor"> - <imageSize>800 600</imageSize> - <hfov>60</hfov> - <nearClip>0.1</nearClip> - <farClip>100</farClip> + <body:cylinder name="cylinder1_body"> + <geom:cylinder name="cylinder1_geom"> + <size>0.5 1</size> + <mass>1.0</mass> + <visual> + <mesh>unit_cylinder</mesh> + <material>Gazebo/RustyBarrel</material> + </visual> - <saveFrames>false</saveFrames> - <saveFramePath>frames</saveFramePath> - </sensor:camera> - </body:empty> + <laserFiducialId>1</laserFiducialId> + </geom:cylinder> + </body:cylinder> </model:physical> + + <!-- + Include the complete model described in the .model file + This assumes the root node is a <model:...> + --> + <!-- <include embedded="false"> + <xi:include href="pioneer2dx.model" /> + </include> + --> + + <model:physical name="pioneer2dx_model1"> + <xyz>0 0 0.145</xyz> + <rpy>0.0 0.0 0.0</rpy> + + <controller:differential_position2d name="controller1"> + <leftJoint>left_wheel_hinge</leftJoint> + <rightJoint>right_wheel_hinge</rightJoint> + <wheelSeparation>0.34</wheelSeparation> + <wheelDiameter>0.15</wheelDiameter> + <torque>5</torque> + <interface:position name="position_iface_0"/> + </controller:differential_position2d> + + <model:physical name="laser"> + <xyz>0.15 0 0.18</xyz> + + <attach> + <parentBody>chassis_body</parentBody> + <myBody>laser_body</myBody> + </attach> + + <include embedded="true"> + <xi:include href="models/sicklms200.model" /> + </include> + </model:physical> + + <!-- + The include should be last within a model. All previous statements + will override those in the included file + --> + <include embedded="true"> + <xi:include href="models/pioneer2dx.model" /> + </include> + </model:physical> + + <!-- White Directional light --> + <model:renderable name="directional_white"> + <light> + <type>directional</type> + <direction>0 -0.6 -0.4</direction> + <diffuseColor>1.0 1.0 1.0</diffuseColor> + <specularColor>0.2 0.2 0.2</specularColor> + <attenuation>1000 1.0 0.0 0</attenuation> + </light> + </model:renderable> + + </gazebo:world> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-24 09:09:33
|
Revision: 6678 http://playerstage.svn.sourceforge.net/playerstage/?rev=6678&view=rev Author: natepak Date: 2008-06-24 09:09:36 -0700 (Tue, 24 Jun 2008) Log Message: ----------- Check for null pointer in SickLMS200 controller. Modified Paths: -------------- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/worlds/test.world Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-24 16:02:24 UTC (rev 6677) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-24 16:09:36 UTC (rev 6678) @@ -98,10 +98,15 @@ if (this->laserIface->Lock(1)) { laserOpened = this->laserIface->GetOpenCount() > 0; - fidOpened = this->fiducialIface->GetOpenCount() > 0; this->laserIface->Unlock(); } + if (this->fiducialIface && this->fiducialIface->Lock(1)) + { + fidOpened = this->fiducialIface->GetOpenCount() > 0; + this->fiducialIface->Unlock(); + } + if (laserOpened) { this->myParent->SetActive(true); Modified: code/gazebo/trunk/worlds/test.world =================================================================== --- code/gazebo/trunk/worlds/test.world 2008-06-24 16:02:24 UTC (rev 6677) +++ code/gazebo/trunk/worlds/test.world 2008-06-24 16:09:36 UTC (rev 6678) @@ -70,12 +70,12 @@ <geom:cylinder name="cylinder1_geom"> <size>0.5 1</size> <mass>1.0</mass> + <laserFiducialId>1</laserFiducialId> + <visual> <mesh>unit_cylinder</mesh> <material>Gazebo/RustyBarrel</material> </visual> - - <laserFiducialId>1</laserFiducialId> </geom:cylinder> </body:cylinder> </model:physical> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-25 12:12:23
|
Revision: 6683 http://playerstage.svn.sourceforge.net/playerstage/?rev=6683&view=rev Author: natepak Date: 2008-06-25 12:12:30 -0700 (Wed, 25 Jun 2008) Log Message: ----------- Incorporated Differentail steering patch from Jerome Berger Modified Paths: -------------- code/gazebo/trunk/examples/player/position2d/position2d.cc code/gazebo/trunk/libgazebo/gazebo.h code/gazebo/trunk/player/LaserInterface.cc code/gazebo/trunk/player/gazebo.cfg code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh code/gazebo/trunk/worlds/pioneer2dx.world Modified: code/gazebo/trunk/examples/player/position2d/position2d.cc =================================================================== --- code/gazebo/trunk/examples/player/position2d/position2d.cc 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/examples/player/position2d/position2d.cc 2008-06-25 19:12:30 UTC (rev 6683) @@ -14,7 +14,7 @@ PlayerClient robot(PlayerCc::PLAYER_HOSTNAME, PlayerCc::PLAYER_PORTNUM); // Subscribe to the simulation proxy - Position2dProxy pp(&robot, 0); + Position2dProxy pp(&robot, 1); // Print out some stuff std::cout << robot << std::endl; @@ -26,7 +26,7 @@ // This blocks until new data comes robot.Read(); - pp.SetSpeed(0.2, 0.05); + pp.SetSpeed(0.0, 0.05); } } catch (PlayerCc::PlayerError e) Modified: code/gazebo/trunk/libgazebo/gazebo.h =================================================================== --- code/gazebo/trunk/libgazebo/gazebo.h 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/libgazebo/gazebo.h 2008-06-25 19:12:30 UTC (rev 6683) @@ -793,6 +793,9 @@ /// Angular resolution public: double res_angle; + /// Range resolution + public: double res_range; + /// Max range value public: double max_range; Modified: code/gazebo/trunk/player/LaserInterface.cc =================================================================== --- code/gazebo/trunk/player/LaserInterface.cc 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/player/LaserInterface.cc 2008-06-25 19:12:30 UTC (rev 6683) @@ -115,6 +115,7 @@ plc.max_angle = this->iface->data->max_angle; plc.max_range = this->iface->data->max_range; plc.resolution = this->iface->data->res_angle; + plc.range_res = this->iface->data->res_range; plc.intensity = intensity; this->driver->Publish(this->device_addr, respQueue, @@ -192,12 +193,12 @@ //printf("range res = %f %f\n", rangeRes, this->iface->data->max_range); + double oldCount = this->data.ranges_count; + this->data.min_angle = this->iface->data->min_angle; this->data.max_angle = this->iface->data->max_angle; + this->data.resolution = angleRes; this->data.max_range = this->iface->data->max_range; - this->data.resolution = angleRes; - - double oldCount = this->data.ranges_count; this->data.ranges_count = this->data.intensity_count = this->iface->data->range_count; this->data.id = this->scanId++; @@ -216,10 +217,13 @@ this->data.intensity[i] = (uint8_t) (int) this->iface->data->intensity[i]; } - this->driver->Publish( this->device_addr, - PLAYER_MSGTYPE_DATA, - PLAYER_LASER_DATA_SCAN, - (void*)&this->data, sizeof(this->data), &this->datatime ); + if (this->data.ranges_count > 0) + { + this->driver->Publish( this->device_addr, + PLAYER_MSGTYPE_DATA, + PLAYER_LASER_DATA_SCAN, + (void*)&this->data, sizeof(this->data), &this->datatime ); + } } this->iface->Unlock(); Modified: code/gazebo/trunk/player/gazebo.cfg =================================================================== --- code/gazebo/trunk/player/gazebo.cfg 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/player/gazebo.cfg 2008-06-25 19:12:30 UTC (rev 6683) @@ -27,6 +27,23 @@ driver ( + name "vfh" + provides ["position2d:1"] + requires ["position2d:0" "laser:0"] + cell_size 0.1 + window_diameter 61 + sector_angle 1 + safety_dist_0ms 0.2 + safety_dist_1ms 0.4 + max_speed 0.3 + max_turnrate_0ms 75 + max_turnrate_1ms 50 + weight_desired_dir 5.0 + weight_current_dir 3.0 +) + +driver +( name "gazebo" provides ["fiducial:0"] gz_id "fiducial_iface_0" Modified: code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/server/controllers/camera/stereo/Stereo_Camera.cc 2008-06-25 19:12:30 UTC (rev 6683) @@ -119,8 +119,6 @@ // Put stereo data to the interface void Stereo_Camera::PutStereoData() { - - printf("put stereo data\n"); StereoCameraData *stereo_data = this->stereoIface->data; //const unsigned char *rgb_src = NULL; //unsigned char *rgb_dst = NULL; Modified: code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc =================================================================== --- code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/server/controllers/laser/sicklms200/SickLMS200_Laser.cc 2008-06-25 19:12:30 UTC (rev 6683) @@ -147,7 +147,6 @@ int rayCount = this->myParent->GetRayCount(); int rangeCount = this->myParent->GetRangeCount(); - if (this->laserIface->Lock(1)) { // Data timestamp @@ -157,6 +156,7 @@ this->laserIface->data->min_angle = minAngle; this->laserIface->data->max_angle = maxAngle; this->laserIface->data->res_angle = (maxAngle - minAngle) / (rangeCount - 1); + this->laserIface->data->res_range = 0.1; this->laserIface->data->max_range = maxRange; this->laserIface->data->range_count = rangeCount; Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.cc 2008-06-25 19:12:30 UTC (rev 6683) @@ -57,6 +57,8 @@ this->wheelSpeed[RIGHT] = 0; this->wheelSpeed[LEFT] = 0; + + this->prevUpdateTime = Simulator::Instance()->GetSimTime(); } //////////////////////////////////////////////////////////////////////////////// @@ -90,7 +92,6 @@ if (!this->joints[RIGHT]) gzthrow("The controller couldn't get right hinge joint"); - } //////////////////////////////////////////////////////////////////////////////// @@ -148,14 +149,16 @@ ws = this->wheelSep; - stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime(); + //stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime(); + stepTime = Simulator::Instance()->GetSimTime() - this->prevUpdateTime; + this->prevUpdateTime = Simulator::Instance()->GetSimTime(); // Distance travelled by front wheels d1 = stepTime * wd / 2 * this->joints[LEFT]->GetAngleRate(); d2 = stepTime * wd / 2 * this->joints[RIGHT]->GetAngleRate(); dr = (d1 + d2) / 2; - da = (d2 - d1) / ws; + da = (d1 - d2) / ws; // Compute odometric pose this->odomPose[0] += dr * cos( this->odomPose[2] ); Modified: code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh =================================================================== --- code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/server/controllers/position2d/differential/Differential_Position2d.hh 2008-06-25 19:12:30 UTC (rev 6683) @@ -113,6 +113,9 @@ /// Speeds of the wheels private: float wheelSpeed[2]; + // Simulation time of the last update + private: double prevUpdateTime; + /// True = enable motors private: bool enableMotors; Modified: code/gazebo/trunk/worlds/pioneer2dx.world =================================================================== --- code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-25 17:00:34 UTC (rev 6682) +++ code/gazebo/trunk/worlds/pioneer2dx.world 2008-06-25 19:12:30 UTC (rev 6683) @@ -62,20 +62,22 @@ </body:plane> </model:physical> - <model:physical name="cylinder1_model"> - <xyz>1 -1.5 0.5</xyz> + <model:physical name="sphere1_model"> + <xyz>2.15 -1.68 .7</xyz> <rpy>0.0 0.0 0.0</rpy> + <static>true</static> - <body:cylinder name="cylinder1_body"> - <geom:cylinder name="cylinder1_geom"> - <size>0.5 1</size> - <mass>1.0</mass> + <body:sphere name="sphere1_body"> + <geom:sphere name="sphere1_geom"> + <size>0.1</size> + <visual> - <mesh>unit_cylinder</mesh> - <material>Gazebo/RustyBarrel</material> + <scale>0.1 0.1 0.1</scale> + <mesh>unit_sphere</mesh> + <material>Gazebo/Rocky</material> </visual> - </geom:cylinder> - </body:cylinder> + </geom:sphere> + </body:sphere> </model:physical> @@ -95,7 +97,7 @@ <controller:differential_position2d name="controller1"> <leftJoint>left_wheel_hinge</leftJoint> <rightJoint>right_wheel_hinge</rightJoint> - <wheelSeparation>0.34</wheelSeparation> + <wheelSeparation>0.39</wheelSeparation> <wheelDiameter>0.15</wheelDiameter> <torque>5</torque> <interface:position name="position_iface_0"/> This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-06-26 14:04:09
|
Revision: 6696 http://playerstage.svn.sourceforge.net/playerstage/?rev=6696&view=rev Author: natepak Date: 2008-06-26 14:04:18 -0700 (Thu, 26 Jun 2008) Log Message: ----------- Fixed camera masking, and fixed generic_camera controller Modified Paths: -------------- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh code/gazebo/trunk/server/rendering/OgreCamera.cc code/gazebo/trunk/server/rendering/UserCamera.cc code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc code/gazebo/trunk/worlds/models/sonyvid30.model code/gazebo/trunk/worlds/pioneer2dx_gripper.world Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.cc 2008-06-26 21:04:18 UTC (rev 6696) @@ -35,7 +35,7 @@ #include "gazebo.h" #include "GazeboError.hh" #include "ControllerFactory.hh" -#include "OgreCamera.hh" +#include "MonoCameraSensor.hh" #include "Generic_Camera.hh" using namespace gazebo; @@ -47,7 +47,7 @@ Generic_Camera::Generic_Camera(Entity *parent) : Controller(parent) { - this->myParent = dynamic_cast<OgreCamera*>(this->parent); + this->myParent = dynamic_cast<MonoCameraSensor*>(this->parent); if (!this->myParent) gzthrow("Generic_Camera controller requires a Camera Sensor as its parent"); @@ -123,22 +123,11 @@ assert (data->image_size <= sizeof(data->image)); // Copy the pixel data to the interface - //src = this->myParent->GetImageData(); + src = this->myParent->GetImageData(0); dst = data->image; memcpy(dst, src, data->image_size); - - //unsigned int i, j, k; - // OGRE image data is A8 B8 G8 R8. Must convert to R8 G8 B8 - /* for (i=0; i<data->height; i++) - for (j=0; j<data->width; j++) - for (k=2; k>=0; k--) - memcpy(dst + i*data->width*3 + j*3 + 2-k, - src + i*data->width*4 + j*4 + k, - 1); - */ - this->cameraIface->Unlock(); // New data is available Modified: code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh =================================================================== --- code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/server/controllers/camera/generic/Generic_Camera.hh 2008-06-26 21:04:18 UTC (rev 6696) @@ -34,7 +34,7 @@ namespace gazebo { class CameraIface; - class OgreCamera; + class MonoCameraSensor; /// @addtogroup gazebo_controller /// @{ @@ -97,7 +97,7 @@ private: CameraIface *cameraIface; /// The parent sensor - private: OgreCamera *myParent; + private: MonoCameraSensor *myParent; }; Modified: code/gazebo/trunk/server/rendering/OgreCamera.cc =================================================================== --- code/gazebo/trunk/server/rendering/OgreCamera.cc 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/server/rendering/OgreCamera.cc 2008-06-26 21:04:18 UTC (rev 6696) @@ -132,7 +132,6 @@ // Initialize the camera void OgreCamera::InitCam() { - this->camera = OgreCreator::CreateCamera(this->cameraName, this->nearClip, this->farClip, this->hfov, this->renderTarget ); // Create a scene node to control pitch motion @@ -142,7 +141,6 @@ this->saveCount = 0; - //this->camera->getViewport()->setVisibilityMask(this->visibilityMask); } ////////////////////////////////////////////////////////////////////////////// @@ -155,6 +153,7 @@ // Update the drawing void OgreCamera::UpdateCam() { + if (World::Instance()->GetWireframe()) { this->camera->setPolygonMode(Ogre::PM_WIREFRAME); Modified: code/gazebo/trunk/server/rendering/UserCamera.cc =================================================================== --- code/gazebo/trunk/server/rendering/UserCamera.cc 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/server/rendering/UserCamera.cc 2008-06-26 21:04:18 UTC (rev 6696) @@ -74,7 +74,7 @@ this->SetAspectRatio( Ogre::Real(this->viewport->getActualWidth()) / Ogre::Real(this->viewport->getActualHeight()) ); - //this->viewport->setVisibilityMask(this->visibilityMask); + this->viewport->setVisibilityMask(this->visibilityMask); } //////////////////////////////////////////////////////////////////////////////// Modified: code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/server/sensors/camera/MonoCameraSensor.cc 2008-06-26 21:04:18 UTC (rev 6696) @@ -124,25 +124,6 @@ this->renderTarget->update(); - if (this->saveFrames) - this->SaveFrame(); -} - -//////////////////////////////////////////////////////////////////////////////// -// 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(unsigned int i) -{ - if (i!=0) - gzerr(0) << "Camera index must be zero for mono cam"; - Ogre::HardwarePixelBufferSharedPtr mBuffer; size_t size; @@ -176,6 +157,26 @@ mBuffer->unlock(); + + if (this->saveFrames) + this->SaveFrame(); +} + +//////////////////////////////////////////////////////////////////////////////// +// 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(unsigned int i) +{ + if (i!=0) + gzerr(0) << "Camera index must be zero for mono cam"; + return this->saveFrameBuffer; } Modified: code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc =================================================================== --- code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/server/sensors/camera/StereoCameraSensor.cc 2008-06-26 21:04:18 UTC (rev 6696) @@ -139,6 +139,7 @@ cviewport->setClearEveryFrame(true); cviewport->setOverlaysEnabled(false); cviewport->setBackgroundColour( *OgreAdaptor::Instance()->backgroundColor ); + cviewport->setVisibilityMask(this->visibilityMask); } // Create materials for all the render textures. Modified: code/gazebo/trunk/worlds/models/sonyvid30.model =================================================================== --- code/gazebo/trunk/worlds/models/sonyvid30.model 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/worlds/models/sonyvid30.model 2008-06-26 21:04:18 UTC (rev 6696) @@ -96,6 +96,10 @@ <farClip>100</farClip> <saveFrames>true</saveFrames> <saveFramePath>frames</saveFramePath> + + <controller:generic_camera name="sonyvid30_camera_controller"> + <interface:camera name="camera_iface_0"/> + </controller:generic_camera> </sensor:camera> </body:box> Modified: code/gazebo/trunk/worlds/pioneer2dx_gripper.world =================================================================== --- code/gazebo/trunk/worlds/pioneer2dx_gripper.world 2008-06-26 20:33:09 UTC (rev 6695) +++ code/gazebo/trunk/worlds/pioneer2dx_gripper.world 2008-06-26 21:04:18 UTC (rev 6696) @@ -93,19 +93,6 @@ </include> </model:physical> - <model:physical name="laser"> - <xyz>0.15 0 0.18</xyz> - - <attach> - <parentBody>chassis_body</parentBody> - <myBody>laser_body</myBody> - </attach> - - <include embedded="true"> - <xi:include href="models/sicklms200.model" /> - </include> - </model:physical> - <!-- The include should be last within a model. All previous statements will override those in the included file This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |
From: <na...@us...> - 2008-07-01 11:50:46
|
Revision: 6723 http://playerstage.svn.sourceforge.net/playerstage/?rev=6723&view=rev Author: natepak Date: 2008-07-01 11:50:43 -0700 (Tue, 01 Jul 2008) Log Message: ----------- Updates to the documentation Modified Paths: -------------- code/gazebo/trunk/doc/gazebo.dox code/gazebo/trunk/doc/header.html code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc code/gazebo/trunk/server/physics/BallJoint.hh code/gazebo/trunk/server/physics/Body.hh code/gazebo/trunk/server/physics/BoxGeom.hh code/gazebo/trunk/server/physics/CylinderGeom.hh code/gazebo/trunk/server/physics/Hinge2Joint.hh code/gazebo/trunk/server/physics/HingeJoint.hh code/gazebo/trunk/server/physics/SliderJoint.hh code/gazebo/trunk/server/physics/SphereGeom.hh code/gazebo/trunk/server/physics/TrimeshGeom.hh code/gazebo/trunk/server/physics/UniversalJoint.hh code/gazebo/trunk/server/physics/ode/ODEPhysics.cc code/gazebo/trunk/server/physics/ode/ODEPhysics.hh Added Paths: ----------- code/gazebo/trunk/doc/xmltags.html Modified: code/gazebo/trunk/doc/gazebo.dox =================================================================== --- code/gazebo/trunk/doc/gazebo.dox 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/doc/gazebo.dox 2008-07-01 18:50:43 UTC (rev 6723) @@ -170,7 +170,7 @@ # The TAB_SIZE tag can be used to set the number of spaces in a tab. # Doxygen uses this value to replace tabs by spaces in code fragments. -TAB_SIZE = 8 +TAB_SIZE = 4 # This tag can be used to specify a number of aliases that acts # as commands in the documentation. An alias has the form "name=value". @@ -459,7 +459,7 @@ # directories like "/usr/src/myproject". Separate the files or directories # with spaces. -INPUT = mainpage.html install.html modules.html tutorial_model.html tutorial_controller.html tutorial_sensor.html tutorial_player.html tutorial_terrain.html config_syntax.html gazebo.txt gui.html prerequisites.html tutorial_mesh.html media_organization.html ../server ../player ../libgazebo +INPUT = mainpage.html install.html modules.html tutorial_model.html tutorial_controller.html tutorial_sensor.html tutorial_player.html tutorial_terrain.html config_syntax.html gazebo.txt gui.html prerequisites.html tutorial_mesh.html media_organization.html xmltags.html ../server ../player ../libgazebo # If the value of the INPUT tag contains directories, you can use the # FILE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp Modified: code/gazebo/trunk/doc/header.html =================================================================== --- code/gazebo/trunk/doc/header.html 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/doc/header.html 2008-07-01 18:50:43 UTC (rev 6723) @@ -47,6 +47,7 @@ <dd><a href="modules.html">Code Reference</a></dd> <dd><a href="annotated.html">Class List</a></dd> <dd><a href="hierarchy.html">Class Hierarchy</a></dd> + <dd><a href="xmltags.html">Worldfile XML Tags</a></dd> </dl> </div> Added: code/gazebo/trunk/doc/xmltags.html =================================================================== --- code/gazebo/trunk/doc/xmltags.html (rev 0) +++ code/gazebo/trunk/doc/xmltags.html 2008-07-01 18:50:43 UTC (rev 6723) @@ -0,0 +1,100 @@ +/** + +\page xmltags Worldfile XML Tags + +\section generaltags General Tags + +The following XML tags are general attributes that are applicable to all world files. Specific details of geometries, joints, sensors, etc can be found in their documentation. + +\verbatim + + <!-- Parameters for ODE physics engine --> + <physics:ode> + <stepTime>#</stepTime> <!-- Duration of a step, in seconds. (default=0.03)--> + <gravity># # #</gravity> <!-- Gravity vector. (default=0 0 -9.8)--> + <cfm>#</cfm> <!-- Constraint force mixing. (default = 10e-5) --> + <erp>#</erp> <!-- Error reduction. (default=0.3) --> + </physics:ode> + + <!-- Parameters for the GUI --> + <rendering:gui> + <size># #</size> <!-- Size in pixels of the initial GUI --> + <pos># #</pos> <!-- Position on the screen to place the GUI --> + </rendering:gui> + + <!-- Parameters for the OGRE rendering engine --> + <rendering:ogre> + <ambient># # # #</ambient> <!-- Ambient light color --> + + <sky> <!-- Sky material. Optional --> + <material>string</material> + </sky> + + <fog> <!-- Fog, optional --> + <color># # #</color> <!-- Color of the fog --> + <linearStart>#</linearStart> <!-- Distance at which the fog starts --> + <linearEnd>#</linearEnd> <!-- Distance at which the fog reaches full density--> + </fog> + </rendering:ogre> + + <!-- A model. There is no limit to the number of models in a world--> + <model:physical name=""> + <xyz># # #</xyz> <!-- Position of the model in the world --> + <rpy># # #</rpy> <!-- Roll, pitch, yaw of the model --> + + <static>true|false</static> <!-- True = makes the model immovable --> + + <!-- A body. There is no limit to the number of bodies in a model --> + <body:[plane|box|sphere|cylinder|trimesh] name=""> + <xyz># # #</xyz> <!-- XYZ position of the body relative to the model --> + <rpy># # #</rpy> <!-- Roll, Pitch, Yaw of the body relative to the model --> + + <!-- A geometry that is attached to the parent body. There can be 1..n geometries per body --> + <geom:[box|sphere|cylinder|trimesh] name=""> + <xyz># # #</xyz> <!-- XYZ position of the geom, relative to the body --> + <rpy># # #</rpy> <!-- Roll, pitch, yaw of the geom, relative to the body --> + <mass>#</mass> <!-- Mass in kilograms --> + <laserFiducialId>#</laserFiducialId> <!-- Integer laser fiducial ID --> + <laserRetro>#</laserRetro> <!-- Retro reflectance value --> + + <size># # #</size> <!-- Size of geom. Sphere uses radius, Cylinder uses radius, height. Box uses xyz lengths --> + + <visual> + <xyz># # #</xyz> <!-- Position relative to the geom --> + <rpy># # #<rpy> <!-- Roll, pitch, yaw relative to the geom --> + <size># # #<size> <!-- Size of the mesh in the x,y,z dimensions --> + <mesh>string</mesh> <!-- Name of the mesh --> + <material>string</mesh> <!-- Name of the material. This name must match a name in one of the file at install_path/share/gazebo/Media/materials/scripts. --> + <castShadows>true|false</castShadows> <!-- True = cast shadows --> + </visual> + + </geom:[box|sphere|cylinder|trimesh]> + + <!-- Attach a sensor to the body. Each sensor has a different set of parameters --> + <sensor:type name=""> + + <!-- Define a controller to process the sensor information, and expose the data to libgazebo --> + <controller:type name=""> + <!-- Define an type of interface that the controller interacts with --> + <interface:type name="" /> + </controller:type name=""> + </sensor> + + </body:[box|sphere|cylinder|trimesh]> + + <!-- A joint connects two bodies together --> + <joint:type> + <body1>string</body1> <!-- Name of the first body --> + <body2>string</body2> <!-- Name of the second body --> + <anchor>string</anchor> <!-- Name of the model that acts as the anchor --> + <anchorOffset># # #</anchorOffset> <!-- Meters to move the anchor, relative to the anchor --> + <erp>#</erp> <!-- Error reduction parameter --> + <cfm>#</cfm> <!-- Constraint force mixing parameter --> + </joint:type> + + </model:physical> + + +\endverbatim + +*/ Modified: code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc =================================================================== --- code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/examples/libgazebo/simiface/simiface.cc 2008-07-01 18:50:43 UTC (rev 6723) @@ -30,28 +30,34 @@ return -1; } - simIface->Lock(1); //simIface->data->reset = 1; // Example of how to move a model (box1_model) - uint8_t name[512] = "box1_model"; + uint8_t name[512] = "pioneer2dx_model1"; uint8_t cmd[32] = "set_pose3d"; - memcpy(simIface->data->model_name, name, 512); - memcpy(simIface->data->model_req,cmd, 32); + for (int i=0; i< 10; i++) + { + simIface->Lock(1); - simIface->data->model_pose.pos.x = 2; - simIface->data->model_pose.pos.y = 0; - simIface->data->model_pose.pos.z = 0; + memcpy(simIface->data->model_name, name, 512); + memcpy(simIface->data->model_req,cmd, 32); - simIface->Unlock(); + simIface->data->model_pose.pos.x = i+.1; + simIface->data->model_pose.pos.y = 0; + simIface->data->model_pose.pos.z = 0.2; - usleep(1000000); + simIface->Unlock(); + usleep(100000); + } + + // Example of resetting the simulator - simIface->Lock(1); + /*simIface->Lock(1); simIface->data->reset = 1; simIface->Unlock(); + */ usleep(1000000); Modified: code/gazebo/trunk/server/physics/BallJoint.hh =================================================================== --- code/gazebo/trunk/server/physics/BallJoint.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/BallJoint.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -46,6 +46,10 @@ - Name of the second body to attach to the joint - anchor (string) - Name of the body which will act as the anchor to the joint + - erp (double) + - Error reduction parameter. Default = 0.4 + - cfm (double) + - Constraint force mixing. Default = 0.8 \par Example \verbatim Modified: code/gazebo/trunk/server/physics/Body.hh =================================================================== --- code/gazebo/trunk/server/physics/Body.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/Body.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -35,11 +35,13 @@ #include "Entity.hh" #include "Pose3d.hh" + namespace gazebo { class Geom; class Sensor; + /// \addtogroup gazebo_physics /// \brief The body class /// \{ Modified: code/gazebo/trunk/server/physics/BoxGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/BoxGeom.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/BoxGeom.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -52,11 +52,15 @@ <xyz>1 2 3</xyz> <rpy>0 0 30</rpy> <size>0.1 0.2 0.3</size> - <mesh>default</mesh> <mass>0.5</mass> - <material>Gazebo/Red</material> <laserFiducialId>1</laserFiducialId> <laserRetro>0.5</laserRetro> + + <visual> + <size>0.1 0.2 0.3</size> + <mesh>default</mesh> + <material>Gazebo/Red</material> + </visual> </geom:box> \endverbatim */ Modified: code/gazebo/trunk/server/physics/CylinderGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/CylinderGeom.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/CylinderGeom.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -51,11 +51,15 @@ <xyz>1 2 3</xyz> <rpy>0 0 30</rpy> <size>0.1 0.5</size> - <mesh>default</mesh> <mass>0.5</mass> - <material>Gazebo/Red</material> <laserFiducialId>1</laserFiducialId> <laserRetro>0.5</laserRetro> + + <visual> + <size>0.1 0.1 0.5</size> + <mesh>default</mesh> + <material>Gazebo/Red</material> + </visual> </geom:box> \endverbatim */ Modified: code/gazebo/trunk/server/physics/Hinge2Joint.hh =================================================================== --- code/gazebo/trunk/server/physics/Hinge2Joint.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/Hinge2Joint.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -64,7 +64,14 @@ - highStop2 (float, degrees) - The high stop angle for the second degree of freedom - Default: infinity + - erp (double) + - Error reduction parameter. + - Default = 0.4 + - cfm (double) + - Constraint force mixing. + - Default = 0.8 + \par Example \verbatim <joint:hinge2 name="hinge2_joint> Modified: code/gazebo/trunk/server/physics/HingeJoint.hh =================================================================== --- code/gazebo/trunk/server/physics/HingeJoint.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/HingeJoint.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -55,6 +55,12 @@ - highStop (float, degrees) - The high stop angle for the first degree of freedom - Default: infinity + - erp (double) + - Error reduction parameter. + - Default = 0.4 + - cfm (double) + - Constraint force mixing. + - Default = 0.8 \par Example \verbatim Modified: code/gazebo/trunk/server/physics/SliderJoint.hh =================================================================== --- code/gazebo/trunk/server/physics/SliderJoint.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/SliderJoint.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -56,7 +56,14 @@ - highStop (float, meters) - The high stop position - Default: infinity + - erp (double) + - Error reduction parameter. + - Default = 0.4 + - cfm (double) + - Constraint force mixing. + - Default = 0.8 + \par Example \verbatim <joint:slider name="slider_joint> Modified: code/gazebo/trunk/server/physics/SphereGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/SphereGeom.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/SphereGeom.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -52,11 +52,15 @@ <xyz>1 2 3</xyz> <rpy>0 0 30</rpy> <size>0.1</size> - <mesh>default</mesh> <mass>0.5</mass> - <material>Gazebo/Red</material> <laserFiducialId>1</laserFiducialId> <laserRetro>0.5</laserRetro> + + <visual> + <mesh>default</mesh> + <size>0.1 0.1 0.1</size> + <material>Gazebo/Red</material> + </visual> </geom:sphere> \endverbatim */ Modified: code/gazebo/trunk/server/physics/TrimeshGeom.hh =================================================================== --- code/gazebo/trunk/server/physics/TrimeshGeom.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/TrimeshGeom.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -50,10 +50,15 @@ \par Example \verbatim <geom:trimesh name="pallet_geom"> - <mesh>kitchen.mesh</mesh> + <mesh>WoodPallet.mesh</mesh> <scale>.2 .2 .2</scale> - <material>Gazebo/WoodPallet</material> - <mass>1.0</mass> + <mass>0.1</mass> + + <visual> + <scale>.2 .2 .2</scale> + <material>Gazebo/WoodPallet</material> + <mesh>WoodPallet.mesh</mesh> + </visual> </geom:trimesh> \endverbatim */ Modified: code/gazebo/trunk/server/physics/UniversalJoint.hh =================================================================== --- code/gazebo/trunk/server/physics/UniversalJoint.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/UniversalJoint.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -62,7 +62,14 @@ - highStop2 (float, degrees) - The high stop angle for the second degree of freedom - Default: infinity + - erp (double) + - Error reduction parameter. + - Default = 0.4 + - cfm (double) + - Constraint force mixing. + - Default = 0.8 + \par Example \verbatim <joint:universal name="universal_joint> Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2008-07-01 18:50:43 UTC (rev 6723) @@ -100,7 +100,7 @@ this->gravity = cnode->GetVector3("gravity",this->gravity); this->stepTime = cnode->GetDouble("stepTime",this->stepTime); this->updateRate = cnode->GetDouble("maxUpdateRate", 0, 0); - this->globalCFM = cnode->GetDouble("cfm",1e-5,0); + this->globalCFM = cnode->GetDouble("cfm",10e-5,0); this->globalERP = cnode->GetDouble("erp",0.2,0); } Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.hh =================================================================== --- code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2008-07-01 08:51:20 UTC (rev 6722) +++ code/gazebo/trunk/server/physics/ode/ODEPhysics.hh 2008-07-01 18:50:43 UTC (rev 6723) @@ -48,23 +48,35 @@ The \c param:physics tag is used to specify certain parameters for the ODE phyics engine. The following parameters are in addition to those provided by the PhysicsEngine base class. -\verbatim -<param:physics> - <cfm>10e-9</cfm> - <erp>0.2</erp> -</param:physics> -\endverbatim +\par Attributes - cfm (float) - - Global Constraint Force Mixing parameter + - Global constraint force mixing + - Default: 10e-5 - Range: 10e-10 to 1.0 - - Recommended value: 10e-9 - + - Recommended value: 10e-5 - erp (float) - - Global Error Reduction parameter + - Global error reduction parameter + - Default: 0.2 - Range: 0 to 1.0 - Recommended Range: 0.1 to 0.8 +- stepTime (float) + - Time, in seconds, that elapse for each iteration of the physics engine + - Default: 0.025 +-gravity (float float float) + - Gravity vector. + - Default: 0 0 -9.8 +\verbatim +<physics:ode> + <stepTime>0.03</stepTime> + <gravity>0 0 -9.8</gravity> + <cfm>10e-5</cfm> + <erp>0.2</erp> +</physcis:ode> +\endverbatim + + \{ */ This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site. |