Revision: 7448
http://playerstage.svn.sourceforge.net/playerstage/?rev=7448&view=rev
Author: natepak
Date: 2009-03-10 17:41:45 +0000 (Tue, 10 Mar 2009)
Log Message:
-----------
Updated the graphics3d example. Added drawing of text
Modified Paths:
--------------
code/branches/federation/gazebo/SConstruct
code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc
code/branches/federation/gazebo/examples/libgazebo/position/position.cc
code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc
code/branches/federation/gazebo/libgazebo/gazebo.h
code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc
code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh
code/branches/federation/gazebo/server/rendering/OgreCreator.cc
Modified: code/branches/federation/gazebo/SConstruct
===================================================================
--- code/branches/federation/gazebo/SConstruct 2009-03-10 09:40:49 UTC (rev
7447)
+++ code/branches/federation/gazebo/SConstruct 2009-03-10 17:41:45 UTC (rev
7448)
@@ -144,8 +144,8 @@
#######
subdirs = ['libgazebo','server', 'player']
# Need test for <event.h>
-if True:
- subdirs.append('webgazebo')
+#if True:
+# subdirs.append('webgazebo')
#######
# Generate help text
Modified:
code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc
2009-03-10 09:40:49 UTC (rev 7447)
+++ code/branches/federation/gazebo/examples/libgazebo/graphics3d/graphics3d.cc
2009-03-10 17:41:45 UTC (rev 7448)
@@ -1,13 +1,184 @@
#include <string.h>
+#include <stdio.h>
#include <string>
#include <iostream>
+#include <vector>
+#include <math.h>
+
#include <gazebo/gazebo.h>
+// All the interfaces
+gazebo::Client *client;
+gazebo::SimulationIface *simIface;
+gazebo::PositionIface *posIface;
+gazebo::Graphics3dIface *g3dIface;
+gazebo::Graphics3dIface *pioneerG3DIface;
+
+// Stuff to draw the square
+std::string squareName = "square";
+gazebo::Vec3 squareSize;
+int dir = 1;
+
+// Stuff to draw the sphere
+std::string sphereName = "sphere";
+gazebo::Vec3 spherePos, sphereSize;
+float radius = 0.2;
+float theta = 0;
+
+// Stuff to draw the robot's path
+std::string pathName = "path";
+std::vector<gazebo::Vec3> positions;
+
+// Stuff to draw the text
+std::string textName = "velocities";
+
+void UpdateSquare()
+{
+ gazebo::Color clr;
+ gazebo::Vec3 vec[5];
+
+ vec[0].x = squareSize.x;
+ vec[0].y = squareSize.y;
+ vec[0].z = squareSize.z;
+
+ vec[1].x = -squareSize.x;
+ vec[1].y = squareSize.y;
+ vec[1].z = squareSize.z;
+
+ vec[2].x = -squareSize.x;
+ vec[2].y = -squareSize.y;
+ vec[2].z = squareSize.z;
+
+ vec[3].x = squareSize.x;
+ vec[3].y = -squareSize.y;
+ vec[3].z = squareSize.z;
+
+ vec[4].x = squareSize.x;
+ vec[4].y = squareSize.y;
+ vec[4].z = squareSize.z;
+
+ squareSize.z += dir*0.01;
+ if (squareSize.z >= 0.2 || squareSize.z < 0)
+ dir *= -1;
+
+ clr.r = 1.0;
+ clr.g = 0.0;
+ clr.b = 0.0;
+ clr.a = 1.0;
+
+ // Draw the bouncing square
+ pioneerG3DIface->DrawSimple(sphereName.c_str(),
+ gazebo::Graphics3dDrawData::LINE_STRIP, vec, 5, clr);
+
+}
+
+void UpdateSphere()
+{
+ gazebo::Color clr;
+
+ clr.r = 1.0;
+ clr.g = 0.0;
+ clr.b = 0.0;
+ clr.a = 1.0;
+
+ spherePos.x = radius * cos(theta);
+ spherePos.y = radius * sin(theta);
+ spherePos.z = 0.8;
+
+ pioneerG3DIface->DrawShape("mysphere",
+ gazebo::Graphics3dDrawData::SPHERE, spherePos, sphereSize, clr);
+
+ theta += 0.1;
+
+}
+
+void UpdatePath()
+{
+ gazebo::Vec3 rPos;
+ gazebo::Vec3 blockSize;
+ gazebo::Color clr;
+ std::ostringstream blockName;
+
+ blockSize.x = 0.05;
+ blockSize.y = 0.05;
+ blockSize.z = 0.05;
+
+ // Get the simulation pose of the robot
+ simIface->Lock(1);
+ simIface->data->requestCount = 0;
+ simIface->Unlock();
+ simIface->GetPose2d("pioneer2dx_model1");
+ while (simIface->data->requestCount == 0)
+ usleep(1000);
+
+ rPos.x = simIface->data->responses[0].modelPose.pos.x;
+ rPos.y = simIface->data->responses[0].modelPose.pos.y;
+ rPos.z = 0.15;
+
+ // Draw the robot's path
+ if (positions.size() == 0 ||
+ sqrt( pow(positions[positions.size()-1].x - rPos.x,2) +
+ pow(positions[positions.size()-1].y - rPos.y,2)) > 0.5)
+ {
+
+ // Store the new position
+ positions.push_back(rPos);
+
+ gazebo::Vec3 tmpvec[positions.size()];
+ for (unsigned int j =0; j < positions.size(); j++)
+ {
+ tmpvec[j].x = positions[j].x;
+ tmpvec[j].y = positions[j].y;
+ tmpvec[j].z = positions[j].z;
+ }
+
+ clr.r = 0.0;
+ clr.g = 1.0;
+ clr.b = 0.0;
+
+ // Draw the line
+ g3dIface->DrawSimple(pathName.c_str(),
+ gazebo::Graphics3dDrawData::LINE_STRIP,
+ tmpvec, positions.size(), clr);
+
+ blockName << "path_block:" << positions.size();
+
+ clr.r = 1.0;
+ clr.g = 1.0;
+ clr.b = 0.0;
+ clr.a = 1.0;
+
+ g3dIface->DrawShape(blockName.str().c_str(),
+ gazebo::Graphics3dDrawData::CUBE, tmpvec[positions.size()-1],
+ blockSize, clr);
+ }
+}
+
+void UpdateText()
+{
+ gazebo::Vec3 pos;
+ char vel[50];
+ float fontSize = 0.1;
+
+ pos.x = 0;
+ pos.y = 0;
+ pos.z = 0.2;
+
+ sprintf(vel,"Linear %4.2f Angular %4.2f",
+ posIface->data->velocity.pos.x,
+ posIface->data->velocity.yaw);
+
+ // Draw some text on the robot
+ pioneerG3DIface->DrawText(textName.c_str(), vel, pos, fontSize);
+}
+
int main()
{
- gazebo::Client *client = new gazebo::Client();
- gazebo::Graphics3dIface *g3dIface = new gazebo::Graphics3dIface();
- gazebo::Graphics3dIface *pioneerG3DIface = new gazebo::Graphics3dIface();
+ client = new gazebo::Client();
+ simIface = new gazebo::SimulationIface();
+ posIface = new gazebo::PositionIface();
+ g3dIface = new gazebo::Graphics3dIface();
+ pioneerG3DIface = new gazebo::Graphics3dIface();
int serverId = 0;
@@ -25,86 +196,54 @@
/// Open the global graphics Interface
try
{
+ simIface->Open(client, "default");
+
g3dIface->Open(client, "default");
+ posIface->Open(client, "position_iface_0");
pioneerG3DIface->Open(client,"pioneer2dx_model1");
}
catch (std::string e)
{
- std::cerr << "Gazebo error: Unable to connect to the graphics3d
interface\n" << e << "\n";
+ std::cerr << "Gazebo error: Unable to connect to an interface\n"
+ << e << "\n";
return -1;
}
- float radius = 0.2;
- float theta = 0;
+ // Set the size of the square
+ squareSize.x = 0.25;
+ squareSize.y = 0.25;
+ squareSize.z = 0.0;
- float size = 0.4;
- float h = 0;
- int dir = 1;
- std::string name = "square";
- int count = 0;
-
- gazebo::Vec3 spherePos, sphereSize;
+ // Set the size of the sphere
sphereSize.x = 0.1;
sphereSize.y = 0.1;
sphereSize.z = 0.1;
- gazebo::Vec3 vec[5];
- gazebo::Color clr;
+ // Draw a billboard
+ pioneerG3DIface->DrawBillboard("mybillboard", "heartpsg.png",
+ gazebo::Vec3(0.4,0.0,0.1), gazebo::Vec2(0.2, 0.2) );
+
+ // Update all the drawables
while (true)
{
- vec[0].x = size;
- vec[0].y = size;
- vec[0].z = h;
+ UpdateSquare();
+ UpdateSphere();
+ UpdatePath();
+ UpdateText();
- vec[1].x = -size;
- vec[1].y = size;
- vec[1].z = h;
-
- vec[2].x = -size;
- vec[2].y = -size;
- vec[2].z = h;
-
- vec[3].x = size;
- vec[3].y = -size;
- vec[3].z = h;
-
- vec[4].x = size;
- vec[4].y = size;
- vec[4].z = h;
-
- clr.r = 1.0;
- clr.g = 0.0;
- clr.b = 0.0;
- clr.a = 1.0;
-
- spherePos.x = radius * cos(theta);
- spherePos.y = radius * sin(theta);
- spherePos.z = 0.8;
-
- // Draw the bouncing square
- pioneerG3DIface->DrawSimple(name.c_str(),
- gazebo::Graphics3dDrawData::LINE_STRIP, vec, 5, clr);
-
- // Draw the sliding billboard
- pioneerG3DIface->DrawBillboard("mybillboard", "heartpsg.png",
- gazebo::Vec3(h,0,0.4), gazebo::Vec2(0.2, 0.2) );
-
-
- pioneerG3DIface->DrawShape("mysphere",
- gazebo::Graphics3dDrawData::SPHERE, spherePos, sphereSize, clr);
-
- h += dir*0.01;
- if (h >= 0.2 || h < 0)
- dir *= -1;
-
- theta += 0.1;
usleep(10000);
}
-
+ simIface->Close();
+ posIface->Close();
+ pioneerG3DIface->Close();
g3dIface->Close();
+ delete simIface;
+ delete posIface;
+ delete pioneerG3DIface;
delete g3dIface;
+ delete client;
return 0;
}
Modified:
code/branches/federation/gazebo/examples/libgazebo/position/position.cc
===================================================================
--- code/branches/federation/gazebo/examples/libgazebo/position/position.cc
2009-03-10 09:40:49 UTC (rev 7447)
+++ code/branches/federation/gazebo/examples/libgazebo/position/position.cc
2009-03-10 17:41:45 UTC (rev 7448)
@@ -48,6 +48,7 @@
posIface->data->cmdEnableMotors = 1;
posIface->Unlock();
+ posIface->data->cmdVelocity.yaw = -0.1;
while (true)
{
posIface->Lock(1);
Modified: code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc
===================================================================
--- code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc
2009-03-10 09:40:49 UTC (rev 7447)
+++ code/branches/federation/gazebo/libgazebo/Graphics3dIface.cc
2009-03-10 17:41:45 UTC (rev 7448)
@@ -128,3 +128,30 @@
this->Unlock();
}
+
+////////////////////////////////////////////////////////////////////////////////
+/// Draw text
+void Graphics3dIface::DrawText(const char *name, const char *text, Vec3 pos,
+ float fontSize)
+{
+ this->Lock(1);
+ Graphics3dDrawData *cmd = &(this->data->commands[this->data->cmdCount++]);
+
+ cmd->drawMode = Graphics3dDrawData::TEXT;
+
+ // Set the name of the graphics drawable
+ memset( cmd->name, 0, GAZEBO_GRAPHICS3D_MAX_NAME);
+ strcpy( cmd->name, name);
+
+ // Set the text to draw
+ memset( cmd->text, 0, GAZEBO_GRAPHICS3D_MAX_NAME);
+ strcpy( cmd->text, text);
+
+ cmd->pose.pos.x = pos.x;
+ cmd->pose.pos.y = pos.y;
+ cmd->pose.pos.z = pos.z;
+
+ cmd->size.x = fontSize;
+
+ this->Unlock();
+}
Modified: code/branches/federation/gazebo/libgazebo/gazebo.h
===================================================================
--- code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 09:40:49 UTC
(rev 7447)
+++ code/branches/federation/gazebo/libgazebo/gazebo.h 2009-03-10 17:41:45 UTC
(rev 7448)
@@ -807,13 +807,17 @@
/// Type of drawing to perform
public: enum DrawMode { POINTS, LINES, LINE_STRIP, TRIANGLES,
TRIANGLE_STRIP,
TRIANGLE_FAN, PLANE, SPHERE, CUBE, CYLINDER,
- BILLBOARD };
+ BILLBOARD, TEXT };
/// Drawing mode
public: DrawMode drawMode;
+ /// Unique name of the operation
public: char name[GAZEBO_GRAPHICS3D_MAX_NAME];
+ /// Text to display
+ public: char text[GAZEBO_GRAPHICS3D_MAX_NAME];
+
/// Number of vertices
public: unsigned int pointCount;
@@ -874,6 +878,11 @@
public: void DrawBillboard(const char *name, const char *texture,
Vec3 pos, Vec2 size);
+ /// \brief Draw text
+ public: void DrawText(const char *name, const char *text, Vec3 pos,
+ float fontSize);
+
+
/// Pointer to the graphics3d data
public: Graphics3dData *data;
};
Modified: code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc
===================================================================
--- code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc
2009-03-10 09:40:49 UTC (rev 7447)
+++ code/branches/federation/gazebo/server/GraphicsIfaceHandler.cc
2009-03-10 17:41:45 UTC (rev 7448)
@@ -34,6 +34,7 @@
#include "OgreCreator.hh"
#include "OgreVisualManager.hh"
#include "OgreDynamicLines.hh"
+#include "MovableText.hh"
#include "GraphicsIfaceHandler.hh"
#include "OgreAdaptor.hh"
@@ -129,6 +130,10 @@
this->DrawShape(vis, &this->threeDIface->data->commands[i] );
break;
+ case Graphics3dDrawData::TEXT:
+ this->DrawText(vis, &this->threeDIface->data->commands[i] );
+ break;
+
default:
gzerr(0) << "Unknown draw mode["
<< this->threeDIface->data->commands[i].drawMode << "\n";
@@ -194,7 +199,7 @@
{
pos.Set( data->points[i].x, data->points[i].y, data->points[i].z);
- if (attached)
+ if (attached && i < line->GetNumPoints())
line->SetPoint(i,pos);
else
line->AddPoint(pos);
@@ -215,8 +220,12 @@
case Graphics3dDrawData::CUBE:
{
if (vis->GetNumAttached() <= 0)
- vis->AttachMesh("unit_box");
+ vis->AttachMesh("unit_box_U1V1");
+ vis->SetMaterial( OgreCreator::CreateMaterial( data->color.r,
+ data->color.g,
+ data->color.b,
+ data->color.a ));
vis->SetScale(Vector3(data->size.x, data->size.y, data->size.z) );
vis->SetPosition(Vector3(data->pose.pos.x,
data->pose.pos.y,
@@ -229,6 +238,12 @@
if (vis->GetNumAttached() <= 0)
vis->AttachMesh("unit_cylinder");
+ vis->SetMaterial( OgreCreator::CreateMaterial( data->color.r,
+ data->color.g,
+ data->color.b,
+ data->color.a ));
+
+
vis->SetScale(Vector3(data->size.x, data->size.y, data->size.z) );
vis->SetPosition(Vector3(data->pose.pos.x,
data->pose.pos.y,
@@ -241,6 +256,11 @@
if (vis->GetNumAttached() <= 0)
vis->AttachMesh("unit_sphere");
+ vis->SetMaterial( OgreCreator::CreateMaterial( data->color.r,
+ data->color.g,
+ data->color.b,
+ data->color.a ));
+
vis->SetScale(Vector3(data->size.x, data->size.y, data->size.z) );
vis->SetPosition(Vector3(data->pose.pos.x,
data->pose.pos.y,
@@ -290,3 +310,40 @@
}
}
+////////////////////////////////////////////////////////////////////////////////
+/// Helper funciton used to draw text
+void GraphicsIfaceHandler::DrawText(OgreVisual *vis, Graphics3dDrawData *data)
+{
+ bool attached = false;
+ MovableText* msg = NULL;
+
+ if (vis->GetNumAttached() > 0)
+ {
+ attached = true;
+ msg = (MovableText*)(vis->GetAttached(0));
+ }
+ else
+ msg = new MovableText();
+
+ try
+ {
+ msg->Load(data->name, data->text, "Arial", data->size.x);
+ }
+ catch (Ogre::Exception e)
+ {
+ std::ostringstream stream;
+ stream << "Unable to create the text. " << e.getDescription() <<std::endl;
+ gzthrow(stream.str() );
+ }
+
+ msg->SetTextAlignment(MovableText::H_CENTER, MovableText::V_ABOVE);
+
+ vis->SetPosition(Vector3(data->pose.pos.x,
+ data->pose.pos.y,
+ data->pose.pos.z));
+
+ if (!attached)
+ vis->AttachObject( msg );
+
+}
+
Modified: code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh
===================================================================
--- code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh
2009-03-10 09:40:49 UTC (rev 7447)
+++ code/branches/federation/gazebo/server/GraphicsIfaceHandler.hh
2009-03-10 17:41:45 UTC (rev 7448)
@@ -61,6 +61,9 @@
/// \brief Helper funciton used to draw shapes
private: void DrawShape(OgreVisual *vis, Graphics3dDrawData *data);
+ /// \brief Helper funciton used to draw text
+ private: void DrawText(OgreVisual *vis, Graphics3dDrawData *data);
+
private: std::string name;
private: std::map<std::string, OgreVisual* > visuals;
private: Graphics3dIface *threeDIface;
Modified: code/branches/federation/gazebo/server/rendering/OgreCreator.cc
===================================================================
--- code/branches/federation/gazebo/server/rendering/OgreCreator.cc
2009-03-10 09:40:49 UTC (rev 7447)
+++ code/branches/federation/gazebo/server/rendering/OgreCreator.cc
2009-03-10 17:41:45 UTC (rev 7448)
@@ -628,7 +628,7 @@
matPtr->getTechnique(0)->setLightingEnabled(true);
matPtr->getTechnique(0)->getPass(0)->setDiffuse(r,g,b,a);
matPtr->getTechnique(0)->getPass(0)->setAmbient(r,g,b);
- matPtr->getTechnique(0)->getPass(0)->setSelfIllumination(r,g,b);
+// matPtr->getTechnique(0)->getPass(0)->setSelfIllumination(r,g,b);
// matPtr->setReceiveShadows(false);
}
@@ -643,8 +643,13 @@
{
Ogre::MaterialPtr matPtr = Ogre::MaterialManager::getSingleton().create(
filename, "General");
- matPtr->setLightingEnabled(true);
- matPtr->getTechnique(0)->getPass(0)->createTextureUnitState(filename);
+ Ogre::Pass *pass = matPtr->getTechnique(0)->createPass();
+
+ matPtr->setLightingEnabled(false);
+
+ //pass->setSceneBlending(Ogre::SBT_TRANSPARENT_ALPHA);
+ //pass->setDepthWriteEnabled(false);
+ pass->createTextureUnitState(filename);
}
return filename;
This was sent by the SourceForge.net collaborative development platform, the
world's largest Open Source development site.
------------------------------------------------------------------------------
_______________________________________________
Playerstage-commit mailing list
[email protected]
https://lists.sourceforge.net/lists/listinfo/playerstage-commit
