Revision: 7634
http://playerstage.svn.sourceforge.net/playerstage/?rev=7634&view=rev
Author: natepak
Date: 2009-05-11 01:43:31 +0000 (Mon, 11 May 2009)
Log Message:
-----------
Added Image and Color classes
Modified Paths:
--------------
code/gazebo/trunk/libgazebo/gazebo.h
code/gazebo/trunk/server/Entity.cc
code/gazebo/trunk/server/Global.hh
code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc
code/gazebo/trunk/server/gui/Gui.cc
code/gazebo/trunk/server/gui/StatusBar.cc
code/gazebo/trunk/server/gui/StatusBar.hh
code/gazebo/trunk/server/physics/Body.cc
code/gazebo/trunk/server/physics/Geom.cc
code/gazebo/trunk/server/physics/MapGeom.cc
code/gazebo/trunk/server/physics/MapGeom.hh
code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
code/gazebo/trunk/server/rendering/CMakeLists.txt
code/gazebo/trunk/server/rendering/OgreAdaptor.cc
code/gazebo/trunk/server/rendering/OgreHUD.cc
code/gazebo/trunk/worlds/map.world
Added Paths:
-----------
code/gazebo/trunk/server/rendering/Color.cc
code/gazebo/trunk/server/rendering/Color.hh
code/gazebo/trunk/server/rendering/Image.cc
code/gazebo/trunk/server/rendering/Image.hh
Modified: code/gazebo/trunk/libgazebo/gazebo.h
===================================================================
--- code/gazebo/trunk/libgazebo/gazebo.h 2009-05-10 23:47:42 UTC (rev
7633)
+++ code/gazebo/trunk/libgazebo/gazebo.h 2009-05-11 01:43:31 UTC (rev
7634)
@@ -647,7 +647,7 @@
/// Maximum image pixels (width x height)
-#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 640 * 480 * 3
+#define GAZEBO_CAMERA_MAX_IMAGE_SIZE 1024 * 1024 * 3
/// \brief Camera interface data
class CameraData
@@ -1503,7 +1503,7 @@
\{
*/
-#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 3
+#define GAZEBO_STEREO_CAMERA_MAX_RGB_SIZE 640 * 480 * 9
#define GAZEBO_STEREO_CAMERA_MAX_DISPARITY_SIZE 640 * 480
/// \brief Stereo data
Modified: code/gazebo/trunk/server/Entity.cc
===================================================================
--- code/gazebo/trunk/server/Entity.cc 2009-05-10 23:47:42 UTC (rev 7633)
+++ code/gazebo/trunk/server/Entity.cc 2009-05-11 01:43:31 UTC (rev 7634)
@@ -78,7 +78,6 @@
World::Instance()->GetPhysicsEngine()->RemoveEntity(this);
OgreCreator::Instance()->DeleteVisual(this->visualNode);
- //GZ_DELETE(this->visualNode);
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/Global.hh
===================================================================
--- code/gazebo/trunk/server/Global.hh 2009-05-10 23:47:42 UTC (rev 7633)
+++ code/gazebo/trunk/server/Global.hh 2009-05-11 01:43:31 UTC (rev 7634)
@@ -81,12 +81,6 @@
#define ISEVEN(x) ( ((x) % 2) == 0)
#define ISODD(x) ( ((x) % 2) != 0)
-// Min and Max macros
-#define MAX(x,y) ( (x) > (y) ? (x) : (y) )
-#define MIN(x,y) ( (x) < (y) ? (x) : (y) )
-
#define ROUND(x) ( (int)( floor((x)+0.5) ) )
-#define GZ_DELETE(p) { if(p) { delete (p); (p)=NULL; } }
-
#endif
Modified:
code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc
===================================================================
--- code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc
2009-05-10 23:47:42 UTC (rev 7633)
+++ code/gazebo/trunk/server/controllers/actarray/bandit/Bandit_Actarray.cc
2009-05-11 01:43:31 UTC (rev 7634)
@@ -57,9 +57,17 @@
{
for (int i=0; i<JOINTCNT; i++)
{
- GZ_DELETE(this->jointNamesP[i]);
- GZ_DELETE(this->forcesP[i]);
- GZ_DELETE(this->gainsP[i]);
+ if (this->jointNamesP[i])
+ delete this->jointNamesP[i];
+ this->jointNamesP[i] = NULL;
+
+ if(this->forcesP[i])
+ delete this->forcesP[i];
+ this->forcesP[i] = NULL;
+
+ if(this->gainsP[i])
+ delete this->gainsP[i];
+ this->gainsP[i] = NULL;
}
}
Modified: code/gazebo/trunk/server/gui/Gui.cc
===================================================================
--- code/gazebo/trunk/server/gui/Gui.cc 2009-05-10 23:47:42 UTC (rev 7633)
+++ code/gazebo/trunk/server/gui/Gui.cc 2009-05-11 01:43:31 UTC (rev 7634)
@@ -108,7 +108,6 @@
delete this->sizeP;
delete this->posP;
- //GZ_DELETE (this->toolbar)
//delete this->statusbar;
}
Modified: code/gazebo/trunk/server/gui/StatusBar.cc
===================================================================
--- code/gazebo/trunk/server/gui/StatusBar.cc 2009-05-10 23:47:42 UTC (rev
7633)
+++ code/gazebo/trunk/server/gui/StatusBar.cc 2009-05-11 01:43:31 UTC (rev
7634)
@@ -44,23 +44,33 @@
y += 5;
this->box(FL_UP_BOX);
- this->fps = new Fl_Value_Output(x,y,35,20,"FPS");
+ this->fps = new Fl_Value_Output(x,y,25,20,"FPS");
+ this->fps->labelsize(11);
+ this->fps->textsize(11);
this->fps->precision(0);
- x = this->fps->x() + this->fps->w() + 95;
- this->percent = new Fl_Value_Output(x,y,65,20,"% Real Time");
- this->percent->precision(2);
+ x = this->fps->x() + this->fps->w() + 75;
+ this->percentOutput = new Fl_Value_Output(x,y,40,20,"Sim speed");
+ this->percentOutput->labelsize(11);
+ this->percentOutput->textsize(11);
+ this->percentOutput->precision(2);
- x = this->percent->x() + this->percent->w() + 120;
- this->realTime = new Fl_Value_Output(x,y,55,20,"Real Time (sec)");
+ x = this->percentOutput->x() + this->percentOutput->w() + 100;
+ this->realTime = new Fl_Value_Output(x,y,45,20,"Real Time (sec)");
+ this->realTime->labelsize(11);
+ this->realTime->textsize(11);
this->realTime->precision(2);
- x = this->realTime->x() + this->realTime->w() + 120;
- this->simTime = new Fl_Value_Output(x,y,65,20,"Sim Time (sec)");
+ x = this->realTime->x() + this->realTime->w() + 100;
+ this->simTime = new Fl_Value_Output(x,y,45,20,"Sim Time (sec)");
+ this->simTime->labelsize(11);
+ this->simTime->textsize(11);
this->simTime->precision(2);
- x = this->simTime->x() + this->simTime->w() + 100;
- this->pauseTime = new Fl_Value_Output(x,y,55,20,"Pause Time");
+ x = this->simTime->x() + this->simTime->w() + 80;
+ this->pauseTime = new Fl_Value_Output(x,y,45,20,"Pause Time");
+ this->pauseTime->labelsize(11);
+ this->pauseTime->textsize(11);
this->pauseTime->precision(2);
x = this->pauseTime->x() + this->pauseTime->w() + 20;
@@ -90,11 +100,12 @@
float avgFPS = 0;
float percent = 0;
float sim = 0;
+ float real = 0;
if (Simulator::Instance()->GetRealTime() - this->lastUpdateTime > 0.05)
{
avgFPS = this->gui->GetAvgFPS();
- percent = 100. * (Simulator::Instance()->GetSimTime() /
Simulator::Instance()->GetRealTime());
+ percent = (Simulator::Instance()->GetSimTime() /
Simulator::Instance()->GetRealTime());
sim = Simulator::Instance()->GetSimTime();
if (sim > 99999)
@@ -113,15 +124,30 @@
this->simTime->label("Sim Time (min)");
}
+ real = Simulator::Instance()->GetRealTime();
+ if (sim > 99999)
+ {
+ real /= (120*24);
+ this->realTime->label("Real Time (dys)");
+ }
+ else if (real > 9999)
+ {
+ real /= 120;
+ this->realTime->label("Real Time (hrs)");
+ }
+ else if (real > 999)
+ {
+ real /= 60;
+ this->realTime->label("Real Time (min)");
+ }
+
//this->iterations->value(Simulator::Instance()->GetIterations());
this->fps->value(avgFPS);
- this->percent->value(percent);
+ this->percentOutput->value(percent);
- if (Simulator::Instance()->GetRealTime() - this->realTime->value() > 0.1)
- {
- this->realTime->value(Simulator::Instance()->GetRealTime());
- }
+ //if (Simulator::Instance()->GetRealTime() - this->realTime->value() > 0.1)
+ this->realTime->value(real);
this->simTime->value(sim);
this->pauseTime->value(Simulator::Instance()->GetPauseTime());
Modified: code/gazebo/trunk/server/gui/StatusBar.hh
===================================================================
--- code/gazebo/trunk/server/gui/StatusBar.hh 2009-05-10 23:47:42 UTC (rev
7633)
+++ code/gazebo/trunk/server/gui/StatusBar.hh 2009-05-11 01:43:31 UTC (rev
7634)
@@ -54,7 +54,7 @@
public: static void StepButtonCB( Fl_Widget *w, void *data );
private: Fl_Value_Output *iterations;
- private: Fl_Value_Output *percent;
+ private: Fl_Value_Output *percentOutput;
private: Fl_Value_Output *fps;
private: Fl_Value_Output *realTime;
private: Fl_Value_Output *pauseTime;
Modified: code/gazebo/trunk/server/physics/Body.cc
===================================================================
--- code/gazebo/trunk/server/physics/Body.cc 2009-05-10 23:47:42 UTC (rev
7633)
+++ code/gazebo/trunk/server/physics/Body.cc 2009-05-11 01:43:31 UTC (rev
7634)
@@ -90,13 +90,17 @@
for (giter = this->geoms.begin(); giter != this->geoms.end(); giter++)
{
- GZ_DELETE (giter->second);
+ if (giter->second)
+ delete giter->second;
+ giter->second = NULL;
}
this->geoms.clear();
for (siter = this->sensors.begin(); siter != this->sensors.end(); siter++)
{
- GZ_DELETE (*siter);
+ if (*siter)
+ delete (*siter);
+ (*siter) = NULL;
}
this->sensors.clear();
Modified: code/gazebo/trunk/server/physics/Geom.cc
===================================================================
--- code/gazebo/trunk/server/physics/Geom.cc 2009-05-10 23:47:42 UTC (rev
7633)
+++ code/gazebo/trunk/server/physics/Geom.cc 2009-05-11 01:43:31 UTC (rev
7634)
@@ -98,7 +98,9 @@
/*for (iter = this->visuals.begin(); iter != this->visuals.end(); iter++)
{
- GZ_DELETE (*iter)
+ if (*iter)
+ delete *iter;
+ *iter = NULL;
}
this->visuals.clear();
*/
Modified: code/gazebo/trunk/server/physics/MapGeom.cc
===================================================================
--- code/gazebo/trunk/server/physics/MapGeom.cc 2009-05-10 23:47:42 UTC (rev
7633)
+++ code/gazebo/trunk/server/physics/MapGeom.cc 2009-05-11 01:43:31 UTC (rev
7634)
@@ -24,18 +24,18 @@
* CVS: $Id$
*/
+
#include <ode/ode.h>
-#include <Ogre.h>
#include <iostream>
#include <string.h>
#include <math.h>
+#include "GazeboConfig.hh"
+#include "OgreVisual.hh"
+#include "Image.hh"
#include "BoxGeom.hh"
#include "GazeboError.hh"
-#include "OgreAdaptor.hh"
#include "Simulator.hh"
-#include "OgreAdaptor.hh"
-#include "OgreVisual.hh"
#include "Global.hh"
#include "Body.hh"
#include "MapGeom.hh"
@@ -70,6 +70,10 @@
if (this->root)
delete this->root;
+ if (this->mapImage)
+ delete this->mapImage;
+ this->mapImage = NULL;
+
delete this->negativeP;
delete this->thresholdP;
delete this->wallHeightP;
@@ -88,10 +92,7 @@
/// Load the heightmap
void MapGeom::LoadChild(XMLConfigNode *node)
{
- OgreAdaptor *ogreAdaptor;
- ogreAdaptor = Simulator::Instance()->GetRenderEngine();
-
std::string imageFilename = node->GetString("image","",1);
this->negativeP->Load(node);
@@ -107,14 +108,31 @@
if (this->wallHeightP->GetValue() <= 0) this->wallHeightP->SetValue( 1.0 );
// Load the image
- this->mapImage.load(imageFilename,
- Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
+ this->mapImage = new Image();
+ std::list<std::string>::iterator iter;
+ GazeboConfig *gzcfg = Simulator::Instance()->GetGazeboConfig();
+ std::string filename;
+
+ for (iter = gzcfg->GetGazeboPaths().begin();
+ iter != gzcfg->GetGazeboPaths().end(); iter++)
+ {
+ filename = (*iter) + "/Media/materials/textures/" + imageFilename;
+ if (this->mapImage->Load(filename) >= 0)
+ {
+ break;
+ }
+ }
+
+ if (!this->mapImage->Valid())
+ gzthrow(std::string("Unable to open image file[") + imageFilename + "]" );
+
this->root->x = 0;
this->root->y = 0;
- this->root->width = this->mapImage.getWidth();
- this->root->height = this->mapImage.getHeight();
+ this->root->width = this->mapImage->GetWidth();
+ this->root->height = this->mapImage->GetHeight();
+
this->BuildTree(this->root);
this->merged = true;
@@ -376,7 +394,7 @@
unsigned int &freePixels,
unsigned int &occPixels )
{
- Ogre::ColourValue pixColor;
+ Color pixColor;
unsigned char v;
unsigned int x,y;
@@ -386,8 +404,9 @@
{
for (x = xStart; x < xStart + width; x++)
{
- pixColor = this->mapImage.getColourAt(x, y, 0);
- v = (unsigned char)(255 * ((pixColor[0] + pixColor[1] + pixColor[2]) /
3.0));
+ pixColor = this->mapImage->GetPixel(x, y);
+
+ v = (unsigned char)(255 * ((pixColor.R() + pixColor.G() + pixColor.B())
/ 3.0));
if (this->negativeP->GetValue())
v = 255 - v;
Modified: code/gazebo/trunk/server/physics/MapGeom.hh
===================================================================
--- code/gazebo/trunk/server/physics/MapGeom.hh 2009-05-10 23:47:42 UTC (rev
7633)
+++ code/gazebo/trunk/server/physics/MapGeom.hh 2009-05-11 01:43:31 UTC (rev
7634)
@@ -27,7 +27,6 @@
#ifndef MAPGEOM_HH
#define MAPGEOM_HH
-#include <Ogre.h>
#include <deque>
#include "Vector2.hh"
@@ -36,6 +35,8 @@
namespace gazebo
{
+ class Image;
+
/// \addtogroup gazebo_physics_geom
/// \{
/** \defgroup gazebo_map_geom Map geom
@@ -141,7 +142,7 @@
private: ParamT<double> *wallHeightP;
- private: Ogre::Image mapImage;
+ private: Image *mapImage;
private: QuadNode *root;
Modified: code/gazebo/trunk/server/physics/ode/ODEPhysics.cc
===================================================================
--- code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-05-10 23:47:42 UTC
(rev 7633)
+++ code/gazebo/trunk/server/physics/ode/ODEPhysics.cc 2009-05-11 01:43:31 UTC
(rev 7634)
@@ -275,15 +275,15 @@
contact.surface.soft_erp = h * kp / (h * kp + kd);
contact.surface.soft_cfm = 1 / (h * kp + kd);
- contact.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1);
- contact.surface.mu2 = MIN(geom1->contact->mu2, geom2->contact->mu2);
- contact.surface.bounce = MIN(geom1->contact->bounce,
+ contact.surface.mu = std::min(geom1->contact->mu1,
geom2->contact->mu1);
+ contact.surface.mu2 = std::min(geom1->contact->mu2,
geom2->contact->mu2);
+ contact.surface.bounce = std::min(geom1->contact->bounce,
geom2->contact->bounce);
- contact.surface.bounce_vel = MIN(geom1->contact->bounceVel,
+ contact.surface.bounce_vel = std::min(geom1->contact->bounceVel,
geom2->contact->bounceVel);
- contact.surface.slip1 = MIN(geom1->contact->slip1,
+ contact.surface.slip1 = std::min(geom1->contact->slip1,
geom2->contact->slip1);
- contact.surface.slip2 = MIN(geom1->contact->slip2,
+ contact.surface.slip2 = std::min(geom1->contact->slip2,
geom2->contact->slip2);
dJointID c = dJointCreateContact (self->worldId,
@@ -387,7 +387,7 @@
// Compute friction effects; this is standard Coulomb friction
contactInfo.surface.mode |= dContactApprox1;
- contactInfo.surface.mu = MIN(geom1->contact->mu1, geom2->contact->mu1);
+ contactInfo.surface.mu = std::min(geom1->contact->mu1,
geom2->contact->mu1);
contactInfo.surface.mu2 = 0;
contactInfo.surface.bounce = 0.1;
contactInfo.surface.bounce_vel = 0.1;
Modified: code/gazebo/trunk/server/rendering/CMakeLists.txt
===================================================================
--- code/gazebo/trunk/server/rendering/CMakeLists.txt 2009-05-10 23:47:42 UTC
(rev 7633)
+++ code/gazebo/trunk/server/rendering/CMakeLists.txt 2009-05-11 01:43:31 UTC
(rev 7634)
@@ -12,6 +12,8 @@
OgreCamera.cc
CameraManager.cc
UserCamera.cc
+ Image.cc
+ Color.cc
)
APPEND_TO_SERVER_SOURCES(${sources})
Added: code/gazebo/trunk/server/rendering/Color.cc
===================================================================
--- code/gazebo/trunk/server/rendering/Color.cc (rev 0)
+++ code/gazebo/trunk/server/rendering/Color.cc 2009-05-11 01:43:31 UTC (rev
7634)
@@ -0,0 +1,426 @@
+/*
+ * 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: Color class
+ * Author: Nate Koenig
+ * Date: 08 May 2009
+ * CVS: $Id$
+ */
+
+#include <algorithm>
+#include <math.h>
+
+#include "GazeboMessage.hh"
+#include "Color.hh"
+
+using namespace gazebo;
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Color::Color()
+ : r(0), g(0), b(0), a(0)
+{
+ this->Clamp();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Color::Color( const float &r, const float &g, const float &b )
+ : r(r), g(g), b(b), a(0)
+{
+ this->Clamp();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Constructor
+Color::Color( const float &r, const float &g, const float &b, const float &a )
+ : r(r), g(g), b(b), a(a)
+{
+ this->Clamp();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Copy Constructor
+Color::Color( const Color &pt )
+ : r(pt.r), g(pt.g), b(pt.b), a(pt.a)
+{
+ this->Clamp();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Destructor
+Color::~Color()
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Reset the vector
+void Color::Reset()
+{
+ this->r = this->g = this->b = this->a = 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set the contents of the vector
+void Color::Set(float r, float g, float b, float a)
+{
+ this->r = r;
+ this->g = g;
+ this->b = b;
+ this->a = a;
+
+ this->Clamp();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set a color based on HSV values
+void Color::SetFromHSV(float h, float s, float v)
+{
+ int i;
+ float f, p , q, t;
+
+ h = (int)(h) % 360;
+
+ if (s==0)
+ {
+ //acromatic (grey)
+ this->r = this->g = this->b = v;
+ return;
+ }
+
+ h /= 60; // sector 0 - 5
+ i = (int)floor(h);
+
+ f = h - i;
+
+ p = v * (1-s);
+ q = v * (1 - s * f);
+ t = v * (1 - s * (1-f));
+
+ switch(i)
+ {
+ case 0:
+ this->r = v;
+ this->g = t;
+ this->b = p;
+ break;
+ case 1:
+ this->r = q;
+ this->g = v;
+ this->b = p;
+ break;
+ case 2:
+ this->r = p;
+ this->g = v;
+ this->b = t;
+ break;
+ case 3:
+ this->r = p;
+ this->g = q;
+ this->b = v;
+ break;
+ case 4:
+ this->r = t;
+ this->g = p;
+ this->b = v;
+ break;
+ case 5:
+ this->r = v;
+ this->g = p;
+ this->b = q;
+ break;
+ }
+
+ this->Clamp();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the color in HSV colorspace
+Vector3 Color::GetAsHSV() const
+{
+ Vector3 hsv;
+ float x,v, f, i;
+
+ x = std::min(this->r, std::min(this->g, this->b));
+ v = std::max(this->r, std::max(this->g, this->b));
+
+ if(v == x)
+ {
+ gzerr(0) << "rgb to hsv undefined\n";
+ return hsv;
+ }
+
+ if (r == x)
+ f = this->g - this->b;
+ else if (this->g == x)
+ f = this->b - this->r;
+ else
+ f = this->r - this->g;
+
+ if (this->r == x)
+ i = 3;
+ else if (this->g == x)
+ i = 5;
+ else
+ i = 1;
+
+ hsv.x = i - f /(v - x);
+ hsv.y = (v - x)/v;
+ hsv.z = v;
+
+ return hsv;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the color in YUV colorspace
+Vector3 Color::GetAsYUV() const
+{
+ Vector3 yuv;
+
+ yuv.x = 0.299*this->r + 0.587*this->g + 0.114*this->b;
+ yuv.y = -0.1679*this->r - 0.332*this->g + 0.5*this->b + 0.5;
+ yuv.z = 0.5*this->r - 0.4189*this->g - 0.08105*this->b + 0.5;
+
+ yuv.x = yuv.x < 0 ? 0: yuv.x;
+ yuv.x = yuv.x > 255 ? 255.0: yuv.x;
+
+ yuv.y = yuv.y < 0 ? 0: yuv.y;
+ yuv.y = yuv.y > 255 ? 255.0: yuv.y;
+
+ yuv.z = yuv.z < 0 ? 0: yuv.z;
+ yuv.z = yuv.z > 255 ? 255.0: yuv.z;
+
+
+
+ /*if (yuv.x > 255)
+ yuv.x = 255;
+ if (yuv.x < 0)
+ yuv.x = 0;
+
+ if (yuv.y > 255)
+ yuv.y = 255;
+ if (yuv.y < 0)
+ yuv.y = 0;
+
+ if (yuv.z > 255)
+ yuv.z = 255;
+ if (yuv.z < 0)
+ yuv.z = 0;
+ */
+
+ return yuv;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Set from yuv
+void Color::SetFromYUV(float y, float u, float v)
+{
+ this->r = y + 1.140*v;
+ this->g = y - 0.395*u - 0.581*v;
+ this->b = y + 2.032*u;
+ this->Clamp();
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Array index operator
+float Color::operator[](unsigned int index)
+{
+ switch(index)
+ {
+ case 0: return this->R();
+ case 1: return this->G();
+ case 2: return this->B();
+ case 3: return this->A();
+ default: gzerr(0) << "Invalid color index[" << index << "]\n";
+ }
+
+ return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Get the red color
+float Color::R()
+{
+ return this->r;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Get the green color
+float Color::G()
+{
+ return this->g;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Get the blue color
+float Color::B()
+{
+ return this->b;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Get the alpha color
+float Color::A()
+{
+ return this->a;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Equals operator
+const Color &Color::operator=( const Color &pt )
+{
+ this->r = pt.r;
+ this->g = pt.g;
+ this->b = pt.b;
+ this->a = pt.a;
+
+ return *this;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Addition operator
+Color Color::operator+( const Color &pt ) const
+{
+ return Color(this->r + pt.r, this->g + pt.g, this->b + pt.b, this->a + pt.a);
+}
+
+Color Color::operator+( const float &v ) const
+{
+ return Color(this->r + v, this->g + v, this->b + v, this->a + v);
+}
+
+const Color &Color::operator+=( const Color &pt )
+{
+ this->r += pt.r;
+ this->g += pt.g;
+ this->b += pt.b;
+ this->a += pt.a;
+
+ this->Clamp();
+
+ return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Subtraction operators
+Color Color::operator-( const Color &pt ) const
+{
+ return Color(this->r - pt.r, this->g - pt.g, this->b - pt.b, this->a - pt.a);
+}
+
+Color Color::operator-( const float &v ) const
+{
+ return Color(this->r - v, this->g - v, this->b - v, this->a - v);
+}
+
+const Color &Color::operator-=( const Color &pt )
+{
+ this->r -= pt.r;
+ this->g -= pt.g;
+ this->b -= pt.b;
+ this->a -= pt.a;
+
+ this->Clamp();
+
+ return *this;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Division operators
+const Color Color::operator/( const float &i ) const
+{
+ return Color(this->r / i, this->g / i, this->b / i, this->a / i);
+}
+
+const Color Color::operator/( const Color &pt ) const
+{
+ return Color(this->r / pt.r, this->g / pt.g, this->b / pt.b, this->a / pt.a);
+}
+
+const Color &Color::operator/=( const Color &pt )
+{
+ this->r /= pt.r;
+ this->g /= pt.g;
+ this->b /= pt.b;
+ this->a /= pt.a;
+
+ this->Clamp();
+
+ return *this;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Mulitplication operators
+const Color Color::operator*(const float &i) const
+{
+ return Color(this->r * i, this->g * i, this->b * i, this->a * i);
+}
+
+const Color Color::operator*( const Color &pt ) const
+{
+ return Color(this->r * pt.r, this->g * pt.g, this->b * pt.b, this->a * pt.a);
+}
+
+const Color &Color::operator*=( const Color &pt )
+{
+ this->r *= pt.r;
+ this->g *= pt.g;
+ this->b *= pt.b;
+ this->a *= pt.a;
+
+ this->Clamp();
+
+ return *this;
+}
+
+
+////////////////////////////////////////////////////////////////////////////////
+// Equality operator
+bool Color::operator==( const Color &pt ) const
+{
+ return this->r == pt.r && this->g == pt.g && this->b == pt.b && this->a ==
pt.a;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+// Inequality operator
+bool Color::operator!=( const Color &pt ) const
+{
+ return !(*this == pt);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Clamp the color values
+void Color::Clamp()
+{
+ this->r = this->r < 0 ? 0: this->r;
+ this->r = this->r > 1 ? this->r/255.0: this->r;
+
+ this->g = this->g < 0 ? 0: this->g;
+ this->g = this->g > 1 ? this->g/255.0: this->g;
+
+ this->b = this->b < 0 ? 0: this->b;
+ this->b = this->b > 1 ? this->b/255.0: this->b;
+}
+
+
Added: code/gazebo/trunk/server/rendering/Color.hh
===================================================================
--- code/gazebo/trunk/server/rendering/Color.hh (rev 0)
+++ code/gazebo/trunk/server/rendering/Color.hh 2009-05-11 01:43:31 UTC (rev
7634)
@@ -0,0 +1,124 @@
+/*
+ * 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: Color class
+ * Author: Nate Koenig
+ * Date: 08 May 2009
+ * CVS: $Id$
+ */
+
+#ifndef COLOR_HH
+#define COLOR_HH
+
+#include <iostream>
+//#include <math.h>
+//#include <fstream>
+#include "Vector3.hh"
+
+namespace gazebo
+{
+
+ class Color
+ {
+ // Constructors
+ public: Color();
+ public: Color( const float &r, const float &g, const float &b );
+ public: Color( const float &r, const float &g, const float &b,
+ const float &a );
+ public: Color( const Color &clr );
+
+ // Destructor
+ public: virtual ~Color();
+
+ public: void Reset();
+
+ /// \brief Set the contents of the vector
+ public: void Set(float r = 0, float g =0 , float b = 0, float a = 0);
+
+ /// \brief Get the color in HSV colorspace
+ public: Vector3 GetAsHSV() const;
+
+ /// \brief Set a color based on HSV values
+ public: void SetFromHSV(float h, float s, float v);
+
+ /// \brief Get the color in YUV colorspace
+ public: Vector3 GetAsYUV() const;
+
+ /// \brief Set from yuv
+ public: void SetFromYUV(float y, float u, float v);
+
+ // Equal operator
+ public: const Color &operator=( const Color &pt );
+
+ /// \brief Array index operator
+ public: float operator[](unsigned int index);
+
+ /// \brief Get the red color
+ public: float R();
+
+ /// \brief Get the green color
+ public: float G();
+
+ /// \brief Get the blue color
+ public: float B();
+
+ /// \brief Get the alpha color
+ public: float A();
+
+ // Addition operators
+ public: Color operator+( const Color &pt ) const;
+ public: Color operator+( const float &v ) const;
+ public: const Color &operator+=( const Color &pt );
+
+ // Subtraction operators
+ public: Color operator-( const Color &pt ) const;
+ public: Color operator-( const float &pt ) const;
+ public: const Color &operator-=( const Color &pt );
+
+ // Division operators
+ public: const Color operator/( const float &i ) const;
+ public: const Color operator/( const Color &pt ) const;
+ public: const Color &operator/=( const Color &pt );
+
+ // Multiplication operators
+ public: const Color operator*(const float &i) const;
+ public: const Color operator*( const Color &pt ) const;
+ public: const Color &operator*=( const Color &pt );
+
+ // Equality operators
+ public: bool operator==( const Color &pt ) const;
+ public: bool operator!=( const Color &pt ) const;
+
+ /// Clamp the color values
+ private: void Clamp();
+
+
+ // Ostream operator
+ public: friend std::ostream &operator<< (std::ostream &out, const Color
&pt) {
+ out << pt.r << " " << pt.g << " " << pt.b << " " << pt.a;
+ return out;
+ }
+
+ // The values
+ private: float r, g, b, a;
+ };
+}
+
+#endif
Added: code/gazebo/trunk/server/rendering/Image.cc
===================================================================
--- code/gazebo/trunk/server/rendering/Image.cc (rev 0)
+++ code/gazebo/trunk/server/rendering/Image.cc 2009-05-11 01:43:31 UTC (rev
7634)
@@ -0,0 +1,309 @@
+/*
+ * 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: Image class
+ * Author: Nate Koenig
+ * Date: 14 July 2008
+ * SVN: $Id$
+ */
+
+#include <iostream>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+//#include <GL/gl.h>
+
+#include "Vector3.hh"
+#include "GazeboMessage.hh"
+#include "Image.hh"
+
+using namespace gazebo;
+
+int Image::count = 0;
+
+////////////////////////////////////////////////////////////////////////////////
+/// Constructor
+Image::Image()
+{
+ if (count == 0)
+ FreeImage_Initialise();
+
+ count++;
+
+ this->bitmap = NULL;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Destructor
+Image::~Image()
+{
+ count--;
+
+ if (this->bitmap)
+ FreeImage_Unload(this->bitmap);
+ this->bitmap = NULL;
+
+ if (count == 0)
+ FreeImage_DeInitialise();
+
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Load
+int Image::Load(const std::string &filename)
+{
+ struct stat st;
+
+ if (stat(filename.c_str(), &st) != 0)
+ {
+ gzerr(5) << "Unable to open image file[" << filename << "]\n";
+ return -1;
+ }
+
+ FREE_IMAGE_FORMAT fifmt = FreeImage_GetFIFFromFilename(filename.c_str());
+
+ if (this->bitmap)
+ FreeImage_Unload(this->bitmap);
+ this->bitmap = NULL;
+
+ if (fifmt == FIF_PNG)
+ this->bitmap = FreeImage_Load(fifmt, filename.c_str(), PNG_DEFAULT);
+ else if (fifmt == FIF_JPEG)
+ this->bitmap = FreeImage_Load(fifmt, filename.c_str(), JPEG_DEFAULT);
+ else if (fifmt == FIF_BMP)
+ this->bitmap = FreeImage_Load(fifmt, filename.c_str(), BMP_DEFAULT);
+ else
+ {
+ std::cerr << "Unknown image format[" << filename << "]\n";
+ return -1;
+ }
+
+ return 0;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Set the image from raw data (R8G8B8)
+void Image::SetFromData( const unsigned char *data, unsigned int width,
+ unsigned int height, int scanline_bytes, unsigned int bpp)
+{
+ //int redmask = FI_RGBA_RED_MASK;
+ int redmask = 0xff0000;
+
+ //int greenmask = FI_RGBA_GREEN_MASK;
+ int greenmask = 0x00ff00;
+
+ //int bluemask = FI_RGBA_BLUE_MASK;
+ int bluemask = 0x0000ff;
+
+ if (this->bitmap)
+ {
+ FreeImage_Unload(this->bitmap);
+ }
+ this->bitmap = NULL;
+
+ this->bitmap = FreeImage_ConvertFromRawBits((BYTE*)data, width, height,
scanline_bytes, bpp, redmask, greenmask, bluemask);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the width
+unsigned int Image::GetWidth() const
+{
+ if (!this->Valid())
+ return 0;
+
+ return FreeImage_GetWidth( this->bitmap );
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the height
+unsigned int Image::GetHeight() const
+{
+ if (!this->Valid())
+ return 0;
+
+ return FreeImage_GetHeight( this->bitmap );
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the size of one pixel in bits
+unsigned int Image::GetBPP() const
+{
+ if (!this->Valid())
+ return 0;
+
+ return FreeImage_GetBPP( this->bitmap );
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get a pixel color value
+Color Image::GetPixel(unsigned int x, unsigned int y)
+{
+ Color clr;
+
+ if (!this->Valid())
+ return clr;
+
+ unsigned int colorsUsed = FreeImage_GetImageType(this->bitmap);
+
+ if (colorsUsed == 0)
+ {
+ RGBQUAD firgb;
+
+ if (FreeImage_GetPixelColor( this->bitmap, x, y, &firgb ) == FALSE)
+ {
+ std::cerr << "Image: Coordinates out of range["
+ << x << " " << y << "] \n";
+ return clr;
+ }
+
+ if (FREEIMAGE_COLORORDER == FREEIMAGE_COLORORDER_RGB)
+ {
+ clr.Set( firgb.rgbRed, firgb.rgbGreen, firgb.rgbBlue);
+ }
+ else
+ {
+ clr.Set( firgb.rgbBlue, firgb.rgbGreen, firgb.rgbRed);
+ }
+
+ }
+ else
+ {
+ BYTE byteValue;
+ if (FreeImage_GetPixelIndex(this->bitmap, x, y, &byteValue) == FALSE)
+ {
+ std::cerr << "Image: Coordinates out of range["
+ << x << " " << y << "] \n";
+ return clr;
+ }
+
+ clr.Set( byteValue, byteValue, byteValue);
+ }
+
+ return clr;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the average color
+Color Image::GetAvgColor()
+{
+ unsigned int x, y;
+ Color clrSum;
+
+ for (y = 0; y < this->GetHeight(); y++)
+ {
+ for (x = 0; x < this->GetWidth(); x++)
+ {
+ clrSum += this->GetPixel(x,y);
+ }
+ }
+
+ return clrSum / (this->GetWidth() * this->GetHeight());
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Get the max color
+Color Image::GetMaxColor()
+{
+ unsigned int x, y;
+ Color clr;
+ Color maxClr;
+
+ maxClr.Set(0,0,0,0);
+
+ for (y = 0; y < this->GetHeight(); y++)
+ {
+ for (x = 0; x < this->GetWidth(); x++)
+ {
+ clr = this->GetPixel(x,y);
+
+ if ( clr.R() + clr.G() + clr.B() > maxClr.R() + maxClr.G() + maxClr.B())
+ {
+ maxClr = clr;
+ }
+ }
+ }
+
+ return maxClr;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Rescale the image
+void Image::Rescale(int width, int height)
+{
+ this->bitmap = FreeImage_Rescale(this->bitmap, width, height,
FILTER_BICUBIC);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Render this image using opengl
+/*void Image::RenderOpengl(float destW, float destH)
+{
+ if (!this->Valid())
+ {
+ printf("Invalid\n");
+ return;
+ }
+
+ FIBITMAP *resizedBitmap = FreeImage_Rescale(this->bitmap, destW, destH,
+ FILTER_LANCZOS3);
+
+ Vector3d pt;
+ Color clr;
+
+ unsigned int x,y;
+ unsigned int w = this->GetWidth();
+ unsigned int h = this->GetHeight();
+
+ //float xRes = destW / w;
+ //float yRes = destH / h;
+
+ pt.z = 0;
+
+ glBegin(GL_TRIANGLE_STRIP);
+ for (y = h-1; y > 0; y--)
+ {
+ for (x=0; x < w; x++)
+ {
+ pt.x = x;// * xRes;
+ pt.y = (h-y);// * yRes;
+
+ clr = this->GetPixel(x,y);
+ glColor3ub( clr.r, clr.g, clr.b );
+ glVertex3f(pt.x, pt.y, pt.z);
+
+ pt.y = (h-y-1);// * yRes;
+ clr = this->GetPixel(x,y-1);
+ glColor3ub( clr.r, clr.g, clr.b );
+ glVertex3f(pt.x, pt.y, pt.z);
+ }
+ glVertex3f(pt.x, pt.y, pt.z);
+ glVertex3f(pt.x, pt.y, pt.z);
+ }
+ glEnd();
+
+ FreeImage_Unload(resizedBitmap);
+}*/
+
+////////////////////////////////////////////////////////////////////////////////
+/// Returns whether this is a valid image
+bool Image::Valid() const
+{
+ return this->bitmap != NULL;
+}
Added: code/gazebo/trunk/server/rendering/Image.hh
===================================================================
--- code/gazebo/trunk/server/rendering/Image.hh (rev 0)
+++ code/gazebo/trunk/server/rendering/Image.hh 2009-05-11 01:43:31 UTC (rev
7634)
@@ -0,0 +1,91 @@
+/*
+ * 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: Image class
+ * Author: Nate Koenig
+ * Date: 14 July 2008
+ * SVN: $Id$
+ */
+
+#ifndef IMAGE_HH
+#define IMAGE_HH
+
+#include <string>
+#include <FreeImage.h>
+
+#include "Color.hh"
+
+namespace gazebo
+{
+ class Image
+ {
+ /// \brief Constructor
+ public: Image();
+
+ /// \brief Destructor
+ public: virtual ~Image();
+
+ /// \brief Load an image. Return 0 on success
+ public: int Load(const std::string &filename);
+
+ /// \brief Set the image from raw data (R8G8B8)
+ /// \param data Pointer to the raw image data
+ /// \param width Width in pixels
+ /// \param height Height in pixels
+ /// \param scanline_bytes Size of a image row in bytes
+ /// \param bpp Bits per pixels, aka depth
+ public: void SetFromData( const unsigned char *data, unsigned int width,
+ unsigned int height, int scanline_bytes, unsigned int
bpp);
+
+ /// \brief Get the width
+ public: unsigned int GetWidth() const;
+
+ /// \brief Get the height
+ public: unsigned int GetHeight() const;
+
+ /// \brief Get the size of one pixel in bits
+ public: unsigned int GetBPP() const;
+
+ /// \brief Get a pixel color value
+ public: Color GetPixel(unsigned int x, unsigned int y);
+
+ /// \brief Get the average color
+ public: Color GetAvgColor();
+
+ /// \brief Get the max color
+ public: Color GetMaxColor();
+
+ /// \brief Rescale the image
+ public: void Rescale(int width, int height);
+
+ /// \brief Render this image using opengl
+ //public: void RenderOpengl(float destW, float destH);
+
+ /// \brief Returns whether this is a valid image
+ public: bool Valid() const;
+
+ /// Count the number of images created. Used for initialising free image
+ private: static int count;
+
+ private: FIBITMAP *bitmap;
+ };
+}
+
+#endif
Modified: code/gazebo/trunk/server/rendering/OgreAdaptor.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2009-05-10 23:47:42 UTC
(rev 7633)
+++ code/gazebo/trunk/server/rendering/OgreAdaptor.cc 2009-05-11 01:43:31 UTC
(rev 7634)
@@ -84,14 +84,6 @@
/// Destructor
OgreAdaptor::~OgreAdaptor()
{
- //GZ_DELETE (this->backgroundColor)
- //GZ_DELETE (this->frameListener)
- //GZ_DELETE (this->logManager)
-// this->root->shutdown();
- //GZ_DELETE (this->root)
-// GZ_DELETE (this->sceneMgr) //this objects seems to be destroyed by root
-// GZ_DELETE (this->viewport)
-
if (this->dummyDisplay)
{
glXDestroyContext(this->dummyDisplay, this->dummyContext);
@@ -113,10 +105,9 @@
// Closes and free
void OgreAdaptor::Close()
{
- GZ_DELETE (this->frameListener)
-
- // This causes a seg fault. Need to fix
- //GZ_DELETE (this->root)
+ if (this->frameListener)
+ delete this->frameListener;
+ this->frameListener = NULL;
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/server/rendering/OgreHUD.cc
===================================================================
--- code/gazebo/trunk/server/rendering/OgreHUD.cc 2009-05-10 23:47:42 UTC
(rev 7633)
+++ code/gazebo/trunk/server/rendering/OgreHUD.cc 2009-05-11 01:43:31 UTC
(rev 7634)
@@ -94,7 +94,9 @@
// Get a pointer to the text renderer
void OgreHUD::Close()
{
- GZ_DELETE(myself)
+ if (myself)
+ delete myself;
+ myself = NULL;
}
////////////////////////////////////////////////////////////////////////////////
Modified: code/gazebo/trunk/worlds/map.world
===================================================================
--- code/gazebo/trunk/worlds/map.world 2009-05-10 23:47:42 UTC (rev 7633)
+++ code/gazebo/trunk/worlds/map.world 2009-05-11 01:43:31 UTC (rev 7634)
@@ -19,7 +19,7 @@
<verbosity>5</verbosity>
<physics:ode>
- <stepTime>0.3</stepTime>
+ <stepTime>0.03</stepTime>
<gravity>0 0 -9.8</gravity>
<cfm>10e-5</cfm>
<erp>0.3</erp>
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
The NEW KODAK i700 Series Scanners deliver under ANY circumstances! Your
production scanning environment may not be a perfect world - but thanks to
Kodak, there's a perfect scanner to get the job done! With the NEW KODAK i700
Series Scanner you'll get full speed at 300 dpi even with all image
processing features enabled. http://p.sf.net/sfu/kodak-com
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
